/*
 * Purpose:
 *   Numberical integrators for MD++, included in md.cpp
 *   different ensemble_type's and integrator_type's
 *
 * To Do:
 *
 *   1. HELMP not conserved in NPH Gear6, HELMP oscilliates in NPH VVerlet
 *
 *   2. NPT VVerlet need to be implemented
 */


/*******************************************************
 *
 *  NVE ensemble
 *  Gear6 predictor-corrector integrator
 *
 ******************************************************/
#define F02  (3./16.)
#define F12  (251./360.)
#define F32  (11./18.)
#define F42  (1./6.)
#define F52  (1./60.)

void MDFrame::NVE_Gear6()
{
    int i;
    double tmp;
    Matrix33 hinv; Vector3 serr;

    /* initializing memory */    
    Gear6_init();

    /* predictor */
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        _SR[i]+=_VSR[i]+s2[i]+s3[i]+s4[i]+s5[i];
        _VSR[i]+=s2[i]*2.+s3[i]*3.+s4[i]*4.+s5[i]*5.;
        s2[i]+=s3[i]*3.+s4[i]*6.+s5[i]*10.;
        s3[i]+=s4[i]*4.+s5[i]*10.;
        s4[i]+=s5[i]*5.;
    }

    /* evaluator */
    call_potential();
    calcprop();
    
    hinv=_H.inv();

    /* fix me! need to change the unit of velocity to A/ps */
    tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24  //(ps^2)
        /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20;     //(A^2)
    
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2real[i]=hinv*_F[i]*tmp;
        else
            s2real[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }

    /* corrector */
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        serr=s2[i]-s2real[i];
        _SR[i]-=serr*F02;
        _VSR[i]-=serr*F12;
        s2[i]-=serr;
        s3[i]-=serr*F32;
        s4[i]-=serr*F42;
        s5[i]-=serr*F52;
    }
}

/*******************************************************
 *
 *  NVT ensemble
 *  Gear6 predictor-corrector integrator
 *
 ******************************************************/
void MDFrame::NVT_Gear6()
{
    int i;
    double tmp, zetaerr;
    Matrix33 hinv; Vector3 serr;    

    /* initializing memory */    
    Gear6_init();
    
     /* predictor */
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        _SR[i]+=_VSR[i]+s2[i]+s3[i]+s4[i]+s5[i];
        _VSR[i]+=s2[i]*2.+s3[i]*3.+s4[i]*4.+s5[i]*5.;
        s2[i]+=s3[i]*3.+s4[i]*6.+s5[i]*10.;
        s3[i]+=s4[i]*4.+s5[i]*10.;
        s4[i]+=s5[i]*5.;
    }

    zeta+=zetav+zeta2+zeta3+zeta4+zeta5;
    zetav+=zeta2*2.+zeta3*3.+zeta4*4.+zeta5*5.;
    zeta2+=zeta3*3.+zeta4*6.+zeta5*10.;
    zeta3+=zeta4*4.+zeta5*10.;
    zeta4+=zeta5*5.;
    
    /* evaluator */
    call_potential();
    calcprop();
    
    hinv=_H.inv();

    tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24  //(ps^2)
        /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20;     //(A^2)
    
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2real[i]=hinv*_F[i]*tmp;
        else
            s2real[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }

    zetaa=vt2*(_T/_TDES-1)*_TIMESTEP*_TIMESTEP*1e-24*0.5;

    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        s2real[i]-=_VSR[i]*zetav*0.5;
    }

    /* corrector */
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        serr=s2[i]-s2real[i];
        _SR[i]-=serr*F02;
        _VSR[i]-=serr*F12;
        s2[i]-=serr;
        s3[i]-=serr*F32;
        s4[i]-=serr*F42;
        s5[i]-=serr*F52;
    }
    zetaerr=zeta2-zetaa;
    zeta-=zetaerr*F02;
    zetav-=zetaerr*F12;
    zeta2-=zetaerr;
    zeta3-=zetaerr*F32;
    zeta4-=zetaerr*F42;
    zeta5-=zetaerr*F52;
    
}


/*******************************************************
 *
 *  NPH ensemble
 *  Gear6 predictor-corrector integrator
 *
 ******************************************************/
void MDFrame::NPH_Gear6()
{
    int i, j;
    double tmp;
    Matrix33 hinv, htran, vhtran, G, Ginv, VG, VG1, VG2, GiVG, herr;
    Vector3 serr;    

    /* initializing memory */    
    Gear6_init();
    
     /* predictor */
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        _SR[i]+=_VSR[i]+s2[i]+s3[i]+s4[i]+s5[i];
        _VSR[i]+=s2[i]*2.+s3[i]*3.+s4[i]*4.+s5[i]*5.;
        s2[i]+=s3[i]*3.+s4[i]*6.+s5[i]*10.;
        s3[i]+=s4[i]*4.+s5[i]*10.;
        s4[i]+=s5[i]*5.;
    }
    for(i=0;i<3;i++)
        for(j=0;j<3;j++)
            if(conj_fixboxvec[i][j]==1)
                _VH[i][j]=h2[i][j]=h3[i][j]=h4[i][j]=h5[i][j]=0;

    _H+=_VH+h2+h3+h4+h5;
    _VH+= h2*2.+ h3*3.+ h4*4.+ h5*5.;
    h2+= h3*3.+ h4*6.+ h5*10.;
    h3+= h4*4.+ h5*10.;
    h4+= h5*5.;

    /* evaluator */
    call_potential();
    calcprop();
    
    hinv=_H.inv();

    tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24  //(ps^2)
        /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20;     //(A^2)
    
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2real[i]=hinv*_F[i]*tmp;
        else
            s2real[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }
    h2real=_GH*(-tmp)* _ATOMMASS[0]/_WALLMASS;

    if(_BOXDAMP!=0)
        for(i=0;i<3;i++)
            for(j=0;j<3;j++)
                if(conj_fixboxvec[i][j]==0)
                    h2real[i][j]-=_VH[i][j]*_BOXDAMP;

    htran=_H.tran(); G=htran*_H; Ginv=G.inv();
    vhtran=_VH.tran(); VG1=vhtran*_H; VG2=htran*_VH;
    VG=VG1+VG2; GiVG=Ginv*VG;

    for(i=0;i<_NP;i++)
        s2real[i]-=GiVG*_VSR[i];

    /* corrector */
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        serr=s2[i]-s2real[i];
        _SR[i]-=serr*F02;
        _VSR[i]-=serr*F12;
        s2[i]-=serr;
        s3[i]-=serr*F32;
        s4[i]-=serr*F42;
        s5[i]-=serr*F52;
    }    

    /* Parrinello-Rahman boundary condition */
    herr=h2-h2real;
    for(i=0;i<3;i++)
        for(j=0;j<3;j++)
            if(conj_fixboxvec[i][j]==0)
            {
                 _H[i][j] -= herr[i][j]*F02;
                _VH[i][j] -= herr[i][j]*F12;
                 h2[i][j] -= herr[i][j];
                 h3[i][j] -= herr[i][j]*F32;
                 h4[i][j] -= herr[i][j]*F42;
                 h5[i][j] -= herr[i][j]*F52;
            }

}


/*******************************************************
 *
 *  NPT ensemble
 *  Gear6 predictor-corrector integrator
 *
 ******************************************************/
void MDFrame::NPT_Gear6()
{
    int i, j;
    double tmp, zetaerr;
    Matrix33 hinv, htran, vhtran, G, Ginv, VG, VG1, VG2, GiVG, herr;
    Vector3 serr;    

    /* initializing memory */    
    Gear6_init();
    
     /* predictor */
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        _SR[i]+=_VSR[i]+s2[i]+s3[i]+s4[i]+s5[i];
        _VSR[i]+=s2[i]*2.+s3[i]*3.+s4[i]*4.+s5[i]*5.;
        s2[i]+=s3[i]*3.+s4[i]*6.+s5[i]*10.;
        s3[i]+=s4[i]*4.+s5[i]*10.;
        s4[i]+=s5[i]*5.;
    }
    for(i=0;i<3;i++)
        for(j=0;j<3;j++)
            if(conj_fixboxvec[i][j]==1)
                _VH[i][j]=h2[i][j]=h3[i][j]=h4[i][j]=h5[i][j]=0;

    zeta+=zetav+zeta2+zeta3+zeta4+zeta5;
    zetav+=zeta2*2.+zeta3*3.+zeta4*4.+zeta5*5.;
    zeta2+=zeta3*3.+zeta4*6.+zeta5*10.;
    zeta3+=zeta4*4.+zeta5*10.;
    zeta4+=zeta5*5.;
    
    _H+=_VH+h2+h3+h4+h5;
    _VH+= h2*2.+ h3*3.+ h4*4.+ h5*5.;
    h2+= h3*3.+ h4*6.+ h5*10.;
    h3+= h4*4.+ h5*10.;
    h4+= h5*5.;
    
    /* evaluator */
    call_potential();
    calcprop();
    
    hinv=_H.inv();

    tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24  //(ps^2)
        /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20;     //(A^2)
    
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2real[i]=hinv*_F[i]*tmp;
        else
            s2real[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }

    zetaa=vt2*(_T/_TDES-1)*_TIMESTEP*_TIMESTEP*1e-24*0.5;

    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        s2real[i]-=_VSR[i]*zetav*0.5;
    }

    h2real=_GH*(-tmp)* _ATOMMASS[0]/_WALLMASS;
    
    if(_BOXDAMP!=0)
        for(i=0;i<3;i++)
            for(j=0;j<3;j++)
                if(conj_fixboxvec[i][j]==0)
                    h2real[i][j]-=_VH[i][j]*_BOXDAMP;

    htran=_H.tran(); G=htran*_H; Ginv=G.inv();
    vhtran=_VH.tran(); VG1=vhtran*_H; VG2=htran*_VH;
    VG=VG1+VG2; GiVG=Ginv*VG;

    for(i=0;i<_NP;i++)
        s2real[i]-=GiVG*_VSR[i];
    
    /* corrector */
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        serr=s2[i]-s2real[i];
        _SR[i]-=serr*F02;
        _VSR[i]-=serr*F12;
        s2[i]-=serr;
        s3[i]-=serr*F32;
        s4[i]-=serr*F42;
        s5[i]-=serr*F52;
    }
    zetaerr=zeta2-zetaa;
    zeta-=zetaerr*F02;
    zetav-=zetaerr*F12;
    zeta2-=zetaerr;
    zeta3-=zetaerr*F32;
    zeta4-=zetaerr*F42;
    zeta5-=zetaerr*F52;

    herr=h2-h2real;
    for(i=0;i<3;i++)
        for(j=0;j<3;j++)
            if(conj_fixboxvec[i][j]==0)
            {
                 _H[i][j] -= herr[i][j]*F02;
                _VH[i][j] -= herr[i][j]*F12;
                 h2[i][j] -= herr[i][j];
                 h3[i][j] -= herr[i][j]*F32;
                 h4[i][j] -= herr[i][j]*F42;
                 h5[i][j] -= herr[i][j]*F52;
            }        
}



/* Gear6 initializing memory */   
void MDFrame::Gear6_init()
{
  
    if(npold!=_NP)
    {
        Realloc(s2,Vector3,_NP);
        Realloc(s3,Vector3,_NP);
        Realloc(s4,Vector3,_NP);
        Realloc(s5,Vector3,_NP);
        Realloc(s2real,Vector3,_NP);
        
        memset(s2,0,sizeof(Vector3)*_NP);
        memset(s3,0,sizeof(Vector3)*_NP);
        memset(s4,0,sizeof(Vector3)*_NP);
        memset(s5,0,sizeof(Vector3)*_NP);
        memset(s2real,0,sizeof(Vector3)*_NP);

        npold=_NP;
    }
}





/*******************************************************
 *
 *  NVE ensemble
 *  Velocity Verlet integrator
 *
 ******************************************************/
void MDFrame::NVE_VVerlet(int nstep)
{
    int i;
    double tmp;
    Matrix33 hinv;
    
    if(npold!=_NP)
    {
        Realloc(s2,Vector3,_NP);
        memset(s2,0,sizeof(Vector3)*_NP);
        npold=_NP;
    }

    if(nstep>0) /* skip this in the very first step */
    {
        /* s2[i] is the acceleration in previous time step (n) */
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            _VSR[i]+=s2[i];        /* p(n+1/2) */
            _SR[i]+=_VSR[i];       /* q(n+1) */
        }
    }
    /* _SR[i] is the position at current step,  q(n+1) */
    /* _VSR[i] is the velocity at half-step,    p(n+1/2) */
    
    /* compute acceleration a(n+1),  s2[i]=0.5*a*(dt)^2 */
    call_potential();
    calcprop();

    hinv=_H.inv();

    /* fix me! need to change the unit of velocity to A/ps */
    tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24  //(ps^2)
        /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20; //(A^2)

    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2[i]=hinv*_F[i]*tmp;
        else
            s2[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }
        
    /* s2[i] is the acceleration at current step, 0.5*a(n+1)*(dt)^2 */
    
    if(nstep>0) /* skip this in the very first step */
    {
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            _VSR[i]+=s2[i];
        }
    }
    /* _VSR[i] is the velocity at current step, p(n+1) */
}


/*******************************************************
 *
 *  NVT ensemble
 *  Velocity Verlet integrator, implicit algorithm
 *
 ******************************************************/
void MDFrame::NVT_VVerlet_Implicit(int nstep)
{
    int i, n, maxiter;
    double tmp, xinew, B, C;
    Matrix33 hinv;
    Vector3 dvs;
    
    if(npold!=_NP)
    {
        Realloc(s2,Vector3,_NP);
        memset(s2,0,sizeof(Vector3)*_NP);
        npold=_NP;
    }

    if(nstep>0) /* skip this in the very first step */
    {
        zetaa=vt2* (_T/_TDES-1) *_TIMESTEP*_TIMESTEP*1e-24*0.5;
        /* zetaa = xi'(n) *(dt)^2 *0.5 */
        
        /* s2[i] is the acceleration in previous time step (n) */
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;                
            dvs=_VSR[i]; dvs*=zetav*0.5; /* viscous force */
            _VSR[i]+=s2[i]; _VSR[i]-=dvs; /* p(n+1/2) */
            _SR[i]+=_VSR[i];              /* q(n+1) */
        }
        zetav+=zetaa;  /* zetav = xi(n+1/2) *(dt) */
        zeta +=zetav;  /* zeta  = eta(n+1) */
        }
    /* _SR[i] is the position at current step,  q(n+1) */
    /* _VSR[i] is the velocity at half-step,    p(n+1/2) */
    
    /* compute acceleration a(n+1),  s2[i]=0.5*a*(dt)^2 */
    call_potential();
    calcprop();
    
    hinv=_H.inv();
    
    /* fix me! need to change the unit of velocity to A/ps */
    tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24  //(ps^2)
        /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20; //(A^2)
    
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2[i]=hinv*_F[i]*tmp;
        else
            s2[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }
        
    /* s2[i] is the acceleration at current step, 0.5*a(n+1)*(dt)^2 */
    
    if(nstep>0) /* skip this in the very first step */
    {
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            _VSR[i]+=s2[i];
        }
        /* _VSR[i] is the velocity at current step with an unknown factor */
        /*    p(n+1) * (1 + xi(n+1)*dt/2) */
        /* need to solve for xi(n+1) */
        
        /* first compute instananeous temperature */
        tmp=0.5*(_ATOMMASS[0]*1.e-3/AVO)*1e-20 /(_TIMESTEP*_TIMESTEP*1e-24)/EV;
        _KATOM=0; n=0;
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
                n++;
                _VR[i]=_H*_VSR[i];
                if(species[i]==0)
                    _KATOM+=tmp*_VR[i].norm2();
                else
                    _KATOM+=tmp*_VR[i].norm2()*_ATOMMASS[species[i]]/_ATOMMASS[0];
        }
        /* _T=_KATOM*EV/(1.5*BOLZ*(n-1)); */
        _T=_KATOM*EV/(1.5*BOLZ*(n));
        
        /* xi(n+1) satisfies the equation:
         *
         *   xi(n+1) = xi(n+1/2) + vt2*(_T/_TDES/(1+0.5*xi(n+1))^2 - 1)*0.5
         *           = xi(n+1/2) + (B/(1+0.5*xi(n+1))^2 - 1)*C
         */
        maxiter = 1000;
        B = _T/_TDES;
        C = vt2*_TIMESTEP*_TIMESTEP*1e-24*0.5;
        xinew = zetav;
        for(i=0;i<maxiter;i++)
        {
            tmp = zetav + (B/(1+0.5*xinew)/(1+0.5*xinew) - 1)*C;
            if(fabs(tmp-xinew)<1e-14)
                break;
            else
                xinew = tmp;
        }
        
        if(i>=maxiter)
        {
            INFO_Printf("maxiter = %d xinew = %e %e\n",maxiter,xinew,tmp);
            ERROR("maximum iteration exceeded");
            zetav = xinew;        
        }
        else
        {
            //INFO_Printf("zetav_old = %e zetav = %e iter = %d\n",zetav,xinew,i);
            zetav = xinew;        
        }
        
        tmp = 1.0/(1+zetav*0.5);
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            _VSR[i]*=tmp;
        }
        tmp = tmp*tmp; _KATOM *= tmp; _T *= tmp;
    }
}

/*******************************************************
 *
 *  NVT ensemble
 *  Velocity Verlet integrator, explicit algorithm 1
 *
 ******************************************************/
void MDFrame::NVT_VVerlet_Explicit_1(int nstep)
{
    int i, n;
    double tmp;
    Matrix33 hinv;
    Vector3 dvs;
    
    if(npold!=_NP)
    {
        Realloc(s2,Vector3,_NP);
        memset(s2,0,sizeof(Vector3)*_NP);
        npold=_NP;
    }

    if(nstep>0) /* skip this in the very first step */
    {
        zetaa=vt2* (_T/_TDES-1) *_TIMESTEP*_TIMESTEP*1e-24*0.5;
        /* zetaa = xi'(n) *(dt)^2 *0.5 */
        
        /* s2[i] is the acceleration in previous time step (n) */
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;                
            dvs=_VSR[i]; dvs*=zetav*0.5; /* viscous force */
            _VSR[i]+=s2[i]; _VSR[i]-=dvs; /* p(n+1/2) */
            _SR[i]+=_VSR[i];              /* q(n+1) */
        }
        
        /* compute instananeous temperature at half step*/
        tmp=0.5*(_ATOMMASS[0]*1.e-3/AVO)*1e-20 /(_TIMESTEP*_TIMESTEP*1e-24)/EV;
        _KATOM=0; n=0;
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            n++;
            _VR[i]=_H*_VSR[i];
            if(species[i]==0)
                _KATOM+=tmp*_VR[i].norm2();
            else
                _KATOM+=tmp*_VR[i].norm2()*_ATOMMASS[species[i]]/_ATOMMASS[0];
        }
        /* _T=_KATOM*EV/(1.5*BOLZ*(n-1)); */
        _T=_KATOM*EV/(1.5*BOLZ*(n));
        
        zetaa=vt2* (_T/_TDES-1) *_TIMESTEP*_TIMESTEP*1e-24*0.5;
        /* zetaa = xi'(n) *(dt)^2 *0.5 */
        
        zeta +=zetav+zetaa;/* zeta  = eta(n+1) */
        zetav+=zetaa*2.0;  /* zetav = xi(n+1/2) *(dt) */
    }
    
    /* compute acceleration a(n+1),  s2[i]=0.5*a*(dt)^2 */
    call_potential();
    calcprop();
    hinv=_H.inv();
    
    tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24      //(ps^2)
            /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20;     //(A^2)
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2[i]=hinv*_F[i]*tmp;
        else
            s2[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }
    /* s2[i] is the acceleration at current step, 0.5*a(n+1)*(dt)^2 */
    
    if(nstep>0)
    {
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            dvs=_VSR[i]; dvs*=zetav*0.5; /* viscous force */
            _VSR[i]+=s2[i]; _VSR[i]-=dvs; /* p(n+1/2) */
            }
        /* _VSR[i] is the velocity at current step,    p(n+1) */
    }// if nstep>0    
}

/*******************************************************
 *
 *  NVT ensemble
 *  Velocity Verlet integrator, explicit algorithm 2
 *
 ******************************************************/
void MDFrame::NVT_VVerlet_Explicit_2(int nstep)
{
    int i, n;
    double tmp;
    Matrix33 hinv;
    Vector3 dvs;
    
    if(npold!=_NP)
    {
        Realloc(s2,Vector3,_NP);
        memset(s2,0,sizeof(Vector3)*_NP);
        npold=_NP;
    }

    if(nstep>0)
    {
        /* compute instananeous temperature at half step*/
        tmp=0.5*(_ATOMMASS[0]*1.e-3/AVO)*1e-20 /(_TIMESTEP*_TIMESTEP*1e-24)/EV;
        _KATOM=0; n=0;
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            n++;
            _VR[i]=_H*_VSR[i];
            if(species[i]==0)
                _KATOM+=tmp*_VR[i].norm2();
            else
                _KATOM+=tmp*_VR[i].norm2()*_ATOMMASS[species[i]]/_ATOMMASS[0];
            }
        _T=_KATOM*EV/(1.5*BOLZ*(n-1)); 
//            _T=_KATOM*EV/(1.5*BOLZ*(n));        
        
        zetaa=vt2* (_T/_TDES-1) *_TIMESTEP*_TIMESTEP*1e-24*0.5;
        /* zetaa = xi'(n) *(dt)^2 *0.5 */
        
        zetav+=zetaa;  /* zetav = xi(n+1/2) *(dt) */        
        zeta +=zetav;/* zeta  = eta(n+1) */
        
        /* s2[i] is the acceleration in previous time step a(n)*(dt)^2*0.5 */
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            dvs=_VSR[i]; dvs*=exp(-zetav*0.5); /* viscous force */
            _VSR[i]=dvs + s2[i]; /* p(n+1/2) */            
            _SR[i]+=_VSR[i];              /* q(n+1) */
        }
    }
    
    /* compute acceleration a(n+1),  s2[i]=0.5*a*(dt)^2 */
    call_potential();
    calcprop();
    hinv=_H.inv();
    
    tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24      //(ps^2)
        /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20;     //(A^2)
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2[i]=hinv*_F[i]*tmp;
        else
            s2[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }
    
    if(nstep>0)
    {
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            dvs=_VSR[i]; dvs*=exp(-zetav*0.5); /* viscous force */
            _VSR[i]=dvs + s2[i]; /* p(n+1/2) */                        
        }
        
    //        zeta +=zetav*0.5;/* zeta  = eta(n+1) */                
        /* compute instananeous temperature at half step*/
        tmp=0.5*(_ATOMMASS[0]*1.e-3/AVO)*1e-20 /(_TIMESTEP*_TIMESTEP*1e-24)/EV;
        _KATOM=0; n=0;
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            n++;
            _VR[i]=_H*_VSR[i];
            if(species[i]==0)
                _KATOM+=tmp*_VR[i].norm2();
            else
                    _KATOM+=tmp*_VR[i].norm2()*_ATOMMASS[species[i]]/_ATOMMASS[0];
        }
        _T=_KATOM*EV/(1.5*BOLZ*(n-1)); 
//            _T=_KATOM*EV/(1.5*BOLZ*(n));
        
        zetaa=vt2* (_T/_TDES-1) *_TIMESTEP*_TIMESTEP*1e-24*0.5;
        /* zetaa = xi'(n) *(dt)^2 *0.5 */
        
        zetav+=zetaa;  /* zetav = xi(n+1/2) *(dt) */
    }// if nstep>0
}


/*******************************************************
 *
 *  NPH ensemble
 *  Velocity Verlet integrator
 *
 ******************************************************/
void MDFrame::NPH_VVerlet(int nstep)
{
    int i, j;
    Matrix33 hinv, htran, vhtran, G, Ginv, VG, VG1, VG2, GiVG;
    double mass, tmp;
    
    if(npold!=_NP)
    {
        Realloc(s2,Vector3,_NP);
        memset(s2,0,sizeof(Vector3)*_NP);
        npold=_NP;
    }

    if(nstep>0) /* skip this in the very first step */
    {
        /* s2[i] is the acceleration in previous time step (n) */
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            _VSR[i]+=s2[i];        /* p(n+1/2) */
            _SR[i]+=_VSR[i];       /* q(n+1) */
        }

        for(i=0;i<3;i++)
            for(j=0;j<3;j++)
                if(conj_fixboxvec[i][j]==1)
                    _VH[i][j]=h2[i][j]=0;
        _VH+=h2;
        _H+=_VH;
    }
    /* _SR[i] is the position at current step,  q(n+1) */
    /* _VSR[i] is the velocity at half-step,    p(n+1/2) */
    
    /* compute acceleration a(n+1),  s2[i]=0.5*a*(dt)^2 */
    call_potential();
    calcprop();

    hinv=_H.inv();

    /* tmp=0.5*(_TIMESTEP)*(_TIMESTEP)*1e-24  //(ps^2)
        /(_ATOMMASS[0]*1e-3/AVO)*EV/1e-20; //(A^2)
    */
    
    /* ATOMMASS has unit of g/mol
     * mass     has unit of eV ps^2 / A^2 
     * tmp = 0.5 * dt^2 / m  has unit of (A^2/eV) */
    mass = (_ATOMMASS[0]*1e-3/AVO/EV/1e20)*1e24;
    tmp  = 0.5 / mass * SQR(_TIMESTEP);
    
    for(i=0;i<_NP;i++)
    {
        if(fixed[i]) continue;
        if(species[i]==0)
            s2[i]=hinv*_F[i]*tmp;
        else
            s2[i]=hinv*_F[i]*tmp *_ATOMMASS[0]/_ATOMMASS[species[i]];
    }
    h2=_GH*(-tmp)* _ATOMMASS[0]/_WALLMASS; /* in unit of A */
    
    htran=_H.tran(); G=htran*_H; Ginv=G.inv();
    vhtran=_VH.tran(); VG1=vhtran*_H; VG2=htran*_VH;
    VG=VG1+VG2; GiVG=Ginv*VG;

    if(_BOXDAMP!=0)
        for(i=0;i<3;i++)
            for(j=0;j<3;j++)
                if(conj_fixboxvec[i][j]==0)
                    h2[i][j]-=_VH[i][j]*_BOXDAMP;
    
    for(i=0;i<_NP;i++)
        s2[i]-=GiVG*_VSR[i];
    
    /* s2[i] is the acceleration at current step, 0.5*a(n+1)*(dt)^2 */
    if(nstep>0)
    {
        for(i=0;i<_NP;i++)
        {
            if(fixed[i]) continue;
            _VSR[i]+=s2[i];
        }
        for(i=0;i<3;i++)
            for(j=0;j<3;j++)
                if(conj_fixboxvec[i][j]==1)
                    _VH[i][j]=h2[i][j]=0;
        _VH+=h2;
    }
    /* _VSR[i] is the velocity at current step, p(n+1) */
}


/*******************************************************
 *
 *  NPT ensemble
 *  Velocity Verlet integrator
 *
 ******************************************************/
void MDFrame::NPT_VVerlet_Implicit(int nstep)
{
    FATAL("NPT_VVerlet not implemented yet.  Come back soon!");
}

void MDFrame::NPT_VVerlet_Explicit_1(int nstep)
{
    FATAL("NPT_VVerlet not implemented yet.  Come back soon!");
}

void MDFrame::NPT_VVerlet_Explicit_2(int nstep)
{
    FATAL("NPT_VVerlet not implemented yet.  Come back soon!");
}


