#ifndef MATRIX_OP_H
#define MATRIX_OP_H

#include "TestTimer.h"

// define functions for matrix operations

//#define DEBUG
//#ifndef NLOOP
//#define NLOOP 1000000
//#endif
#include <vector>

using namespace ROOT::Math;

std::vector<float> gV;

void initValues() {
   gV.reserve(10*NLOOP);
   TRandom3 r;
   std::cout << "init smearing vector ";
   for (int l = 0; l < 10*NLOOP; l++)
   {
      gV.push_back( r.Rndm() );
   }
   std::cout << " with size  " << gV.size() << std::endl;

}


// vector assignment
template<class V>
void testVeq(const V & v, double & time, V & result) {
   V vtmp = v;
   test::Timer t(time,"V=V ");
   for (int l = 0; l < 10*NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result = vtmp;
   }
}

// matrix assignmnent
template<class M>
void testMeq(const M & m, double & time, M & result) {
   M mtmp = m;
   test::Timer t(time,"M=M ");
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(0,0) = gV[l];
      result = mtmp;
   }
}



// vector sum
template<class V>
void testVad(const V & v1, const V & v2, double & time, V & result) {
   V vtmp = v2;
   test::Timer t(time,"V+V ");
   for (int l = 0; l < 10*NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result = v1 + vtmp;
   }
}

// matrix sum
template<class M>
void testMad(const M & m1, const M & m2, double & time, M & result) {
   M mtmp = m2;
   test::Timer t(time,"M+M ");;
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(0,0) = gV[l];
      result = m1; result += mtmp;
      //M tmp = m1 + mtmp;
      //result = tmp;
   }
}

// vector * constant
template<class V>
void testVscale(const V & v1, double a, double & time, V & result) {
   V vtmp = v1;
   test::Timer t(time,"a*V ");;
   for (int l = 0; l < NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result = a * vtmp;   // v1 * a does not exist in ROOT
   }
}


// matrix * constant
template<class M>
void testMscale(const M & m1, double a, double & time, M & result) {
   M mtmp = m1;
   test::Timer t(time,"a*M ");;
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(0,0) = gV[l];
      //result = mtmp * a;
      result = mtmp; result *= a;
   }
}


// simple Matrix vector op
template<class M, class V>
void testMV(const M & mat, const V & v, double & time, V & result) {
   V vtmp = v;
   test::Timer t(time,"M*V ");
   for (int l = 0; l < NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result = mat * vtmp;
   }
}

// general matrix vector op
template<class M, class V>
void testGMV(const M & mat, const V & v1, const V & v2, double & time, V & result) {
   V vtmp = v1;
   test::Timer t(time,"M*V+");
   for (int l = 0; l < NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result = mat * vtmp + v2;
   }
}


// general matrix matrix op
template<class A, class B, class C>
void testMM(const A & a, const B & b, const C & c, double & time, C & result) {
   B btmp = b;
   test::Timer t(time,"M*M ");
   for (int l = 0; l < NLOOP; l++)
   {
      btmp(0,0) = gV[l];
      result = a * btmp + c;
   }
}




// specialized functions (depending on the package)

//smatrix
template<class V>
double testDot_S(const V & v1, const V & v2, double & time) {
   V vtmp = v2;
   test::Timer t(time,"dot ");
   double result=0;
   for (int l = 0; l < 10*NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result = Dot(v1,vtmp);
   }
   return result;
}


// double testDot_S(const std::vector<V*> & w1, const std::vector<V*> & w2, double & time) {
//   test::Timer t(time,"dot ");
//   double result=0;
//   for (int l = 0; l < NLOOP; l++)
//     {
//       V & v1 = *w1[l];
//       V & v2 = *w2[l];
//       result = Dot(v1,v2);
//     }
//   return result;
// }

template<class M, class V>
double testInnerProd_S(const M & a, const V & v, double & time) {
   V vtmp = v;
   test::Timer t(time,"prod");
   double result=0;
   for (int l = 0; l < NLOOP; l++)
   {
      vtmp[0] =  gV[l];
      result = Similarity(vtmp,a);
   }
   return result;
}

//inversion
template<class M>
void  testInv_S( const M & m,  double & time, M& result){
   M mtmp = m;
   test::Timer t(time,"inv ");
   int ifail = 0;
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(0,0) = gV[l];
      //result = mtmp.InverseFast(ifail);
      result = mtmp.Inverse(ifail);
      // assert(ifail == 0);
   }
}


// general matrix matrix op
template<class A, class B, class C>
void testATBA_S(const A & a, const B & b, double & time, C & result) {
   B btmp = b;
   test::Timer t(time,"At*M*A");
   for (int l = 0; l < NLOOP; l++)
   {
      //result = Transpose(a) * b * a;
      //result = a * b * Transpose(a);
      //result = a * b * a;
      btmp(0,0) = gV[l];
      //      A at = Transpose(a);
      C tmp = btmp * Transpose(a);
      result = a * tmp;
   }
}

// general matrix matrix op
template<class A, class B, class C>
void testATBA_S2(const A & a, const B & b, double & time, C & result) {
   B btmp = b;

   test::Timer t(time,"At*M*A");
   for (int l = 0; l < NLOOP; l++)
   {
      //result = Transpose(a) * b * a;
      //result = a * b * Transpose(a);
      //result = a * b * a;
      btmp(0,0) = gV[l];
      result  = SimilarityT(a,b);
      //result = a * result;
   }
}

template<class A, class C>
void testMT_S(const A & a, double & time, C & result) {
   A atmp = a;
   test::Timer t(time,"Transp");
   for (int l = 0; l < NLOOP; l++)
   {
      //result = Transpose(a) * b * a;
      //result = a * b * Transpose(a);
      //result = a * b * a;
      atmp(0,0) = gV[l];
      result  = Transpose(atmp);
   }
}

/////////////////////////////////////
// for root
//////////////////////////////////

// simple Matrix vector op
template<class M, class V>
void testMV_T(const M & mat, const V & v, double & time, V & result) {
   V vtmp = v;
   test::Timer t(time,"M*V ");
   for (int l = 0; l < NLOOP; l++)
   {
      vtmp[0] = gV[l];
      Add(result,0.0,mat,vtmp);
   }
}

// general matrix vector op
template<class M, class V>
void testGMV_T(const M & mat, const V & v1, const V & v2, double & time, V & result) {
   V vtmp = v1;
   test::Timer t(time,"M*V+");
   for (int l = 0; l < NLOOP; l++)
   {
      vtmp[0] = gV[l];
      memcpy(result.GetMatrixArray(),v2.GetMatrixArray(),v2.GetNoElements()*sizeof(Double_t));
      Add(result,1.0,mat,vtmp);
   }
}

// general matrix matrix op
template<class A, class B, class C>
void testMM_T(const A & a, const B & b, const C & c, double & time, C & result) {
   B btmp = b;
   test::Timer t(time,"M*M ");
   for (int l = 0; l < NLOOP; l++)
   {
      btmp(0,0) = gV[l];
      result.Mult(a,btmp);
      result += c;
   }
}

// matrix sum
template<class M>
void testMad_T(const M & m1, const M & m2, double & time, M & result) {
   M mtmp = m2;
   test::Timer t(time,"M+M ");
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(0,0) = gV[l];
      result.Plus(m1,mtmp);
   }
}

template<class A, class B, class C>
void testATBA_T(const A & a, const B & b, double & time, C & result) {
   B btmp = b;
   test::Timer t(time,"At*M*A");
   C tmp = a;
   for (int l = 0; l < NLOOP; l++)
   {
      btmp(0,0) = gV[l];
      tmp.Mult(a,btmp);
      result.MultT(tmp,a);
   }
}

template<class V>
double testDot_T(const V & v1, const V & v2, double & time) {
   V vtmp = v2;
   test::Timer t(time,"dot ");
   double result=0;
   for (int l = 0; l < 10*NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result = Dot(v1,vtmp);
   }
   return result;
}

template<class M, class V>
double testInnerProd_T(const M & a, const V & v, double & time) {
   V vtmp = v;
   test::Timer t(time,"prod");
   double result=0;
   for (int l = 0; l < NLOOP; l++) {
      vtmp[0] =  gV[l];
      result = a.Similarity(vtmp);
   }
   return result;
}

//inversion
template<class M>
void  testInv_T(const M & m,  double & time, M& result){
   M mtmp = m;
   test::Timer t(time,"inv ");
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(0,0) = gV[l];
      memcpy(result.GetMatrixArray(),mtmp.GetMatrixArray(),mtmp.GetNoElements()*sizeof(Double_t));
      result.InvertFast();
   }
}

template<class M>
void  testInv_T2(const M & m,  double & time, M& result){
   M mtmp = m;
   test::Timer t(time,"inv2");
   for (int l = 0; l < NLOOP; l++)
   {
      memcpy(result.GetMatrixArray(),mtmp.GetMatrixArray(),mtmp.GetNoElements()*sizeof(Double_t));
      result.InvertFast();
   }
}


// vector sum
template<class V>
void testVad_T(const V & v1, const V & v2, double & time, V & result) {
   V vtmp = v2;
   test::Timer t(time,"V+V ");;
   for (int l = 0; l < 10*NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result.Add(v1,vtmp);
   }
}

// vector * constant
template<class V>
void testVscale_T(const V & v1, double a, double & time, V & result) {
   V vtmp = v1;
   test::Timer t(time,"a*V ");;
   for (int l = 0; l < NLOOP; l++)
   {
      // result = a * v1;
      result.Zero();
      vtmp[0] = gV[l];
      Add(result,a,vtmp);
   }
}

// general matrix matrix op
template<class A, class B, class C>
void testATBA_T2(const A & a, const B & b, double & time, C & result) {
   B btmp = b;
   test::Timer t(time,"At*M*A");
   for (int l = 0; l < NLOOP; l++)
   {
      btmp(0,0) = gV[l];
      memcpy(result.GetMatrixArray(),btmp.GetMatrixArray(),btmp.GetNoElements()*sizeof(Double_t));
      result.Similarity(a);
   }
}

// matrix * constant
template<class M>
void testMscale_T(const M & m1, double a, double & time, M & result) {
   M mtmp = m1;
   test::Timer t(time,"a*M ");;
   for (int l = 0; l < NLOOP; l++)
   {
      //result = a * m1;
      result.Zero();
      mtmp(0,0) = gV[l];
      Add(result,a,mtmp);
   }
}

template<class A, class C>
void testMT_T(const A & a, double & time, C & result) {
   A atmp = a;
   test::Timer t(time,"Transp");
   for (int l = 0; l < NLOOP; l++)
   {
      atmp(0,0) = gV[l];
      result.Transpose(atmp);
   }
}

////////////////////////////////////////////
// for clhep
////////////////////////////////////////////

//smatrix
template<class V>
double testDot_C(const V & v1, const V & v2, double & time) {
   V vtmp =  v2;
   test::Timer t(time,"dot ");
   double result=0;
   for (int l = 0; l < 10*NLOOP; l++)
   {
      vtmp[0] = gV[l];
      result = dot(v1,vtmp);
   }
   return result;
}

template<class M, class V>
double testInnerProd_C(const M & a, const V & v, double & time) {
   V vtmp = v;
   test::Timer t(time,"prod");
   double result=0;
   for (int l = 0; l < NLOOP; l++)
   {
      vtmp[0] = gV[l];
      V tmp = a*vtmp;
      result = dot(vtmp,tmp);
   }
   return result;
}


// matrix assignmnent(index starts from 1)
template<class M>
void testMeq_C(const M & m, double & time, M & result) {
   M mtmp = m;
   test::Timer t(time,"M=M ");
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(1,1) = gV[l];
      result = mtmp;
   }
}

// matrix sum
template<class M>
void testMad_C(const M & m1, const M & m2, double & time, M & result) {
   M mtmp = m2;
   test::Timer t(time,"M+M ");;
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(1,1) = gV[l];
      result = m1; result += mtmp;
   }
}


// matrix * constant
template<class M>
void testMscale_C(const M & m1, double a, double & time, M & result) {
   M mtmp = m1;
   test::Timer t(time,"a*M ");;
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(1,1) = gV[l];
      result = mtmp * a;
   }
}


// clhep matrix matrix op (index starts from 1)
template<class A, class B, class C>
void testMM_C(const A & a, const B & b, const C & c, double & time, C & result) {
   B btmp = b;
   test::Timer t(time,"M*M ");
   for (int l = 0; l < NLOOP; l++)
   {
      btmp(1,1) = gV[l];
      result = a * btmp + c;
   }
}


//inversion
template<class M>
void  testInv_C( const M & a,  double & time, M& result){
   M mtmp = a;
   test::Timer t(time,"inv ");
   int ifail = 0;
   for (int l = 0; l < NLOOP; l++)
   {
      mtmp(1,1) = gV[l];
      result = mtmp.inverse(ifail);
      if (ifail) {std::cout <<"error inverting" << mtmp << std::endl; return; }
   }
}

// general matrix matrix op
template<class A, class B, class C>
void testATBA_C(const A & a, const B & b, double & time, C & result) {
   B btmp = b;
   test::Timer t(time,"At*M*A");
   for (int l = 0; l < NLOOP; l++)
   {
      btmp(1,1) = gV[l];
      //result = a.T() * b * a;
      result = a * btmp * a.T();
   }
}


template<class A, class B, class C>
void testATBA_C2(const A & a, const B & b, double & time, C & result) {
   B btmp = b;
   test::Timer t(time,"At*M*A");
   for (int l = 0; l < NLOOP; l++)
   {
      btmp(1,1) = gV[l];
      result = btmp.similarity(a);
   }
}


template<class A, class C>
void testMT_C(const A & a, double & time, C & result) {
   A atmp = a;
   test::Timer t(time,"Transp");
   for (int l = 0; l < NLOOP; l++)
   {
      atmp(1,1) = gV[l];
      result  = atmp.T();
   }
}


#endif   
