#include <iostream>
#include <gsl/gsl_cblas.h>
#include "../routines/cfortran.h"
#include "../routines/nrtypes_nr.h"
#include <vector>
PROTOCCALLSFSUB9(DSYEV,dsyev,STRING,STRING,INT, DOUBLEVV,INT, DOUBLEV,DOUBLEV,INT,INT);
#define dsyev(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, INFO) \
CCALLSFSUB9(DSYEV,dsyev,STRING,STRING,INT, DOUBLEVV,INT, DOUBLEV,DOUBLEV,INT,INT, JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, INFO )

PROTOCCALLSFSUB12(DSYGV,dsygv,INT,STRING,STRING,INT, DOUBLEVV,INT,DOUBLEVV,INT, DOUBLEV,DOUBLEV,INT,INT);
#define dsygv(ITYPE, JOBZ, UPLO, N, A, LDA, B,  LDB,  W,WORK, LWORK, INFO) \
CCALLSFSUB12(DSYGV,dsygv,INT,STRING,STRING,INT, DOUBLEVV,INT, DOUBLEVV,INT, DOUBLEV,DOUBLEV,INT,INT, ITYPE,JOBZ, UPLO, N, HMatrix, MaxN, SMatrix, MaxN, Diag, Work, MaxWork, ErrInfo )

using namespace std;


      void DiagMat(Mat_IO_DP &Matrix,vector<double> &Diag,int MSize){
       
 
/*  Reference: Jos M. Thijssen, "Computational Physics", (Cambridge University Press, 1999).
C Driver subroutine for matrix diagonalisation
C of a symmetric matrix, argument 'Matrix'. This is 
C a square matrix of size NSizexNSize, but only the 
C upper left block of size NxN is diagonalised.
C Furthermore, only the lower left triangle of this
C NxN block is needed.
C On exit, the eigenvalues are stored in the array Diag,
C and the eigenvectors are the columns of Matrix


 */   
      int ErrInfo=0; 
      
      const int MaxWork = 10000;
      char JOBZ[] = "V";
      char UPLO[] = "U" ;
      double Work[MaxWork];

      if (6*MSize-1 > MaxWork){     
      cout << "Size of array Work too small in subroutine Geneig" << endl; 
          
}


      dsyev(JOBZ, UPLO, MSize, Matrix, MSize, &Diag[0],Work, MaxWork, ErrInfo);
      if (ErrInfo < 0){ 
        cout << "Argument " << -ErrInfo <<" of subroutine DSYEV had illegal value" << endl;
                   
      }else if (ErrInfo > 0){
        cout << "Convergence failure LAPACK subroutine DSYEV" << endl;
   

}

}
      
void CalcVMatrix(Mat_O_DP &VMatrix,Mat_I_DP &SMatrix,int MSize){   

 Mat_DP Matrix(MSize,MSize);

vector<double> Diag(MSize);
/*  Reference: Jos M. Thijssen, "Computational Physics", (Cambridge University Press, 1999).
This routine calculates the matrix Us^{-1/2}, where U is the 
C matrix which diagonalises S, and s is the diagonal form of S.
*/

      int I;   
      char TRANSA[] ="t";
      char TRANSB[] ="n";

      for(I = 0 ; I <= MSize-1 ; I++)  cblas_dcopy(MSize, &SMatrix[I][0], 1, &VMatrix[I][0], 1);                                                              
for(I = 0 ; I <= MSize-1 ; I++)  cblas_dcopy(MSize, &SMatrix[I][0], 1, &Matrix[I][0], 1);   

      DiagMat(Matrix, Diag, MSize);
     
for(I = 0 ; I <= MSize-1 ; I++)  cblas_dcopy(MSize, &Matrix[I][0], 1, &VMatrix[I][0], 1); 


      for(I = 0 ; I <= MSize-1 ; I++)  Diag[I] = 1/sqrt(Diag[I]);


//C BLAS-1 Routine DCAL is used to calculate Us^{-1/2}

      for(I = 0 ; I <= MSize-1 ; I++) cblas_dscal(MSize, Diag[I], &VMatrix[I][0], 1);

}


      void LowdinDiag(Mat_I_DP &VMatrix,Mat_I_DP &FMatrix,Mat_O_DP &Eigen,vector<double> &Diag, int MSize){
   
Mat_DP Matrix(MSize,MSize);
/*  Reference: Jos M. Thijssen, "Computational Physics", (Cambridge University Press, 1999).
 C This routines uses the matrix V=Us^{-1/2}, where U^\dagger S U = 1,
C to construct H'= V^\dagger H V, and then diagonalises H'.
C It is assumed that only the lower left triangle of HMatrix 
C is filled, i.e. H(I,J) = 0 for I<J

*/
      int I;  
      const int MaxWork = 10000;
      Mat_DP Temp(MSize,MSize); 
      Mat_DP Work(MSize,MSize); 
      char TRANSA[] ="t";
      char TRANSB[] ="n";
      if (MSize*MSize > sqrt(double(MaxWork) ) ){
        cout << "Not enough workspace in routine LowdinDiag"<< endl;
}

      for(I = 0 ; I <= MSize-1 ; I++)  cblas_dcopy(MSize, &FMatrix[I][0], 1, &Eigen[I][0], 1); 

      for(int I = 0 ; I <= MSize-1 ; I++){
          for(int j = 0 ; j <= I ; j++){
               Eigen[j][I] = FMatrix[I][j] ;
}}  


      cblas_dgemm(CblasColMajor,CblasTrans, CblasNoTrans, MSize, MSize, MSize, 1.0, &VMatrix[0][0], MSize, &Eigen[0][0], MSize, 0.0, &Work[0][0], MSize);
      cblas_dgemm(CblasColMajor,CblasNoTrans, CblasNoTrans, MSize, MSize, MSize, 1.0, &Work[0][0], MSize, &VMatrix[0][0], MSize, 0.0, &Eigen[0][0], MSize);

for(I = 0 ; I <= MSize-1 ; I++)  cblas_dcopy(MSize, &Eigen[I][0], 1, &Matrix[I][0], 1); 
                                 
      DiagMat(Matrix, Diag, MSize);    

for(I = 0 ; I <= MSize-1 ; I++) cblas_dcopy(MSize, &Matrix[I][0], 1, &Eigen[I][0], 1); 
                                
    cblas_dgemm(CblasColMajor,CblasNoTrans, CblasNoTrans, MSize, MSize, MSize, 1.0, &VMatrix[0][0], MSize, &Eigen[0][0], MSize, 0.0, &Work[0][0], MSize);
    
      for(I = 0 ; I <= MSize-1 ; I++) cblas_dcopy(MSize, &Work[I][0], 1, &Eigen[I][0], 1);

}