// $CVSHeader: pal8e/src/coordcalc.c,v 1.4 2005/02/15 01:10:26 indraneel Rel $

#include <stdio.h>
#include <math.h>
#include "coordcalc.h"

#include <Python.h>
#include <Numeric/arrayobject.h>

double dist_sqr_c(COORDINATE a, COORDINATE b)
{
  double x, y, z, dist_sqr;

  x = a.x - b.x;
  y = a.y - b.y;
  z = a.z - b.z;
  dist_sqr = ( x * x ) + ( y * y ) + ( z * z );
  return dist_sqr;
}

double distance_c ( COORDINATE a, COORDINATE b )
{
  double dist, dist_sqr;

  dist_sqr = dist_sqr_c(a, b);
  dist = sqrt(dist_sqr);
  return dist;
}

double torsion_c ( COORDINATE a, COORDINATE b, COORDINATE c, COORDINATE d )
{
  COORDINATE na,  nb;
  double cos_theta, theta, dot, ma, mb, sign;
  // i mi mj j
  // a b  c  d
  /* we need to find the torsion angle here, not just the simple angle */
  /* get i, j, k values for normal to plane i, mi, mj */
  na.x = ( a.y - b.y ) * ( c.z - b.z ) - ( c.y - b.y ) * ( a.z - b.z );
  na.y = ( a.z - b.z ) * ( c.x - b.x ) - ( a.x - b.x ) * ( c.z - b.z );
  na.z = ( a.x - b.x ) * ( c.y - b.y ) - ( a.y - b.y ) * ( c.x - b.x );
  /* get i, j, k values for normal to plane mi, mj, j */
  nb.x = ( c.y - d.y ) * ( c.z - b.z ) - ( c.y - b.y ) * ( c.z - d.z );
  nb.y = ( c.x - b.x ) * ( c.z - d.z ) - ( c.z - b.z ) * ( c.x - d.x );
  nb.z = ( c.y - b.y ) * ( c.x - d.x ) - ( c.x - b.x ) * ( c.y - d.y );
  /* we need the angle between the above two normals like this:
   * a.b = |a||b|cos_theta */
  dot = ( na.x * nb.x ) + ( na.y * nb.y ) + ( na.z * nb.z );
  ma = sqrt ( ( na.x * na.x ) + ( na.y * na.y ) + ( na.z * na.z ) );
  mb = sqrt ( ( nb.x * nb.x ) + ( nb.y * nb.y ) + ( nb.z * nb.z ) );
  cos_theta = dot / ( ma * mb );
  /* cos_theta can only take values from -1 to 1. */
  if ( cos_theta > 1 )
    cos_theta = 1;
  if ( cos_theta < -1 )
    cos_theta = -1;
  theta = acos ( cos_theta );
  /* now check the direction ( +/- ) by a dot product between mi->mj
   * and the cross product of the normals (nb and na) */
  sign = ( c.x - b.x ) * ( na.z * nb.y - na.y * nb.z )
    + ( c.y - b.y ) * ( na.x * nb.z - na.z * nb.x )
    + ( c.z - b.z ) * ( na.y * nb.x - na.x * nb.y );
  /* use only the sign of sign to determine sign of theta */
  if ( sign < 0 ) theta *= -1;
  /* now orient the angle properly between 0-90/90-180 and
   * 0~-90/-90~-180 */
  /* if ( theta > 0 ) theta = PI - theta;
   * else theta = - ( PI + theta ); */
  theta = ( theta > 0.0 ) ? M_PI - theta : -1 * ( M_PI + theta );
  /* now convert theta to degrees */
  theta = theta / M_PI * 180;
  return theta;
}

/* angle between two vectors a and b */
double v_angle_c ( COORDINATE a, COORDINATE b, int range)
{
  double cos_theta, theta;
  double dot, ma, mb;

  /* done by a dot product
   * a.b = |a||b|cos_theta */
  dot = (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
  ma = sqrt((a.x * a.x) + (a.y * a.y) + (a.z * a.z));
  mb = sqrt((b.x * b.x) + (b.y * b.y) + (b.z * b.z));
  cos_theta = dot / ( ma * mb );
  if ( cos_theta > 1 )
    cos_theta = 1;
  if ( cos_theta < -1 )
    cos_theta = -1;
  /* theta in radians */
  theta = acos ( cos_theta );
  /* convert to degrees. 0 to 180 */
  theta = fabs ( theta / M_PI * 180 );
  if ( ( range == 90 ) && ( theta > 90 ) )
    theta = 180 - theta;
  return theta;
}

#ifdef Py_PYTHON_H

/* mod fns */

static PyObject *mkcoord(PyObject *pyobj, COORDINATE *ret)
{
  PyArrayObject *pyaobj;

  pyaobj = (PyArrayObject *)
    PyArray_ContiguousFromObject(pyobj, PyArray_DOUBLE, 1, 1);
  if (pyaobj == NULL)
    return NULL;
  if(pyaobj->dimensions[0] != 3){
    PyErr_SetString(PyExc_TypeError, "mkcoord: coord array must be 1x3 (double)");
    Py_DECREF(pyaobj);
    return NULL;
  }

  // array is contiguous (from Bio.PDB)
  ret->x = *(double *) pyaobj->data;
  ret->y = *(double *) (pyaobj->data + pyaobj->strides[0]);
  ret->z = *(double *) (pyaobj->data + (2 * pyaobj->strides[0]));

  Py_DECREF(pyaobj);
  return Py_None;
}

static PyObject *
dist_sqr(PyObject *self, PyObject *args)
{
  PyObject *pyobj, *pyobja, *pyobjb;
  COORDINATE a, b;
  double result;

  if (! PyArg_ParseTuple(args, "OO", &pyobja, &pyobjb)){
    PyErr_SetString(PyExc_TypeError, "dist_sqr: exactly 2 arguments required");
    return NULL;
  }

  if (mkcoord(pyobja, &a) == NULL)
    return NULL;
  if (mkcoord(pyobjb, &b) == NULL)
    return NULL;

  result = dist_sqr_c(a, b);
  pyobj = PyFloat_FromDouble(result);
  return pyobj;
}

static PyObject *
distance(PyObject *self, PyObject *args)
{
  PyObject *pyobj, *pyobja, *pyobjb;
  COORDINATE a, b;
  double result;

  if (! PyArg_ParseTuple(args, "OO", &pyobja, &pyobjb)){
    PyErr_SetString(PyExc_TypeError, "distance: exactly 2 arguments required");
    return NULL;
  }

  if (mkcoord(pyobja, &a) == NULL)
    return NULL;
  if (mkcoord(pyobjb, &b) == NULL)
    return NULL;

  result = distance_c(a, b);
  pyobj = PyFloat_FromDouble(result);
  return pyobj;
}

static PyObject *
torsion(PyObject *self, PyObject *args)
{
  PyObject *pyobj, *pyobja, *pyobjb, *pyobjc, *pyobjd;
  COORDINATE a, b, c, d;
  double result;

  if (! PyArg_ParseTuple(args, "OOOO", &pyobja, &pyobjb, &pyobjc, &pyobjd)){
    PyErr_SetString(PyExc_TypeError, "torsion: exactly 4 arguments required");
    return NULL;
  }

  if (mkcoord(pyobja, &a) == NULL)
    return NULL;
  if (mkcoord(pyobjb, &b) == NULL)
    return NULL;
  if (mkcoord(pyobjc, &c) == NULL)
    return NULL;
  if (mkcoord(pyobjd, &d) == NULL)
    return NULL;

  result = torsion_c(a, b, c, d);
  pyobj = PyFloat_FromDouble(result);
  return pyobj;
}

static PyObject *
v_angle(PyObject *self, PyObject *args)
{
  PyObject *pyobj, *pyobja, *pyobjb;
  int range;
  COORDINATE a, b;
  double result;

  if (! PyArg_ParseTuple(args, "OOi", &pyobja, &pyobjb, &range)){
    PyErr_SetString(PyExc_TypeError, "v_angle: exactly 2 arguments required");
    return NULL;
  }

  if (mkcoord(pyobja, &a) == NULL)
    return NULL;
  if (mkcoord(pyobjb, &b) == NULL)
    return NULL;

  result = v_angle_c(a, b, range);
  pyobj = PyFloat_FromDouble(result);
  return pyobj;
}

/* reg tab */
static struct PyMethodDef coordcalc_methods[] = {
  {"dist_sqr", dist_sqr, 1},
  {"distance", distance, 1},
  {"torsion", torsion, 1},
  {"v_angle", v_angle, 1},
  {NULL, NULL}
};

/* mod init */
void initcoordcalc()
{
  (void) Py_InitModule("coordcalc", coordcalc_methods);
  import_array();
}

#endif
