#include <iostream>
#include <iosfwd>
#include <fstream>
#include <math.h>
#include <dmtk/enums.h>
#include <dmtk/vector.h>
#include <dmtk/matrix.h>
#include <dmtk/tensor.h>
#include <LAPACK/dsygv.h>

#define PI 3.141592653589793238462643383279502884197
#define PIO2 1.57079632679489661923132169163975144209858
#define TWOPI 6.283185307179586476925286766559005768394


int main()
{
	dmtk::Matrix<double> H;
	dmtk::Matrix<double> F;
	dmtk::Matrix<double> S;
	dmtk::Tensor<double> Q;
        dmtk::Vector<double> c;
	
	int dim = 4;
	
	H.resize(dim,dim);
	F.resize(dim,dim);
	S.resize(dim,dim);
        Q.resize(dim,dim,dim,dim);
        c.resize(dim);

        dmtk::Vector<double> alpha(dim);

        alpha[0] = 0.298073;
        alpha[1] = 1.242567;
        alpha[2] = 5.782948;
        alpha[3] = 38.474970;

        c = 0.5;
        double eprev = 10000;
        int iter = 0;
        double tol = 0.000001;


//      Initialize the matrices H,S, and the tensor Q

        for(int i = 0; i < dim; i++){
          for(int j = 0; j < dim; j++){
            for(int m = 0; m < dim; m++){
              for(int n = 0; n < dim; n++){
                Q(i,j,m,n) += 2*pow(PI,2.5)/(alpha[i]+alpha[j])/(alpha[m]+alpha[n])/sqrt(alpha[i]+alpha[j]+alpha[m]+alpha[n]);
              }
            }
          }
        } 

	for(int i = 0; i < dim; i++){
  	  for(int j = i; j < dim; j++){
	    H(i,j) = 3.*alpha[i]*alpha[j]*pow(PI,1.5)/pow(alpha[i]+alpha[j],2.5)-2*TWOPI/(alpha[i]+alpha[j]);
	    S(i,j) = pow(PI/(alpha[i]+alpha[j]),1.5);
	    H(j,i) = H(i,j);
	    S(j,i) = S(i,j);
          }
        }

/////////////////////////////////////////////////////////////////////////////////////
//	Self-consistent loop

        while(true){
		// calculate matrix elements
	
		for(int i = 0; i < dim; i++){
	  	  for(int j = i; j < dim; j++){
                    F(i,j) = H(i,j);
                    for(int m = 0; m < dim; m++){
                      for(int n = 0; n < dim; n++){
                         F(i,j) += c(m)*c(n)*Q(i,j,m,n);
                      }
                    }
		    F(j,i) = F(i,j);
		  }
		}
	
		// solve the generalized eigenvalue problem

		dmtk::Vector<double> w(dim); //eigenvalues
		dmtk::Vector<double> aux(3*dim); // auxiliary "work" array for lapack
                dmtk::Matrix<double> Saux = S;
		int info;
		dsygv_(1, 'V', 'U', 
			   F.rows(), F.array(), F.rows(), 
			   Saux.array(), Saux.rows(), 
			   w.array(), aux.array(), 3*dim, 
			   info);
		
	
    		// output the results
    		//
                // ground-state energy
              
                for(int i = 0; i < dim; i++) {
                  c(i) = F(0,i);
                }
 
                double ener = 0.;
                for(int p = 0; p < dim; p++){
                  for(int q = 0; q < dim; q++){
                    ener += 2*c(p)*c(q)*H(p,q); 
                    for(int r = 0; r < dim; r++){
                      for(int s = 0; s < dim; s++){
                         ener += Q(p,q,r,s)*c(p)*c(q)*c(r)*c(s);
                      }
                    }
                  }
                }
       
                iter++;
                std::cout << "Iter= " << iter << " Ener= " << ener << " Err= " << abs(eprev-ener) << std::endl; 
                if(abs(ener-eprev) < tol) break;                  
                eprev = ener;
   	}
	 
	return 0;
}