/* calibrate.cc */
// this file is to perform the calibration step
// hopefully this should be a lot faster than the python version


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

 
double I[12] = {1,0,0,
		0,1,0,
		0,0,1,
		0,0,0}; // calibration matrix for Mag sensors
double bestM[12];
double bestG[12];
int datasize;

typedef struct dataline {
  double Md[3];
  double Gd[3];
  double bearing, inclination;
} dataline;





/* Get our data into rapid_access C-style data */
dataline
DataFromDict(PyObject *data) {
  dataline temp;
  int i;
  temp.bearing = strtod(PyString_AsString(PyDict_GetItemString(data,"compass")),NULL);
  temp.inclination = strtod(PyString_AsString(PyDict_GetItemString(data,"clino")),NULL);
  for (i = 0; i < 3; ++i) {
    temp.Md[i] = PyFloat_AsDouble(PyList_GetItem(PyDict_GetItemString(data,"M"),i));
    temp.Gd[i] = PyFloat_AsDouble(PyList_GetItem(PyDict_GetItemString(data,"G"),i));
  }
  return temp;
}


dataline*
CreateData(PyObject *data) {
  dataline *temp;
  PyObject *line;
  int i = 0;
  datasize = PyList_Size(data);
  temp = malloc(sizeof(dataline) * datasize); /* this should be free'd on main exit */
  for (i = 0; i < datasize; ++i) {
    temp[i] = DataFromDict(PyList_GetItem(data,i)); 
  }
  return temp;
}


void Normalise(double *x) {
	double _t;
	_t = sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); 
	x[0] /= _t; 
	x[1] /= _t; 
	x[2] /= _t;
}

// a = b * c; b is 3v, c is 34m
void Multiply(double *a, double *b, double *c) {
	a[0] = b[0] * c[0] + b[1] * c[3] + b[2] * c[6] + c[9]; 
	a[1] = b[0] * c[1] + b[1] * c[4] + b[2] * c[7] + c[10];
	a[2] = b[0] * c[2] + b[1] * c[5] + b[2] * c[8] + c[11];
} 

// a = b x c
void CrossProduct(double *a, double *b, double *c) {
	a[0] = b[1] * c[2] - b[2] * c[1]; 
	a[1] = b[2] * c[0] - b[0] * c[2]; 
	a[2] = b[0] * c[1] - b[1] * c[0];
}


void 
GetReadings(double *M, double *G, double *Mc, double *Gc, double *compass, double *clino) {
	// Calculate what our bearing and inclination should be...
	double fM[3];
	double down[3];
	double east[3];
	double north[3];
	Multiply(down,G,Gc);
	Normalise(down);
	down[0] *= -1; // I still don't know why I need to do this!
	down[1] *= -1;
	down[2] *= -1;
	Multiply(fM,M,Mc);
	CrossProduct(east,down,fM);
	Normalise(east);
	CrossProduct(north,east,down);
	Normalise(north);
	*compass = atan2(east[0],north[0]) * (180 / M_PI);
	*clino = atan2(down[0],sqrt(north[0] * north[0] + east[0] * east[0])) * (180 / M_PI);
	if (*compass < 0) *compass += 360;
}

double
GetError(double a1, double a2, double b1, double b2) {
  double diffb = fabs(a1-a2);
  double diffi = fabs(b1-b2);
  while (diffb > 360) diffb -= 360;
  if (diffb > 180) diffb = 360 - diffb;
  while (diffi > 360) diffi -= 360;
  if (diffi > 180) diffi = 360 - diffi;
  return (diffi*diffi)+(diffb*diffb);
}

double
AssessMatrix(double *Mc, double *Gc, dataline *data) {
  double bear, inc;
  double result = 0;
  int i;
  dataline *d;
  d = data;
  for (i = 0; i <  datasize; ++i ) {
    GetReadings(d[i].Md,d[i].Gd,Mc,Gc,&bear,&inc);
    result += GetError(d[i].bearing,bear,d[i].inclination,inc);
  }
  return result;
}

void Teeter(double *Mc, double *Gc, int index, double delta, int axis) {
  switch(axis) {
  case 0: 
    Mc[index] = Mc[index] + delta;
    break;
  case 1:
    Mc[index] = Mc[index] - delta;
    break;
  case 2:
    Gc[index] = Gc[index] + delta;
    break;
  case 3:
    Gc[index] = Gc[index] - delta;
    break;
  }
}

void Calibrate(dataline *data, int iterations) {
  double delta = 1.0;
  double bestR;
  double oldR;
  double result;
  int scale,dir,x;
  bestR = AssessMatrix(bestM,bestG,data);
  oldR = bestR + 1;
  for (scale = 0; scale < iterations; ++scale) {
    printf("%g\n",bestR);
    while (oldR > bestR) {
      oldR = bestR;
      for (dir = 0; dir < 12; ++dir) {
	for (x = 0; x < 4; ++x) {
	  double Mc[12];
	  double Gc[12];
	  memcpy(Mc,bestM,sizeof(double) * 12);
	  memcpy(Gc,bestG,sizeof(double) * 12);
	  Teeter(Mc,Gc,dir,delta,x);
	  result = AssessMatrix(Mc,Gc,data);
	  if (result < bestR) {
	    memcpy(bestM,Mc,sizeof(double) * 12);
	    memcpy(bestG,Gc,sizeof(double) * 12);
	    bestR = result;
	  }
	   
	}
	
      }
    }
    delta = delta/2;
    oldR = bestR + 1;
  }
}

static PyObject*
calibrate(PyObject *self,PyObject *input) {
  dataline *data;
  memcpy(bestM,I,12 * sizeof(double));
  memcpy(bestG,I,12 * sizeof(double));
  data = CreateData(input);
  printf("%d\n",datasize);
  Calibrate(data,12);  
  free(data);
  return Py_BuildValue("[dddddddddddd][dddddddddddd]",
		       bestM[0],bestM[1],bestM[2],bestM[3],
		       bestM[4],bestM[5],bestM[6],bestM[7],
		       bestM[8],bestM[9],bestM[10],bestM[11],
		       bestG[0],bestG[1],bestG[2],bestG[3],
		       bestG[4],bestG[5],bestG[6],bestG[7],
		       bestG[8],bestG[9],bestG[10],bestG[11]);
}

static PyObject*
get_cal_reading(PyObject *self,PyObject *args) {
  PyObject *dict;
  PyObject *M;
  PyObject *G;
  double Mc[12];
  double Gc[12];
  dataline d;
  double compass;
  double clino;
  int i;
  if (!PyArg_ParseTuple(args,"OOO",dict,M,G)) {
    return NULL;
  }
  d = DataFromDict(dict);
  for (i = 0; i < 12; ++i) {
    Mc[i] = PyFloat_AsDouble(PyList_GetItem(M,i));
    Gc[i] = PyFloat_AsDouble(PyList_GetItem(G,i));
  }
  GetReadings(d.Md,d.Gd,Mc,Gc,&compass,&clino);
  return Py_BuildValue("dd",compass,clino);
}

static PyObject*
get_raw_reading(PyObject *self,PyObject *dict) {
  dataline d;
  double compass;
  double clino;
  d = DataFromDict(dict);
  GetReadings(d.Md,d.Gd,I,I,&compass,&clino);
  return Py_BuildValue("dd",compass,clino);
}

/* Python interface stuff */
static PyMethodDef CalMethods[] = {
  {"calibrate",calibrate,METH_O,"run calibration on a data set"},
  {"get_cal_reading",get_cal_reading,METH_VARARGS,"get a calibrated reading on a single data point"},
  {"get_raw_reading",get_raw_reading,METH_O,"get an uncalibrated reading on a single data point"},
  {NULL,NULL,0,NULL}
};

PyMODINIT_FUNC
initcal(void)
{
  (void)Py_InitModule("cal",CalMethods);
}