#include "HybridMonteCarlo.h"

// gaussian random numbers -- should really be in utilities.cpp
double GaussianRandom(){
        static int iset=0;
        static double gset;
        double fac,rsq,v1,v2;

        if(iset==0){
                do{
                        v1=2.0*rand()/(double)RAND_MAX-1.0;
                        v2=2.0*rand()/(double)RAND_MAX-1.0;
                        rsq=v1*v1+v2*v2;}
                while(rsq>=1.0 || rsq == 0.0);
                fac=sqrt(-2.0*log(rsq)/rsq);
                gset=v1*fac;
                iset=1;
                return(v2*fac);}
		else {
                iset=0;
                return(gset);}}


void HybridMonteCarlo(ScalarField &phi){
ScalarField oldphi,p,newforce,oldforce;
static int first_time=1;
int site,mu,LENGTH;
LatticeVector x,e_mu;
double Snew,Hold,Hnew,EPS;
static double S;
static ScalarField force;
static int no_calls=0,count=0;

//set parameters for classical evolution
EPS=0.1;
LENGTH=(int)(1/EPS);
no_calls++;

if(first_time){

S=0.0;
site=0;
while(SumOverLattice(x,site)){

for(mu=0;mu<D;mu++){
e_mu=LatticeVector(mu);
S+=-kappa*phi.get(x)*phi.get(x+e_mu);}

S+=phi.get(x)*phi.get(x)+LAMBDA*
(phi.get(x)*phi.get(x)-1)*(phi.get(x)*phi.get(x)-1);
}

site=0;
while(SumOverLattice(x,site)){
force.set(x,0.0);

// compute initial force
for(mu=0;mu<D;mu++){
e_mu=LatticeVector(mu);
force.set(x,force.get(x)+kappa*(phi.get(x+e_mu)+phi.get(x-e_mu)));
}

force.set(x,force.get(x)-2*phi.get(x)-
4*LAMBDA*phi.get(x)*(phi.get(x)*phi.get(x)-1));
}

first_time=0;
}


// save copies of initial field;

oldphi=phi;
oldforce=force;

// new gaussian distributed momenta
p=ScalarField();
site=0;
while(SumOverLattice(x,site)){
p.set(x,GaussianRandom());
}


//initial value of H
Hold=S;
site=0;
while(SumOverLattice(x,site)){
Hold+=0.5*p.get(x)*p.get(x);}

//cout << "initial Hamiltonian " << Hold << "\n" << flush;

// start classical trajectory
for(int its=0;its<LENGTH;its++){

//update phi field
site=0;
while(SumOverLattice(x,site)){
phi.set(x,phi.get(x)+EPS*p.get(x)+(EPS*EPS)/2*force.get(x));}

site=0;
while(SumOverLattice(x,site)){
newforce.set(x,0.0);

// new forces with updated phi's
for(mu=0;mu<D;mu++){
e_mu=LatticeVector(mu);
newforce.set(x,newforce.get(x)+kappa*(phi.get(x+e_mu)+phi.get(x-e_mu)));
}

newforce.set(x,newforce.get(x)-2*phi.get(x)-
4*LAMBDA*phi.get(x)*(phi.get(x)*phi.get(x)-1));
}

//update momenta
site=0;
while(SumOverLattice(x,site)){
p.set(x,p.get(x)+EPS/2*(force.get(x)+newforce.get(x)));
}

// end loop on its
force=newforce;
}

// compute final action and Hamiltonian
Snew=0.0;
site=0;
while(SumOverLattice(x,site)){

for(mu=0;mu<D;mu++){
e_mu=LatticeVector(mu);
Snew+=-kappa*phi.get(x)*phi.get(x+e_mu);}

Snew+=phi.get(x)*phi.get(x)+
LAMBDA*(phi.get(x)*phi.get(x)-1)*(phi.get(x)*phi.get(x)-1);
}

Hnew=Snew;
site=0;
while(SumOverLattice(x,site)){
Hnew+=0.5*p.get(x)*p.get(x);
}

//cout << "new Hamiltonian " << Hnew << "\n" << flush;
 
// metropolis test
if(RandomNumber() < exp(-Hnew+Hold)){
S=Snew;
force=newforce;
count++;
//cout << "updating fields\n" << flush;
}
else{
// failed - copy back field
phi=oldphi;
force=oldforce;
}

if(no_calls==(SWEEPS+THERM))
{cout << "HMC acceptance " << (double)count/no_calls << "\n"
<< flush;no_calls=0;count=0;}
return;
}