// smatrix.c

#ifndef smatrix_C
#define smatrix_C

#include "smatrix.h"
#include "nvector.h"
#include "trfutil/trfstream.h"
#include <CLHEP/Matrix/SymMatrix.h>

// output stream
template <class T>
void smatrix<T>::ostr(std::ostream& stream) const {
  stream << begin_object;
  int i = 0;
  for ( int irow=0; irow<_nrow; irow++ ) {
    if ( irow ) stream << new_line;
    for ( int icol=0; icol<=irow; icol++ ) {
      if ( icol > 0 ) stream << ' ';
      stream << data[i++];
    }
  }
  stream << end_object;
}

// Normalize a matrix.
// Transformation which makes all diagonal elements unity.
// Zero off-diagonal elements if diagonal is zero.
template <class T>
void normalize(smatrix<T>& sma) {
  nvector<T> vec(sma.nrow());
  for ( int irow=0; irow<sma.nrow(); irow++ ) {
    double fac = sma(irow,irow);
    if ( fac == 0.0 ) {
      vec(irow) = 0.0;
    } else {
      vec(irow) = 1.0/sqrt(fac);
    }
    for ( int icol=0; icol<=irow; icol++ ) {
      sma(irow,icol) *= vec(irow)*vec(icol);
    }
  }
}

//
// multiplication: t = S % v == vT * S * v
//
template <class T>
T operator%(const smatrix<T>& sma, const nvector<T>& vec)
{
 int dim = vec.length();
 if ( sma.nrow() != dim ) {
  T prod = 0;
  return prod;
 }
 T prod = 0.0;
 for ( int i=0; i<dim; ++i ) {
  prod += vec(i)*sma(i,i)*vec(i);
  for ( int j=0; j<i; ++j )
   prod += 2.0*vec(i)*sma(i,j)*vec(j);
 }
 return prod;
}

/*
int invert( smatrix<double>& sma ) {

 // copy matrix to HEP object
 int dim = sma.nrow();
 HepSymMatrix hep(dim);
 int i, j;
 for ( i=0; i<dim; ++i )
  for ( j=0; j<=i; ++j )
   hep.fast(i+1,j+1) = sma(i,j);

 // invert
 int err;
 hep.invert( err );

 // copy back to smatrix
 if ( ! err )
  for ( i=0; i<dim; ++i )
   for ( j=0; j<=i; ++j )
    sma(i,j) = hep.fast(i+1,j+1);

 // return error status.
 return err;

}
*/
 
#endif