#include <type_traits>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>

#include "mkl.h"


#include "densemat.h"
#include "Constants.h"

using namespace std;

#define ZERO 1.0e-30

// extern "C"{
// 	void dgeev_( char* jobvl, char* jobvr, int* n, double* a,
//                 int* lda, double* wr, double* wi, double* vl, int* ldvl,
//                double* vr, int* ldvr, double* work, int* lwork, int* info,
//                int jobvl_size, int jobvr_size);
// }

// extern "C"{
//   void dgesv_( int* n, int* nrhs, double* a, int* lda, int* ipiv,
//                 double* b, int* ldb, int* info );
// }

// extern "C"{
// void sgeev_( char* jobvl, char* jobvr, int* n, float* a,
//              int* lda, float* wr, float* wi, float* vl, int* ldvl,
//              float* vr, int* ldvr, float* work, int* lwork, int* info,
//              int jobvl_size, int jobvr_size);
// }

// extern "C"{
//   void sgesv_( int* n, int* nrhs, float* a, int* lda, int* ipiv,
//                 float* b, int* ldb, int* info );
// }

void solveAx_B(denseMat<fp_t>& A, ArrayFP<fp_t>& B){
  int n = A.N;
  int lda = n, ldb = n, info;
  int nrhs = 1; //x is a column vector
  int ipiv[n];
  
  #ifdef DGTD_USE_DOUBLE
    dgesv_(&n, &nrhs, A.entry, &lda, ipiv, B.getEntryPtr(), &ldb, &info ); //B is rewritten
  #else
    sgesv_(&n, &nrhs, A.entry, &lda, ipiv, B.getEntryPtr(), &ldb, &info ); //B is rewritten
  #endif
}

void MXV(const denseMat<fp_t> a, fp_t *x, fp_t *y)
{
  int i, j;
  fp_t cval;
  
  // first reset y
  for (i = 0; i < a.getRowDim(); i ++) y[i] = 0.0;

  // perform matrix vector multiplication
  for (i = 0; i < a.getRowDim(); i ++) {
    for (j = 0; j < a.getColDim(); j ++) {
      cval = (a.getEntry(i, j)) * (x[j]);

      y[i] = y[i] + cval;
    }
  }
}

void MXV(denseMat<fp_t>* a, fp_t *x, fp_t *y)
{
  int i, j;
  fp_t cval;

  // first reset y
  for (i = 0; i < a->getRowDim(); i ++) y[i] = 0.0;

  // perform matrix vector multiplication
  for (i = 0; i < a->getRowDim(); i ++) {
    for (j = 0; j < a->getColDim(); j ++) {
      cval = (a->getEntry(i, j)) * (x[j]);

      y[i] = y[i] + cval;
    }
  }
}

void AXPY(denseMat<fp_t>* a, fp_t *x, fp_t *y)
{
  int i, j;
  fp_t cval;

  // first reset y
//   for (i = 0; i < a->getRowDim(); i ++) y[i] = 0.0;

  // perform matrix vector multiplication
  for (i = 0; i < a->getRowDim(); i ++) {
    for (j = 0; j < a->getColDim(); j ++) {
      cval = (a->getEntry(i, j)) * (x[j]);

      y[i] = y[i] + cval;
    }
  }
}

template <typename T>
denseMat<T>::denseMat(int rDim, int cDim)
{
  int NZ;

  N = rDim;
  M = cDim;
  NZ = N * M;
  if (NZ == 0) entry = 0;
  else entry = new T[NZ];
  for (int i = 0; i < NZ; i++) entry[i] = 0.;
}

template <typename T>
denseMat<T>::denseMat(const denseMat& denMat) {
  N = denMat.N;
  M = denMat.M;
  entry = new T[N*M];
  memcpy(entry, denMat.entry, N*M*sizeof(T));
}

template <typename T>
denseMat<T>::~denseMat(){

  int NZ;
  NZ = N * M;
  if (entry != 0)
  delete [] entry;

}

template <typename T>
void denseMat<T>::Clear(){

  int NZ;
  NZ = N * M;
  if (entry != 0)
  delete [] entry;

}

template <typename T>
void denseMat<T>::setDim(int rowDim, int colDim)
{
  int NZ;

  N = rowDim;
  M = colDim;
  NZ = N * M;
  if (NZ == 0) entry = 0;
  else entry = new T[NZ];
  for (int i = 0; i < NZ; i++) entry[i] = 0.;

}

template <typename T>
denseMat<T>& denseMat<T>::operator = (const denseMat &right)
{
  return *this;
}
template <>
denseMat<float>& denseMat<float>::operator = (const denseMat<float> &right)
{
  cblas_scopy(M*N, right.entry, 1, entry, 1);
  return *this;
}
template <>
denseMat<double>& denseMat<double>::operator = (const denseMat<double> &right)
{
  cblas_dcopy(M*N, right.entry, 1, entry, 1);
  return *this;
}

template <typename T>
denseMat<T> &denseMat<T>::operator*=(const T &val)
{
  return *this;
}
template <>
denseMat<float> &denseMat<float>::operator*=(const float &val)
{
  cblas_sscal(M*N, val, entry, 1);
  return *this;
}
template <>
denseMat<double> &denseMat<double>::operator*=(const double &val)
{
  cblas_dscal(M*N, val, entry, 1);
  return *this;
}

template <typename T>
denseMat<T> &denseMat<T>::operator+=(const denseMat &operand2)
{
  return *this;
}

template <>
denseMat<float> &denseMat<float>::operator+=(const denseMat<float> &operand2)
{
  if (N != operand2.N ||
      M != operand2.M) {
    cout << "Adding two different sized dense Matrix " << endl;
    exit(1);
  }
  cblas_saxpy(M*N, 1.0, operand2.entry, 1, entry, 1);
  return *this;
}
template <>
denseMat<double> &denseMat<double>::operator+=(const denseMat<double> &operand2)
{
  if (N != operand2.N ||
      M != operand2.M) {
    cout << "Adding two different sized dense Matrix " << endl;
    exit(1);
  }
  cblas_daxpy(M*N, 1.0, operand2.entry, 1, entry, 1);
  return *this;
}

template <typename T>
denseMat<T> &denseMat<T>::operator-=(const denseMat &operand2)
{
  return *this;
}

template <>
denseMat<float> &denseMat<float>::operator-=(const denseMat<float> &operand2)
{
  if (N != operand2.N ||
      M != operand2.M) {
    cout << "Adding two different sized dense Matrix " << endl;
    exit(1);
  }
  cblas_saxpy(M*N, -1.0 , operand2.entry, 1, entry, 1);
  return *this;
}
template <>
denseMat<double> &denseMat<double>::operator-=(const denseMat<double> &operand2)
{
  if (N != operand2.N ||
      M != operand2.M) {
    cout << "Adding two different sized dense Matrix " << endl;
    exit(1);
  }
  cblas_daxpy(M*N, -1.0 , operand2.entry, 1, entry, 1);
  return *this;
}

template <typename T>
denseMat<T> denseMat<T>::operator + (const denseMat &operand2) const
{
//   denseMat sum;
//   int i, j, nn;
//   T cval;

  if (N != operand2.N || M != operand2.M) {
    cout << "Error in Performing Matrix Sum " << endl;
    exit(1);
  }

//   sum.setDim(N, M);
//   nn = 0;
//   for (i = 0; i < N; i ++) {
//     for (j = 0; j < M; j ++) {
//       cval = entry[nn];
//       cval = cval + operand2.getEntry(i, j);
//       nn ++;
//
//       sum.addEntry(i, j, cval);
//     }
//   }
  denseMat sum(*this);
  sum += operand2;
  return sum;

}

template <typename T>
denseMat<T> denseMat<T>::operator - (const denseMat &operand2) const
{
//   denseMat sub;
//   int i, j, nn;
//   T cval;

  if (N != operand2.N || M != operand2.M) {
    cout << "Error in Performing Matrix Subtraction " << endl;
    exit(1);
  }

//   sub.setDim(N, M);
//   nn = 0;
//   for (i = 0; i < N; i ++) {
//     for (j = 0; j < M; j ++) {
//       cval = entry[nn];
//       cval = cval - operand2.getEntry(i, j);
//       nn ++;
//
//       sub.addEntry(i, j, cval);
//     }
//   }

  denseMat sub(*this);
  sub -= operand2;
  return sub;
}

template <typename T>
denseMat<T> &denseMat<T>::operator*(const T &val)
{
  return *this;
}
template <>
denseMat<float> &denseMat<float>::operator*(const float &val)
{
  cblas_sscal(M*N, val, entry, 1);
  return *this;
}
template <>
denseMat<double> &denseMat<double>::operator*(const double &val)
{
  cblas_dscal(M*N, val, entry, 1);
  return *this;
}

template <typename T>
denseMat<T> denseMat<T>::operator*(const denseMat &operand2) const
{
//   denseMat mult;
//   int i, j, k, row, col;
//   T cval, aval, bval;

  if (M != operand2.N) {
    cout << "Error in Matrix Multiplication " << endl;
    exit(1);
  }

//   mult.setDim(N, operand2.M);
//   for (i = 0; i < N; i ++) {
//     row = i;
//     for (j = 0; j < operand2.M; j ++) {
//       col = j;
//
//       cval = 0.0;
//       for (k = 0; k < M; k ++) {
// 	aval = getEntry(i, k);
// 	bval = operand2.getEntry(k, j);
// 	cval = cval + aval * bval;
//       }
//       mult.addEntry(row, col, cval);
//     }
//   }
  denseMat mult(M, operand2.N);
  aABpbC(1.0, 0.0, const_cast<denseMat&>(*this),
         const_cast<denseMat&>(operand2), mult);
  return mult;

}

template <typename T>
void denseMat<T>::scale(const T& val)
{
}
template <>
void denseMat<float>::scale(const float& val)
{
  cblas_sscal(M * N, val, entry, 1);
}
template <>
void denseMat<double>::scale(const double& val)
{
  cblas_dscal(M * N, val, entry, 1);
}

template <typename T>
denseMat<T> denseMat<T>::transpose()
{
  denseMat transP;
  int i, j, row, col;
  T cval;

  transP.setDim(M, N);
  for (i = 0; i < M; i ++) {
    row = i;
    for (j = 0; j < N; j ++) {
      col = j;
      cval = getEntry(col, row);
      transP.addEntry(row, col, cval);
    }
  }

  return transP;
}

template <typename T>
void denseMat<T>::transpose(denseMat* At)
{
  int i, j;
  if (At->entry == 0) At->setDim(M, N);
  for (i = 0; i < M; i++) {
    for (j = 0; j < N; j++)
      At->setEntry(j, i, entry[i * M + j]);
  }
}

template <typename T>
void denseMat<T>::SelfTranspose(){
  int dataLength = M*N;
  T * CopyEntry;
  CopyEntry = new T[dataLength];
  if (entry != 0) {
    for (int c_1 = 0; c_1 < dataLength; ++c_1)
    {
      CopyEntry[c_1] = entry[c_1];
      entry[c_1] = 0.0;
    }

    for (int i = 0; i < M; i++) {
      for (int j = 0; j < N; j++)
        addEntry(j, i, CopyEntry[i * N + j]);
    }
  }
  delete [] CopyEntry;
}

template <typename T>
void denseMat<T>::print()
{
  int i, j, nn;
  FILE *foo;

  foo = fopen("elem.matrix", "a");

  nn = 0;
  for (i = 0; i < N; i ++) {
    for (j = 0; j < M; j ++) {
      fprintf(foo, "%f ", entry[nn]);
      if ((j + 1) % 3 == 0) fprintf(foo, "\n");

      nn ++;
    }
    fprintf(foo, "\n");
  }
  fprintf(foo, "\n\n\n");
  fclose(foo);
}

void Gaussj(denseMat<fp_t> *a)
{
  int    n, *indxc, *indxr, *ipiv;
  int    i, icol, irow, j, k, l, ll;
  fp_t big;
  fp_t  dum, pivinv, tval;

  if (a->getRowDim() != a->getColDim()) {
    printf("Trying to Invert a non-Square matrix \n");
    exit(1);
  }
  n = a->getRowDim();
  indxc = new int[n];
  indxr = new int[n];
  ipiv = new int[n];

  for (j = 0; j < n; j ++) ipiv[j] = 0;
  for (i = 0; i < n; i ++) {
    big = 0.0;
    for (j = 0; j < n; j ++)
      if (ipiv[j] != 1)
	for (k = 0; k < n; k ++) {
	  if (ipiv[k] == 0) {
	    if (fabs(a->getEntry(j, k)) >= big) {
	      big = fabs(a->getEntry(j, k));
	      irow = j; icol = k;
	    }
	  } else if (ipiv[k] > 1) {
	    printf("Gaussj: Singular Matrix -- 1\n");
	    exit(1);
	  }
	}
    ++(ipiv[icol]);
    if (irow != icol) {
      for (l = 0; l < n; l ++) {
	tval = a->getEntry(irow, l);
	a->setEntry(irow, l, a->getEntry(icol, l));
	a->setEntry(icol, l, tval);
      }
    }
    indxr[i] = irow;
    indxc[i] = icol;
    if (fabs(a->getEntry(icol, icol)) < ZERO) {
      printf("Gaussj: Singular Matrix -- 2\n");
      exit(1);
    }
    pivinv = 1.0 / (a->getEntry(icol, icol));
    a->setEntry(icol, icol, 1.0);

    for (l = 0; l < n; l ++) {
      a->setEntry(icol, l, (a->getEntry(icol, l)) * pivinv);
    }
    for (ll = 0; ll < n; ll ++)
      if (ll != icol) {
	dum = a->getEntry(ll, icol);
	a->setEntry(ll, icol, 0.0);

	for (l = 0; l < n; l ++) {
	  tval = a->getEntry(icol, l) * dum;
	  a->setEntry(ll, l, (a->getEntry(ll, l)) - tval);
	}
      }
  }
  for (l = (n-1); l >= 0; l --) {
    if (indxr[l] != indxc[l])
      for (k = 0; k < n; k ++) {
	tval = a->getEntry(k, indxr[l]);
	a->setEntry(k, indxr[l], a->getEntry(k, indxc[l]));
	a->setEntry(k, indxc[l], tval);
      }
  }
  delete [] ipiv;
  delete [] indxr;
  delete [] indxc;
}

void Gaussj(denseMat<fp_t> *a, bool& check)
{
  int    n, *indxc, *indxr, *ipiv;
  int    i, icol, irow, j, k, l, ll;
  fp_t big;
  fp_t  dum, pivinv, tval;

  check = true;

  if (a->getRowDim() != a->getColDim()) {
    printf("Trying to Invert a non-Square matrix \n");
    exit(1);
  }
  n = a->getRowDim();
  indxc = new int[n];
  indxr = new int[n];
  ipiv = new int[n];

  for (j = 0; j < n; j ++) ipiv[j] = 0;
  for (i = 0; i < n; i ++) {
    big = 0.0;
    for (j = 0; j < n; j ++)
      if (ipiv[j] != 1)
	for (k = 0; k < n; k ++) {
	  if (ipiv[k] == 0) {
	    if (fabs(a->getEntry(j, k)) >= big) {
	      big = fabs(a->getEntry(j, k));
	      irow = j; icol = k;
	    }
	  } else if (ipiv[k] > 1) {
	    printf("Gaussj: Singular Matrix -- 1\n");
	    exit(1);
	  }
	}
    ++(ipiv[icol]);
    if (irow != icol) {
      for (l = 0; l < n; l ++) {
	tval = a->getEntry(irow, l);
	a->setEntry(irow, l, a->getEntry(icol, l));
	a->setEntry(icol, l, tval);
      }
    }
    indxr[i] = irow;
    indxc[i] = icol;
    if (fabs(a->getEntry(icol, icol)) < ZERO) {
      printf("Gaussj: Singular Matrix -- 2\n");
      cout << "this is two" << endl;
      check = false;
      exit(1);
    }
    pivinv = 1.0 / (a->getEntry(icol, icol));
    a->setEntry(icol, icol, 1.0);

    for (l = 0; l < n; l ++) {
      a->setEntry(icol, l, (a->getEntry(icol, l)) * pivinv);
    }
    for (ll = 0; ll < n; ll ++)
      if (ll != icol) {
	dum = a->getEntry(ll, icol);
	a->setEntry(ll, icol, 0.0);

	for (l = 0; l < n; l ++) {
	  tval = a->getEntry(icol, l) * dum;
	  a->setEntry(ll, l, (a->getEntry(ll, l)) - tval);
	}
      }
  }
  for (l = (n-1); l >= 0; l --) {
    if (indxr[l] != indxc[l])
      for (k = 0; k < n; k ++) {
	tval = a->getEntry(k, indxr[l]);
	a->setEntry(k, indxr[l], a->getEntry(k, indxc[l]));
	a->setEntry(k, indxc[l], tval);
      }
  }
  delete [] ipiv;
  delete [] indxr;
  delete [] indxc;


}

template <typename T>
denseMat<T> denseMat<T>::inverse()
{
  denseMat invert;
  int i, NZ;

  invert.setDim(N, M);
  NZ = N * M;
  for (i = 0; i < NZ; i ++) invert.entry[i] = entry[i];
  Gaussj(&invert);

  return invert;
}

template <typename T>
void denseMat<T>::inverse(denseMat& invert)
{
  int i, NZ;

  if ((N != invert.N) || (M != invert.M)) {
    delete [] entry;
    invert.setDim(N, M);
  }
  NZ = N * M;
  for (i = 0; i < NZ; i ++) invert.entry[i] = entry[i];
  Gaussj(&invert);
}

template <typename T>
T denseMat<T>::FindSpectralRadius()
{
  return *this;
}

template <>
float denseMat<float>::FindSpectralRadius()
{
  int Ndim = getRowDim();
  int n    = Ndim;
  int lda  = Ndim;
  int ldvl = Ndim;
  int ldvr = Ndim;
  int info;
  /* Local arrays */
  float* EvalReal  = new float[Ndim];
  float* EvalImag  = new float[Ndim];
  float* EvecRight = new float[Ndim*Ndim];
  float* EvecLeft  = new float[Ndim*Ndim];
  int     lwork     = 4*Ndim;
  float* work      = new float[lwork];
  char    VecR      = 'V';
  char    VecL      = 'V';
  /* Solve eigenproblem */
  sgeev_( &VecR, &VecL , &n, getEntryPtr(), &lda, EvalReal, EvalImag, //7
          EvecLeft, &ldvl, EvecRight, &ldvr, work, &lwork, &info); // 9
  /* Check for convergence */
//   if( info > 0 ) {
// 		printf( "Poorly converged eigenvalues.\n" );
// 	}
  /*
// Print eigenvalues
// 	print_eigenvalues( "Eigenvalues", n, EvalReal, EvalImag );
// Print left eigenvectors
// 	print_eigenvectors( "Left eigenvectors", n, EvalImag, EvecLeft, ldvl );
// Print right eigenvectors
// 	print_eigenvectors( "Right eigenvectors", n, EvalImag, EvecRight, ldvr );
*/

  float* MagOfEigVal = new float[Ndim];
  float  lambda_abs  = 0.0;
  for (int i = 0; i < Ndim; i++) {
    lambda_abs     = sqrt(EvalReal[i]*EvalReal[i] + EvalImag[i]*EvalImag[i]);
    MagOfEigVal[i] = lambda_abs;
  }
  float MaxEval = MagOfEigVal[0];
  for (int i = 0; i < Ndim; i++){
    if(MagOfEigVal[i] > MaxEval )  MaxEval = MagOfEigVal[i];
  }

  delete []	EvalReal;
  delete []	EvalImag;
  delete []	EvecRight;
  delete []	EvecLeft;
  delete []	work;
  delete [] MagOfEigVal;

  return MaxEval;
}
template <>
double denseMat<double>::FindSpectralRadius()
{
  int Ndim = getRowDim();
  int n    = Ndim;
  int lda  = Ndim;
  int ldvl = Ndim;
  int ldvr = Ndim;
  int info;
  /* Local arrays */
  double* EvalReal  = new double[Ndim];
  double* EvalImag  = new double[Ndim];
  double* EvecRight = new double[Ndim*Ndim];
  double* EvecLeft  = new double[Ndim*Ndim];
  int     lwork     = 4*Ndim;
  double* work      = new double[lwork];
  char    VecR      = 'V';
  char    VecL      = 'V';
  /* Solve eigenproblem */
  dgeev_( &VecR, &VecL , &n, getEntryPtr(), &lda, EvalReal, EvalImag,
          EvecLeft, &ldvl, EvecRight, &ldvr, work, &lwork, &info);
  /* Check for convergence */
//   if( info > 0 ) {
// 		printf( "Poorly converged eigenvalues.\n" );
// 	}
  /*
// Print eigenvalues
// 	print_eigenvalues( "Eigenvalues", n, EvalReal, EvalImag );
// Print left eigenvectors
// 	print_eigenvectors( "Left eigenvectors", n, EvalImag, EvecLeft, ldvl );
// Print right eigenvectors
// 	print_eigenvectors( "Right eigenvectors", n, EvalImag, EvecRight, ldvr );
*/

  double* MagOfEigVal = new double[Ndim];
  double  lambda_abs  = 0.0;
  for (int i = 0; i < Ndim; i++) {
    lambda_abs     = sqrt(EvalReal[i]*EvalReal[i] + EvalImag[i]*EvalImag[i]);
    MagOfEigVal[i] = lambda_abs;
  }
  double MaxEval = MagOfEigVal[0];
  for (int i = 0; i < Ndim; i++){
    if(MagOfEigVal[i] > MaxEval )  MaxEval = MagOfEigVal[i];
  }

  delete []	EvalReal;
  delete []	EvalImag;
  delete []	EvecRight;
  delete []	EvecLeft;
  delete []	work;
  delete [] MagOfEigVal;

  return MaxEval;
}

template <typename T>
void denseMat<T>::SquareRoot(denseMat& Root)
{
}

template <>
void denseMat<float>::SquareRoot(denseMat<float>& Root)
{
  int Ndim = getRowDim();
  int n    = Ndim;
  int lda  = Ndim;
  int ldvl = Ndim;
  int ldvr = Ndim;
  int info;

  /* Local arrays */
  float* EvalReal  = new float[Ndim];
  float* EvalImag  = new float[Ndim];
  float* EvecRight = new float[Ndim*Ndim];
  float* EvecLeft  = new float[Ndim*Ndim];
  int     lwork     = 4*Ndim;
  float* work      = new float[lwork];
  char    VecR      = 'V';
  char    VecL      = 'V';

  /* Solve eigenproblem */
  sgeev_( &VecR, &VecR, &n, getEntryPtr(), &lda, EvalReal, EvalImag,
          EvecLeft, &ldvl, EvecRight, &ldvr, work, &lwork, &info);

  /* Check for convergence */

  /* allocate and initialise a new matrix B=Z*D */
  float* B = new float[Ndim*Ndim];
  for (int c=0; c<Ndim; c++) {
    for (int r=0; r<Ndim; r++) {
      B[c*N+r] = EvecRight[c*Ndim+r]*sqrt(EvalReal[c]);
    }
  }

  /* calculate the square root R=B*Z^T */
  float* SqRoot = new float[Ndim*Ndim];
  cblas_sgemm(CblasColMajor, CblasNoTrans, CblasTrans, Ndim, Ndim, Ndim,
              1, B, Ndim, EvecRight, Ndim, 0, SqRoot, Ndim);

  for (int c=0; c<Ndim; c++) {
    for (int r=0; r<Ndim; r++) {
      Root.setEntry(r,c, SqRoot[c*Ndim+r]);
    }
  }

  delete []	EvalReal;
  delete []	EvalImag;
  delete []	EvecRight;
  delete []	EvecLeft;
  delete []	work;
  delete []	B;
  delete []	SqRoot;
}


template <>
void denseMat<double>::SquareRoot(denseMat<double>& Root)
{
  int Ndim = getRowDim();
  int n    = Ndim;
  int lda  = Ndim;
  int ldvl = Ndim;
  int ldvr = Ndim;
  int info;

  /* Local arrays */
  double* EvalReal  = new double[Ndim];
  double* EvalImag  = new double[Ndim];
  double* EvecRight = new double[Ndim*Ndim];
  double* EvecLeft  = new double[Ndim*Ndim];
  int     lwork     = 4*Ndim;
  double* work      = new double[lwork];
  char    VecR      = 'V';
  char    VecL      = 'V';

  /* Solve eigenproblem */
  dgeev_( &VecR, &VecR, &n, getEntryPtr(), &lda, EvalReal, EvalImag,
          EvecLeft, &ldvl, EvecRight, &ldvr, work, &lwork, &info);

  /* Check for convergence */

  /* allocate and initialise a new matrix B=Z*D */
  double* B = new double[Ndim*Ndim];
  for (int c=0; c<Ndim; c++) {
    for (int r=0; r<Ndim; r++) {
      B[c*N+r] = EvecRight[c*Ndim+r]*sqrt(EvalReal[c]);
    }
  }

  /* calculate the square root R=B*Z^T */
  double* SqRoot = new double[Ndim*Ndim];
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans, Ndim, Ndim, Ndim,
              1, B, Ndim, EvecRight, Ndim, 0, SqRoot, Ndim);

  for (int c=0; c<Ndim; c++) {
    for (int r=0; r<Ndim; r++) {
      Root.setEntry(r,c, SqRoot[c*Ndim+r]);
    }
  }

  delete []	EvalReal;
  delete []	EvalImag;
  delete []	EvecRight;
  delete []	EvecLeft;
  delete []	work;
  delete []	B;
  delete []	SqRoot;
}

// BLAS 2 Operations
// y = alpha*A*x + beta*y
void aAxpby(float alpha, float beta, denseMat<float>& A, float* x, float* y)
{
    cblas_sgemv(CblasRowMajor, CblasNoTrans, A.getRowDim(), A.getColDim(), alpha,
                A.getEntryPtr(), A.getColDim(), x, 1, beta, y, 1);

}

void aAxpby(double alpha, double beta, denseMat<double>& A, double* x, double* y)
{

    cblas_dgemv(CblasRowMajor, CblasNoTrans, A.getRowDim(), A.getColDim(), alpha,
                A.getEntryPtr(), A.getColDim(), x, 1, beta, y, 1);

}

// y = alpha*A*x + beta*y
void aAxpby(fp_t alpha, fp_t beta, denseMat<fp_t>& A, ArrayFP<fp_t>& x, ArrayFP<fp_t>& y)
{
  aAxpby(alpha, beta, A, x.getEntryPtr(), y.getEntryPtr());
}

// y = alpha*A*x + y
void aAxpy(fp_t alpha, denseMat<fp_t>& A, fp_t* x, fp_t* y)
{
  aAxpby(alpha, 1.0, A, x, y);
}

// y = alpha*A*x + y
void aAxpy(fp_t alpha, denseMat<fp_t>& A, ArrayFP<fp_t>& x, ArrayFP<fp_t>& y)
{
  aAxpby(alpha, 1.0, A, x.getEntryPtr(), y.getEntryPtr());
}

// y = A*x + y
void Axpy(denseMat<fp_t>& A, fp_t* x, fp_t* y)
{
  aAxpby(1.0, 1.0, A, x, y);
}

// y = A*x + y
void Axpy(denseMat<fp_t>& A, ArrayFP<fp_t>& x, ArrayFP<fp_t>& y)
{
  aAxpby(1.0, 1.0, A, x.getEntryPtr(), y.getEntryPtr());
}

// y = alpha*A*x
void aAx(fp_t alpha, denseMat<fp_t>& A, fp_t* x, fp_t* y)
{
  aAxpby(alpha, 0.0, A, x, y);
}

// y = alpha*A*x
void aAx(fp_t alpha, denseMat<fp_t>& A, ArrayFP<fp_t>& x, ArrayFP<fp_t>& y)
{
  aAxpby(alpha, 0.0, A, x.getEntryPtr(), y.getEntryPtr());
}

// y = A*x
void Ax(denseMat<fp_t>& A, fp_t* x, fp_t* y)
{
  aAxpby(1.0, 0.0, A, x, y);
}

// y = A*x
void Ax(denseMat<fp_t>& A, ArrayFP<fp_t>& x, ArrayFP<fp_t>& y)
{
  aAxpby(1.0, 0.0, A, x.getEntryPtr(), y.getEntryPtr());
}

// BLAS 3 Operations
// C = alpha*A*B + beta*C
void aABpbC(float alpha, float beta, denseMat<float>& A, denseMat<float>& B, denseMat<float>& C)
{
#ifdef TEST_CODE
  if (A.getColDim() != B.getRowDim()) {
    cerr << "denscemat.cc::axAxB: Mismatched matrix dimensions." << endl;
    exit(-1);
  }
  if (A.getRowDim() != C.getRowDim()) {
    cerr << "denscemat.cc::axAxB: Mismatched matrix dimensions." << endl;
    exit(-2);
  }
  if (B.getColDim() != C.getColDim()) {
    cerr << "denscemat.cc::axAxB: Mismatched matrix dimensions." << endl;
    exit(-3);
  }
#endif
  // a little dangerous without casting the complex<fp_t> pointers
  cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, A.getRowDim(), B.getColDim(), B.getRowDim(),
              alpha, A.getEntryPtr(), A.getColDim(), B.getEntryPtr(), B.getColDim(), beta,
              C.getEntryPtr(), B.getColDim());
}

void aABpbC(double alpha, double beta, denseMat<double>& A, denseMat<double>& B, denseMat<double>& C)
{
#ifdef TEST_CODE
  if (A.getColDim() != B.getRowDim()) {
    cerr << "denscemat.cc::axAxB: Mismatched matrix dimensions." << endl;
    exit(-1);
  }
  if (A.getRowDim() != C.getRowDim()) {
    cerr << "denscemat.cc::axAxB: Mismatched matrix dimensions." << endl;
    exit(-2);
  }
  if (B.getColDim() != C.getColDim()) {
    cerr << "denscemat.cc::axAxB: Mismatched matrix dimensions." << endl;
    exit(-3);
  }
#endif
  // a little dangerous without casting the complex<fp_t> pointers
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, A.getRowDim(), B.getColDim(), B.getRowDim(),
              alpha, A.getEntryPtr(), A.getColDim(), B.getEntryPtr(), B.getColDim(), beta,
              C.getEntryPtr(), B.getColDim());
}

// C = alpha*A*B
void aAB(fp_t alpha, denseMat<fp_t>& A, denseMat<fp_t>& B, denseMat<fp_t>& C)
{
  aABpbC(alpha, 0.0, A, B, C);
}

// C = A*B
void AB(denseMat<fp_t>& A, denseMat<fp_t>& B, denseMat<fp_t>& C)
{
  aABpbC(1.0, 0.0, A, B, C);
}