#include "Python.h"
#include "arrayobject.h"
#include <math.h>

#define IDATA(p) ((int *) (((PyArrayObject *)p)->data))
#define DDATA(p) ((double *) (((PyArrayObject *)p)->data))

static PyObject *py_cdistance(PyObject *self, PyObject *args);
static PyObject *py_cCalcESEnergy(PyObject *self, PyObject *args);
double c_distance_function(double xi,double yi,double zi,
			   double xj,double yj,double zj);
double c_calc_ES_Energy(int n, int *q, double *x, double *y, double *z);

static PyObject *py_cCalcESEnergy(PyObject *self, PyObject *args){
  int n,ok;
  PyObject *xarray, *yarray, *zarray, *qarray;
  int *q;
  double *x, *y, *z;
  double energy;

  ok = PyArg_ParseTuple(args,"iOOOO",&n,&qarray,&xarray,&yarray,&zarray);

  if (!ok){
    fprintf(stderr,"Error (cCalcESEnergy) in parsing arguments\n");
    exit(1);
  }
  q = IDATA(qarray);
  x = DDATA(xarray);
  y = DDATA(yarray);
  z = DDATA(zarray);
  
  energy = c_calc_ES_Energy(n,q,x,y,z);

  return Py_BuildValue("d",energy);
}

double c_calc_ES_Energy(int n, int *q, double *x, double *y, double *z){
  int i,j;
  double energy = 0.;
  double xi,yi,zi,xj,yj,zj;
  int qi,qj;
  for (i=0; i<n; i++){
    for (j=0; j<i; j++){
      energy += q[i]*q[j]/c_distance_function(x[i],y[i],z[i],
					      x[j],y[j],z[j]);
    }
  }
  return energy;
}

static PyObject *py_cdistance(PyObject *self, PyObject *args){
  double xi,yi,zi,xj,yj,zj;
  double dist;
  int ok;
  ok = PyArg_ParseTuple(args,"dddddd",&xi,&yi,&zi,&xj,&yj,&zj);
  
  if (!ok){
    fprintf(stderr,"Error (cdistance) in parsing arguments\n");
    exit(1);
  }
  dist = c_distance_function(xi,yi,zi,xj,yj,zj);
  return Py_BuildValue("d",dist);
}

double c_distance_function(double xi,double yi,double zi,
			   double xj,double yj,double zj){
  return sqrt((xi-xj)*(xi-xj)+(yi-yj)*(yi-yj)+(zi-zj)*(zi-zj));
}

static PyMethodDef Speedtest_methods[] = {
  {"cdistance",py_cdistance,METH_VARARGS},
  {"cCalcESEnergy",py_cCalcESEnergy,METH_VARARGS},
  {NULL,NULL} /* Sentinel */
};
void initSpeedtest(){
  (void) Py_InitModule("Speedtest",Speedtest_methods);
}