#include <math.h>
#include <assert.h>
#include <string.h>
#include "integrator.h"
#include "force.h"

void check_linearmomentum(const Data *data)
{
  const double errTol = 1e-6;
  double vsum[DIMENSION];
  const double *vel = data->vel;
  const int natoms = data->natoms;
  int i, dim;

  /* memset does not really set the data to zero ? */
  memset(vsum, 0, DIMENSION*sizeof(double));
  for(i = 0; i < natoms; i++) {
    for (dim = 0; dim < DIMENSION; dim++) {
      vsum[dim] += vel[dim];
    }
    vel+=DIMENSION;
  }

  for (dim = 0; dim < DIMENSION; dim++) {
    if (fabs(vsum[dim]) > errTol) {
       printf("linear momentum is not conserved. vsum[%d]=%f\n", 
	      dim, vsum[dim]);
    }
  }

}



int leapfrog(Data *data)
{
  int k, j, nsteps = data->nsteps;
  int natoms = data->natoms;
  double *pos = data->pos;
  double *vel = data->vel;
  double *f = data->f;
  double *m = data->mass;
  double dt = data->dt;
  double half_dt = dt * 0.5;

  for(k=0; k<nsteps; k++) {
    /* HALF KICK */
    for(j=0; j<natoms*DIMENSION; j++)
      vel[j] += (half_dt/m[j])*f[j];
    /* DRIFT : pos n+1 */
    for(j=0; j<natoms*DIMENSION; j++)
      pos[j] += dt*vel[j];
#ifdef PBC
    applyPBC(data);
#endif
    force_poten_eval(data);
    /* HALF KICK : vel n+1 */
    for(j=0; j<natoms*DIMENSION; j++)
      vel[j] += (half_dt/m[j])*f[j];
    energy_eval(data); 
    data->n++;

    if (0 == k % 100) {
      fprintf(data->sim_out,
	    "%8d %20.16g\n",
	    data->n, data->energy * ENERGY_UNIT);  
    }

    check_linearmomentum(data);

  }  

  return 0;
}


int energy_eval(Data *data)
{
  int i, natoms;
  double *vel, *m;
  double sum = 0;

  natoms = data->natoms;
  vel = data->vel;
  m = data->mass;

  for(i=0; i<natoms*DIMENSION; i++) {
    sum += 0.5*m[i]*vel[i]*vel[i];
  }

  data->ke = sum;
  data->energy = data->ke + data->pe;

  return 0;
}


void applyPBC(Data *data)
{
  double len = data->systemsize;
  double *pos = data->pos;
  const int natoms = data->natoms;
  int i;

  for(i=0; i<natoms*DIMENSION; i++) {
    BOUND(pos[i], len); 
  } 
}