#define RUN_ALL_POINTS
//#define HAVE_CLHEP
//#define DEBUG

#include "Math/SVector.h"
#include "Math/SMatrix.h"

#include "TMatrixD.h"
#include "TVectorD.h"

#include "TFile.h"
#include "TSystem.h"
#include "TROOT.h"

#include "TRandom3.h"
#include "TStopwatch.h"

#include <iostream>

#include <map>

//#undef HAVE_CLHEP

#ifdef HAVE_CLHEP
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Matrix/Matrix.h"
#include "CLHEP/Matrix/Vector.h"
#endif


#define NITER 1  // number of iterations

#define NLOOP 1000000 // number of time the test is repeted



template <unsigned int NDIM1, unsigned int NDIM2> 
class TestRunner { 

public: 

  // kalman test with normal  matrices from SMatrix
  int test_smatrix_kalman() ; 
  // kalman test with sym   matrices from SMatrix
  int test_smatrix_sym_kalman();  

  // kalman test with matrices (normal and sym) from TMatrix
  int test_tmatrix_kalman();  

#ifdef HAVE_CLHEP
  // test with CLHEP matrix 
  int test_clhep_kalman(); 
#endif

  // run the all tests
  int  run() { 
     int iret = 0;
     iret |= test_smatrix_sym_kalman(); 
     iret |= test_smatrix_kalman(); 
     iret |= test_tmatrix_kalman(); 
#ifdef HAVE_CLHEP
     iret |= test_clhep_kalman(); 
#endif
     std::cout << "\n\n";

     return iret; 
  }

private: 
   double fX2sum;
   double fC2sum;

}; 


#define TR(N1,N2) \
   { TestRunner<N1,N2> tr; if (tr.run()) return -1; }


int runTest() { 

#ifndef RUN_ALL_POINTS
  TR(2,5) 
    //TR(2,10) 
#else
  TR(2,2)
  TR(2,3)
  TR(2,4)
  TR(2,5)
  TR(2,6)
  TR(2,7)
  TR(2,8)
  TR(3,2)
  TR(3,3)
  TR(3,4)
  TR(3,5)
  TR(3,6)
  TR(3,7)
  TR(3,8)
  TR(4,2)
  TR(4,3)
  TR(4,4)
  TR(4,5)
  TR(4,6)
  TR(4,7)
  TR(4,8)
  TR(5,2)
  TR(5,3)
  TR(5,4)
  TR(5,5)
  TR(5,6)
  TR(5,7)
  TR(5,8)
  TR(6,2)
  TR(6,3)
  TR(6,4)
  TR(6,5)
  TR(6,6)
  TR(6,7)
  TR(6,8)
  TR(7,2)
  TR(7,3)
  TR(7,4)
  TR(7,5)
  TR(7,6)
  TR(7,7)
  TR(7,8)
  TR(8,2)
  TR(8,3)
  TR(8,4)
  TR(8,5)
  TR(8,6)
  TR(8,7)
  TR(8,8)
  TR(9,2)
  TR(9,3)
  TR(9,4)
  TR(9,5)
  TR(9,6)
  TR(9,7)
  TR(9,8)
  TR(10,2)
  TR(10,3)
  TR(10,4)
  TR(10,5)
  TR(10,6)
  TR(10,7)
  TR(10,8)
#endif
     return 0;   
}



// utility functions to fill matrices and vectors with random data

template<class V>
void fillRandomVec(TRandom & r, V  & v, unsigned int len, unsigned int start = 0, double offset = 1) {
  for(unsigned int i = start; i < len+start; ++i)
      v[i] = r.Rndm() + offset;
}

template<class M>
void fillRandomMat(TRandom & r, M  & m, unsigned int first, unsigned int second, unsigned int start = 0, double offset = 1) {
  for(unsigned int i = start; i < first+start; ++i)
    for(unsigned int j = start; j < second+start; ++j)
      m(i,j) = r.Rndm() + offset;
}

template<class M>
void fillRandomSym(TRandom & r, M  & m, unsigned int first, unsigned int start = 0, double offset = 1) {
  for(unsigned int i = start; i < first+start; ++i) { 
    for(unsigned int j = i; j < first+start; ++j) { 
      if ( i != j ) { 
	m(i,j) = r.Rndm() + offset;
	m(j,i) = m(i,j);
      }      
      else // add extra offset to make no singular when inverting
	m(i,i) = r.Rndm() + 3*offset;
    }
  }
}



// simple class to measure time 


void printTime(TStopwatch & time, std::string s) { 
  int pr = std::cout.precision(4);
  std::cout << std::setw(12) << s << "\t" << " Real time = " << time.RealTime() << "\t(sec)\tCPU time = " 
	    << time.CpuTime() << "\t(sec)"  
	    << std::endl;
  std::cout.precision(pr);
}

// reference times for sizes <=6 and > 6 on Linux slc3 P4 3Ghz ("SMatrix","SMatrix_sym","TMatrix")
double refTime1[4] = { 40.49, 53.75, 83.21,1000 }; 
double refTime2[4] = { 393.81, 462.16, 785.50,10000 }; 

#define NMAX1  9   // matrix storese results from 2 to 10
#define NMAX2  7   //  results from 2 to 8 
// class to hold time results 
class TimeReport { 

  //  typedef  std::map<std::string, ROOT::Math::SMatrix<double,NMAX1,NMAX2,ROOT::Math::MatRepSym<double,NMAX1,NMAX2> > > ResultTable; 
  typedef std::map<std::string, double> a;
  typedef   ROOT::Math::SMatrix<double,NMAX1,NMAX2> M; 
  typedef  std::map< std::string, M > ResultTable; 

 public: 

  TimeReport() {}

  ~TimeReport() { /*print(); */   }

  // set timing point
  void Set(const std::string & name, int dim1, int dim2 ) { 
    fDim1 = dim1; 
    fDim2 = dim2; 
    fName = name; 
    // insert in map if not existing 
    if (fResult1.find(name) == fResult1.end() ) 
      fResult1.insert(ResultTable::value_type(name, M() ) ); 
    if (fResult2.find(name) == fResult2.end() ) 
      fResult2.insert(ResultTable::value_type(name, M() )  ); 
    
  }

  std::string name() const { return fName; }

  void report(double rt, double ct) { 
    fResult1[fName](fDim1-2,fDim2-2) = rt; 
    fResult2[fName](fDim1-2,fDim2-2) = ct; 
  }

  double smallSum(const M & m, int cut = 6) { 
    // sum for sizes <= cut
    double sum = 0; 
    for (int i = 0; i<cut-1; ++i)  
      for (int j = 0; j<cut-1; ++j)  
	sum+= m(i,j);

    return sum; 
  }

  double largeSum(const M & m, int cut = 6) { 
    // sum for sizes > cut
    double sum = 0; 
    for (int i = 0; i<M::kRows; ++i)  
      for (int j = 0; j<M::kCols; ++j)  
	if ( i > cut-2 || j > cut-2)
	  sum+= m(i,j);

    return sum; 
  }

  void print(std::ostream & os) { 
    std::map<std::string,double> r1; 
    std::map<std::string,double> r2; 
    os << "Real time results " << std::endl; 
    for (ResultTable::iterator itr = fResult1.begin(); itr != fResult1.end(); ++itr) { 
      std::string type = itr->first; 
      os << " Results for " << type << std::endl; 
      os << "\n" << itr->second << std::endl << std::endl;
      r1[type] = smallSum(itr->second); 
      r2[type] = largeSum(itr->second); 
      os << "\nTotal for N1,N2 <= 6    :  " << r1[type] << std::endl; 
      os << "\nTotal for N1,N2 >  6    :  " << r2[type] << std::endl; 
    }     
    os << "\n\nCPU time results " << std::endl; 
    for (ResultTable::iterator itr = fResult2.begin(); itr != fResult2.end(); ++itr) { 
      os << " Results for " << itr->first << std::endl; 
      os << "\n" << itr->second << std::endl << std::endl;
      os << "\nTotal for N1,N2 <= 6    :  " << smallSum(itr->second) << std::endl; 
      os << "\nTotal for N1,N2 >  6    :  " << largeSum(itr->second) << std::endl; 
    } 

    // print summary
    os << "\n\n****************************************************************************\n";
    os << "Root version: " << gROOT->GetVersion() <<  "   "  
       << gROOT->GetVersionDate() << "/" << gROOT->GetVersionTime() << std::endl;
    os <<     "****************************************************************************\n";
    os << "\n\t ROOTMARKS for N1,N2 <= 6 \n\n"; 
    int j = 0;
    os.setf(std::ios::right,std::ios::adjustfield);
    for (std::map<std::string,double>::iterator i = r1.begin(); i != r1.end(); ++i) { 
      std::string type = i->first; 
      os << std::setw(12) << type << "\t=\t" << refTime1[j]*800/r1[type] << std::endl;   
      j++;
    }
    os << "\n\t ROOTMARKS for N1,N2 >  6 \n\n"; 
    j = 0; 
    for (std::map<std::string,double>::iterator i = r1.begin(); i != r1.end(); ++i) { 
      std::string type = i->first; 
      os << std::setw(12) << type << "\t=\t" << refTime2[j]*800/r2[type] << std::endl;   
      j++;
    }

  }

  void save(const std::string & fileName) { 
    TFile file(fileName.c_str(),"RECREATE"); 
    gSystem->Load("libSmatrix");

    // save RealTime results
    for (ResultTable::iterator itr = fResult1.begin(); itr != fResult1.end(); ++itr) { 
      int ret = file.WriteObject(&(itr->second),(itr->first).c_str() ); 
      if (ret ==0) std::cerr << "==> Error saving results in ROOT file " << fileName << std::endl;  
    }

    // save CPU time results
    for (ResultTable::iterator itr = fResult2.begin(); itr != fResult2.end(); ++itr) { 
       std::string typeName = itr->first + "_2";
       int ret = file.WriteObject(&(itr->second), typeName.c_str() ); 
       if (ret ==0) std::cerr << "==> Error saving results in ROOT file " << fileName << std::endl;  
    }
    file.Close();
  }

 private: 

  int fDim1; 
  int fDim2; 
  std::string fName; 
  
  ResultTable fResult1;  
  ResultTable fResult2;  

}; 

//global instance of time report
TimeReport gReporter; 
 
class TestTimer {
  
public: 
  
  // TestTimer(const std::string & s = "") : 
//     fName(s), fTime(0), fRep(0) 
//   {
//     fWatch.Start();
//   }
  TestTimer(TimeReport & r ) : 
    fTime(0), fRep(&r) 
  {
    fName = fRep->name(); 
    fWatch.Start();
  }
  TestTimer(double & t, const std::string & s = "") : fName(s), fTime(&t), fRep(0) 
  {
    fWatch.Start();
  } 
  
  ~TestTimer() { 
    fWatch.Stop();
    printTime(fWatch,fName);
    if (fRep) fRep->report( fWatch.RealTime(), fWatch.CpuTime() ); 
    if (fTime) *fTime += fWatch.RealTime();
  }

  
private: 
  
  std::string fName;
  double * fTime; 
  TStopwatch fWatch; 
  TimeReport * fRep; 
  
}; 



template <unsigned int NDIM1, unsigned int NDIM2>
int TestRunner<NDIM1,NDIM2>::test_smatrix_kalman() {

  gReporter.Set("SMatrix",NDIM1,NDIM2);
    
  // need to write explicitly the dimensions
   

  typedef ROOT::Math::SMatrix<double, NDIM1, NDIM1>  MnMatrixNN;
  typedef ROOT::Math::SMatrix<double, NDIM2, NDIM2>  MnMatrixMM;
  typedef ROOT::Math::SMatrix<double, NDIM1, NDIM2>  MnMatrixNM;
  typedef ROOT::Math::SMatrix<double, NDIM2 , NDIM1> MnMatrixMN;
  typedef ROOT::Math::SMatrix<double, NDIM1 >        MnSymMatrixNN;
  typedef ROOT::Math::SMatrix<double, NDIM2 >        MnSymMatrixMM;
  typedef ROOT::Math::SVector<double, NDIM1>         MnVectorN;
  typedef ROOT::Math::SVector<double, NDIM2>         MnVectorM;
  


  int first = NDIM1;  //Can change the size of the matrices
  int second = NDIM2;
  

  std::cout << "****************************************************************************\n";
  std::cout << "\t\tSMatrix kalman test  "   <<  first << " x " << second  << std::endl;
  std::cout << "****************************************************************************\n";

  
  
   
  int npass = NITER; 
  TRandom3 r(111);
  double x2sum = 0, c2sum = 0;
  for (int k = 0; k < npass; k++) {



    MnMatrixNM H;
    MnMatrixMN K0;
    MnSymMatrixMM Cp; 
    MnSymMatrixNN V; 
    MnVectorN m;
    MnVectorM xp;


    { 
      // fill matrices with random data
      fillRandomMat(r,H,first,second); 
      fillRandomMat(r,K0,second,first); 
      fillRandomSym(r,Cp,second); 
      fillRandomSym(r,V,first); 
      fillRandomVec(r,m,first); 
      fillRandomVec(r,xp,second); 
    }


    MnSymMatrixMM I; 
    for(int i = 0; i < second; i++) 
      I(i,i) = 1;
     
#ifdef DEBUG
    std::cout << "pass " << k << std::endl;
    if (k == 0) { 
      std::cout << " K0 = " << K0 << std::endl;
      std::cout << " H = " << K0 << std::endl;
      std::cout << " Cp = " << Cp << std::endl;
      std::cout << " V = " << V << std::endl;
      std::cout << " m = " << m << std::endl;
      std::cout << " xp = " << xp << std::endl;
    }
#endif
	
    
    {
      double x2 = 0,c2 = 0;
      TestTimer t(gReporter);

      MnVectorM x; 
      MnMatrixMN tmp;   
      MnSymMatrixNN Rinv; 
      MnMatrixMN K; 
      MnSymMatrixMM C; 
      MnVectorN vtmp1; 
      MnVectorN vtmp;

      for (int l = 0; l < NLOOP; l++) 	
	{



	  vtmp1 = H*xp -m;
	  //x = xp + K0 * (m- H * xp);
	  x = xp - K0 * vtmp1;
	  tmp = Cp * Transpose(H);
	  Rinv = V;  Rinv +=  H * tmp;

	  bool test = Rinv.Invert();
 	  if(!test) { 
 	    std::cout<<"inversion failed" <<std::endl;
	    std::cout << Rinv << std::endl;
	  }

	  K =  tmp * Rinv ; 
	  C = Cp; C -= K * Transpose(tmp);
	  //C = ( I - K * H ) * Cp;
	  //x2 = Product(Rinv,m-H*xp);  // this does not compile on WIN32
 	  vtmp = m-H*xp; 
 	  x2 = Dot(vtmp, Rinv*vtmp);

#ifdef DEBUG 
	  if (l == 0) { 
	    std::cout << " Rinv =\n " << Rinv << std::endl;
	    std::cout << " C =\n " << C << std::endl;
	  }
#endif

	}
	//std::cout << k << " chi2 = " << x2 << std::endl;
      x2sum += x2;
      c2 = 0;
      for (unsigned int i=0; i<NDIM2; ++i)
	for (unsigned int j=0; j<NDIM2; ++j)
	  c2 += C(i,j);
      c2sum += c2;
    }
  }
  //tr.dump();

  int iret = 0;                                 
  double d = std::abs(x2sum-fX2sum);
  if ( d > 1.E-6 * fX2sum  ) {     
     std::cout << "ERROR: difference found in x2sum = " << x2sum << "\tref = " << fX2sum << 
     "\tdiff = " << d <<  std::endl;
     iret = 1; 
  }
  d = std::abs(c2sum-fC2sum); 
  if ( d > 1.E-6 * fC2sum  ) {     
     std::cout << "ERROR: difference found in c2sum = " << c2sum << "\tref = " << fC2sum << 
     "\tdiff = " << d <<  std::endl;
     iret = 1; 
  }

  return iret;
}

template <unsigned int NDIM1, unsigned int NDIM2>
int TestRunner<NDIM1,NDIM2>::test_smatrix_sym_kalman() {

  gReporter.Set("SMatrix_sym",NDIM1,NDIM2);
    
  // need to write explicitly the dimensions
   

  typedef ROOT::Math::SMatrix<double, NDIM1, NDIM1>  MnMatrixNN;
  typedef ROOT::Math::SMatrix<double, NDIM2, NDIM2>  MnMatrixMM;
  typedef ROOT::Math::SMatrix<double, NDIM1, NDIM2>  MnMatrixNM;
  typedef ROOT::Math::SMatrix<double, NDIM2 , NDIM1> MnMatrixMN;
  typedef ROOT::Math::SMatrix<double, NDIM1, NDIM1, ROOT::Math::MatRepSym<double, NDIM1> >   MnSymMatrixNN;
  typedef ROOT::Math::SMatrix<double, NDIM2, NDIM2, ROOT::Math::MatRepSym<double, NDIM2> >   MnSymMatrixMM;
  typedef ROOT::Math::SVector<double, NDIM1>         MnVectorN;
  typedef ROOT::Math::SVector<double, NDIM2>         MnVectorM;
  typedef ROOT::Math::SVector<double, NDIM1*(NDIM1+1)/2>   MnVectorN2;
  typedef ROOT::Math::SVector<double, NDIM2*(NDIM2+1)/2>   MnVectorM2;
  


  int first = NDIM1;  //Can change the size of the matrices
  int second = NDIM2;
  

  std::cout << "****************************************************************************\n";
  std::cout << "\t\tSMatrix_Sym kalman test  "   <<  first << " x " << second  << std::endl;
  std::cout << "****************************************************************************\n";

  
  
   
  int npass = NITER; 
  TRandom3 r(111);
  double x2sum = 0, c2sum = 0;
  for (int k = 0; k < npass; k++) {



    MnMatrixNM H;
    MnMatrixMN K0;
    MnSymMatrixMM Cp; 
    MnSymMatrixNN V; 
    MnVectorN m;
    MnVectorM xp;


    { 
      // fill matrices with random data
      fillRandomMat(r,H,first,second); 
      fillRandomMat(r,K0,second,first); 
      fillRandomSym(r,Cp,second); 
      fillRandomSym(r,V,first); 
      fillRandomVec(r,m,first); 
      fillRandomVec(r,xp,second); 
    }


    MnSymMatrixMM I; 
    for(int i = 0; i < second; i++) 
      I(i,i) = 1;
     
#ifdef DEBUG
    std::cout << "pass " << k << std::endl;
    if (k == 0) { 
      std::cout << " K0 = " << K0 << std::endl;
      std::cout << " H = " << K0 << std::endl;
      std::cout << " Cp = " << Cp << std::endl;
      std::cout << " V = " << V << std::endl;
      std::cout << " m = " << m << std::endl;
      std::cout << " xp = " << xp << std::endl;
    }

#endif
	
    {
      double x2 = 0,c2 = 0;
      TestTimer t(gReporter);

      MnVectorM x; 
      MnSymMatrixNN RinvSym; 
      MnMatrixMN K; 
      MnSymMatrixMM C; 
      MnSymMatrixMM Ctmp; 
      MnVectorN vtmp1; 
      MnVectorN vtmp; 
#define OPTIMIZED_SMATRIX_SYM
#ifdef OPTIMIZED_SMATRIX_SYM
      MnMatrixMN tmp;   
#endif

      for (int l = 0; l < NLOOP; l++) 	
	{


#ifdef OPTIMIZED_SMATRIX_SYM
	  vtmp1 = H*xp -m;
	  //x = xp + K0 * (m- H * xp);
	  x = xp - K0 * vtmp1;
	  tmp = Cp * Transpose(H);
	  // we are sure that H*tmp result is symmetric 
	  ROOT::Math::AssignSym::Evaluate(RinvSym,H*tmp); 
	  RinvSym += V; 

	  bool test = RinvSym.Invert();
 	  if(!test) { 
 	    std::cout<<"inversion failed" <<std::endl;
	    std::cout << RinvSym << std::endl;
	  }

	  K =  tmp * RinvSym ; 
	  // we profit from the fact that result of K*tmpT is symmetric
	  ROOT::Math::AssignSym::Evaluate(Ctmp, K*Transpose(tmp) ); 
	  C = Cp; C -= Ctmp;
	  //C = ( I - K * H ) * Cp;
	  //x2 = Product(Rinv,m-H*xp);  // this does not compile on WIN32
 	  vtmp = m-H*xp; 
 	  x2 = ROOT::Math::Dot(vtmp, RinvSym*vtmp);
#else 
	  // use similarity function
	  vtmp1 = H*xp -m;
	  x = xp - K0 * vtmp1;
 	  RinvSym = V;  RinvSym +=  Similarity(H,Cp);

	  bool test = RinvSym.Invert();
 	  if(!test) { 
 	    std::cout<<"inversion failed" <<std::endl;
	    std::cout << RinvSym << std::endl;
	  }
	  
	  Ctmp = ROOT::Math::SimilarityT(H, RinvSym); 
	  C = Cp; C -= ROOT::Math::Similarity(Cp, Ctmp);
 	  vtmp = m-H*xp; 
 	  x2 = ROOT::Math::Similarity(vtmp, RinvSym);
#endif

	}
	//std::cout << k << " chi2 = " << x2 << std::endl;
      x2sum += x2;
      c2 = 0;
      for (unsigned int i=0; i<NDIM2; ++i)
	for (unsigned int j=0; j<NDIM2; ++j)
	  c2 += C(i,j);
      c2sum += c2;
    }
  }

  // smatrix_sym is always first (skip check test) 
  fX2sum = x2sum; 
  fC2sum = c2sum;
  if (x2sum == 0 || c2sum == 0) {     
     std::cout << "WARNING: x2sum = " << x2sum << "\tc2sum = " << c2sum << std::endl;
  }

  return 0;
}



// ROOT test 


template <unsigned int NDIM1, unsigned int NDIM2>
int TestRunner<NDIM1,NDIM2>::test_tmatrix_kalman() {

  gReporter.Set("TMatrix",NDIM1,NDIM2);
    

  typedef TMatrixD MnMatrix;
  typedef TVectorD MnVector;
  
//   typedef boost::numeric::ublas::matrix<double>  MnMatrix;  
  //typedef HepSymMatrix MnSymMatrixHep; 


  int first = NDIM1;  //Can change the size of the matrices
  int second = NDIM2;


  std::cout << "****************************************************************************\n";
  std::cout << "\t\tTMatrix Kalman test  "   <<  first << " x " << second  << std::endl;
  std::cout << "****************************************************************************\n";
  
  int npass = NITER; 
  TRandom3 r(111);
  double x2sum = 0,c2sum = 0;

  for (int k = 0; k < npass; k++) 
  {

      MnMatrix H(first,second);
      MnMatrix K0(second,first);
      MnMatrix Cp(second,second);
      MnMatrix V(first,first);
      MnVector m(first);
      MnVector xp(second);

      // fill matrices with random data
      fillRandomMat(r,H,first,second); 
      fillRandomMat(r,K0,second,first); 
      fillRandomSym(r,Cp,second); 
      fillRandomSym(r,V,first); 
      fillRandomVec(r,m,first); 
      fillRandomVec(r,xp,second); 


      MnMatrix I(second,second);//Identity matrix
      for (int i = 0; i < second; i++)
	for(int j = 0; j <second; j++) { 
	  I(i,j) = 0.0; 
	  if (i==j) I(i,i) = 1.0;
	}

//       if (k==0) { 
// 	std::cout << " Cp " << std::endl;
// 	Cp.Print();
//       }

    {
      double x2 = 0,c2 = 0;
      TVectorD x(second);
      TMatrixD Rtmp(first,first);
      TMatrixD Rinv(first,first);
      TMatrixDSym RinvSym;
      TMatrixD K(second,first);
      TMatrixD C(second,second);
      TMatrixD Ctmp(second,second);
      TVectorD tmp1(first);
      TMatrixD tmp2(second,first);
      
      TestTimer t(gReporter);
      for (Int_t l = 0; l < NLOOP; l++)
      {
        tmp1 = m; Add(tmp1,-1.0,H,xp);
        x = xp; Add(x,+1.0,K0,tmp1);
        tmp2.MultT(Cp,H);
        Rtmp.Mult(H,tmp2);
        Rinv.Plus(V,Rtmp);
        RinvSym.Use(first,Rinv.GetMatrixArray());
        RinvSym.InvertFast();
        K.Mult(tmp2,Rinv);
        Ctmp.MultT(K,tmp2);
        C.Minus(Cp,Ctmp);
        x2 = RinvSym.Similarity(tmp1);

#ifdef DEBUG 
	if (l == 0) { 
	  std::cout << " Rinv =\n "; Rinv.Print();
	  std::cout << " RinvSym =\n "; RinvSym.Print();
	  std::cout << " C =\n "; C.Print();
	}
#endif

      }
      x2sum += x2; 
      c2 = 0;
      for (unsigned int i=0; i<NDIM2; ++i)
	for (unsigned int j=0; j<NDIM2; ++j)
	  c2 += C(i,j);
      c2sum += c2;
    }

      //   }
  }  
  //tr.dump();
  //std::cout << "x2sum = " << x2sum << "\tc2sum = " << c2sum << std::endl;
  
  //gReporter.print();

  int iret = 0;                                 
  double d = std::abs(x2sum-fX2sum);
  if ( d > 1.E-6 * fX2sum  ) {     
     std::cout << "ERROR: difference found in x2sum = " << x2sum << "\tref = " << fX2sum << 
     "\tdiff = " << d <<  std::endl;
     iret = 1; 
  }
  d = std::abs(c2sum-fC2sum); 
  if ( d > 1.E-6 * fC2sum  ) {     
     std::cout << "ERROR: difference found in c2sum = " << c2sum << "\tref = " << fC2sum << 
     "\tdiff = " << d <<  std::endl;
     iret = 1; 
  }


  return iret;
}


// test CLHEP Kalman

#ifdef HAVE_CLHEP
template <unsigned int NDIM1, unsigned int NDIM2>
int TestRunner<NDIM1,NDIM2>::test_clhep_kalman() {


  gReporter.Set("HepMatrix",NDIM1,NDIM2);
    
  typedef HepSymMatrix MnSymMatrix;   
  typedef HepMatrix MnMatrix;   
  typedef HepVector MnVector;

  
//   typedef boost::numeric::ublas::matrix<double>  MnMatrix;  
  //typedef HepSymMatrix MnSymMatrixHep; 


  int first = NDIM1;  //Can change the size of the matrices
  int second = NDIM2;


  std::cout << "****************************************************************************\n";
  std::cout << "  CLHEP Kalman test  "   <<  first << " x " << second  << std::endl;
  std::cout << "****************************************************************************\n";
   
  int npass = NITER; 
  TRandom3 r(111);
  double x2sum = 0,c2sum = 0;

  for (int k = 0; k < npass; k++) 
  {

    // in CLHEP index starts from 1 
      MnMatrix H(first,second);
      MnMatrix K0(second,first);
      MnMatrix Cp(second,second);
      MnMatrix V(first,first);
      MnVector m(first);
      MnVector xp(second);

      // fill matrices with random data
      fillRandomMat(r,H,first,second,1); 
      fillRandomMat(r,K0,second,first,1); 
      fillRandomSym(r,Cp,second,1); 
      fillRandomSym(r,V,first,1); 
      fillRandomVec(r,m,first); 
      fillRandomVec(r,xp,second); 

      MnSymMatrix I(second,1);//Identity matrix

    {
      double x2 = 0,c2 = 0;
      MnVector x(second);
      MnMatrix Rinv(first,first);
      MnSymMatrix RinvSym(first);
      MnMatrix K(second,first);
      MnSymMatrix C(second);
      MnVector vtmp1(first);
      MnMatrix tmp(second,first);
      
      TestTimer t(gReporter);
      int ifail; 
      for (Int_t l = 0; l < NLOOP; l++)
      {
    

	vtmp1 = H*xp -m;
	//x = xp + K0 * (m- H * xp);
	x = xp - K0 * vtmp1;
	tmp = Cp * H.T();
	Rinv = V;  Rinv +=  H * tmp;
	RinvSym.assign(Rinv); 
	RinvSym.invert(ifail);
	if (ifail !=0) { std::cout << "Error inverting Rinv" << std::endl; break; } 
	K = tmp*RinvSym; 
	//C.assign( (I-K*H)*Cp);	
	//C = (I-K*H)*Cp;	
	C.assign( (I-K*H)*Cp );	
	x2= RinvSym.similarity(vtmp1);
       	if(ifail!=0) { std::cout << "Error inverting Rinv" << std::endl; break; } 
      }
      x2sum += x2;
     
      c2 = 0;
      for (unsigned int i=1; i<=NDIM2; ++i)
	for (unsigned int j=1; j<=NDIM2; ++j)
	  c2 += C(i,j);
      c2sum += c2;
    }

      //   }
  }  
  //tr.dump();
  //std::cout << "x2sum = " << x2sum << "\tc2sum = " << c2sum << std::endl;
  
  int iret = 0;                                 
  double d = std::abs(x2sum-fX2sum);
  if ( d > 1.E-6 * fX2sum  ) {     
     std::cout << "ERROR: difference found in x2sum = " << x2sum << "\tref = " << fX2sum << 
     "\tdiff = " << d <<  std::endl;
     iret = 1; 
  }
  d = std::abs(c2sum-fC2sum); 
  if ( d > 1.E-6 * fC2sum  ) {     
     std::cout << "ERROR: difference found in c2sum = " << c2sum << "\tref = " << fC2sum << 
     "\tdiff = " << d <<  std::endl;
     iret = 1; 
  }

  return iret;
}
#endif


int main(int argc, char *argv[]) { 

  
   if (runTest() ) {
      std::cout << "\nERROR - stressKalman FAILED - exit!" << std::endl ;
      return -1;
   }; 

  gReporter.print(std::cout); 
  std::string fname = "kalman"; 
  if (argc > 1) { 
    std::string platf(argv[1]); 
    fname = fname + "_" + platf; 
  }
   
  gReporter.save(fname+".root");

  return 0;
}