#include "layers.h"
#include <values.h>

void relax(double* E, double* V) {

  int i,n;
  double xm,ym,zm,dx,dy,dz,sqdr,rsqdr,fx,fy,fz,dEdr,rf,rdr6;
  double ddx,ddy,ddz;
  double sqcutoff = cutoff*cutoff;
  double adLxh = 2.0 / Lx;
  double adLyh = 2.0 / Ly;
  double adLzh = 2.0 / Lz;
  double sig6 = pow(sigma,6.0);
  double sig12 = pow(sigma,12.0);
  double A = 4.0*epsilon*sig12;
  double B = 4.0*epsilon*sig6;
  double dA = 48.0*epsilon*sig12;
  double dB = 24.0*epsilon*sig6;
  double E1old,E1new,V1,dE,dV;

  E1old = 0.0;
  E1new = 0.0;
  V1=0.0;

  fx = 0.0;
  fy = 0,0;
  fz = 0.0;

  xm = x[nsites];
  ym = y[nsites];
  zm = z[nsites];

  n = 0;

  if(SHIFT) {
    dE = - EshiftpIA;
    dV = - VshiftpIA;
  } else {
    dE = 0.0;
    dV = 0.0;
  }

  for(i=0;i<nsites;i++) {

    dx = x[i]-xm;
    dy = y[i]-ym;
    dz = z[i]-zm;
    dx -= Lx * (int)(dx * adLxh);
    dy -= Ly * (int)(dy * adLyh);
    dz -= Lz * (int)(dz * adLzh);

    sqdr = dx*dx+dy*dy+dz*dz;

    if(sqdr<sqcutoff) {

      rsqdr  =  1.0/sqdr;
      rdr6   =  rsqdr*rsqdr*rsqdr;
      E1new +=  A * rdr6 * rdr6 - B * rdr6 + dE;
      dEdr   = -dA * rdr6 * rdr6 * rsqdr + dB *rdr6 * rsqdr;
      V1    -=  dEdr * sqdr + dV;

      fx += dEdr * dx;
      fy += dEdr * dy;
      fz += dEdr * dz;

    }
  }

  if(LRC) { 
    E1new += Elrc * (double)(2*nsites+1);
    V1    += Vlrc * (double)(2*nsites+1);
  }

  rf = 1.0/sqrt(fx*fx+fy*fy+fz*fz);
  ddx = fx * rf*0.005;
  ddy = fy * rf*0.005;
  ddz = fz * rf*0.005;
  xm += ddx;
  ym += ddy;
  zm += ddz;

  do {

    n++;

    E1old = E1new;
    E1new = 0.0;

    for(i=0;i<nsites;i++) {

      dx = x[i]-xm;
      dy = y[i]-ym;
      dz = z[i]-zm;
      dx -= Lx * (int)(dx * adLxh);
      dy -= Ly * (int)(dy * adLyh);
      dz -= Lz * (int)(dz * adLzh);

      sqdr = dx*dx+dy*dy+dz*dz;

      if(sqdr<sqcutoff) {

        rsqdr  =  1.0/sqdr;
        rdr6   =  rsqdr*rsqdr*rsqdr;
        E1new +=  A * rdr6 * rdr6 - B * rdr6 + dE;
        dEdr   = -dA * rdr6 * rdr6 * rsqdr + dB *rdr6 * rsqdr;
        V1    -=  dEdr * sqdr + dV;

        fx += dEdr * dx;
        fy += dEdr * dy;
        fz += dEdr * dz;

      }
    }

    rf = 1.0/sqrt(fx*fx+fy*fy+fz*fz);

    ddx = fx * rf * 0.005;
    ddy = fy * rf * 0.005;
    ddz = fz * rf * 0.005;

    xm += ddx;
    ym += ddy;
    zm += ddz;

    if(LRC) { 
      E1new += Elrc * (double)(2*nsites+1);
      V1    += Vlrc * (double)(2*nsites+1);
    }

  } while(E1new<E1old);

  x[nsites] = xm;
  y[nsites] = ym;
  z[nsites] = zm;

  *E = E1new;
  *V = V1;
}
