/* Simple utilities for 3d stuff
 */

#include "Python.h"

/*
def normalise(x, y, z):
    d = math.sqrt(x*x + y*y + z*z)
    return x/d, y/d, z/d
*/
static PyObject *normalise(PyObject *self, PyObject *args) {
	double x, y, z, d;
    PyObject *result, *value;

	if (!PyArg_ParseTuple(args, "ddd", &x, &y, &z)) return NULL;

    // get the magnitude
	d = sqrt(x*x + y*y + z*z);

    // construct the result tuple
    if (!(result = PyTuple_New(3))) return NULL;
    if (!(value = PyFloat_FromDouble(x /= d))) goto normalise_error;
    PyTuple_SET_ITEM(result, 0, value);
    if (!(value = PyFloat_FromDouble(y /= d))) goto normalise_error;
    PyTuple_SET_ITEM(result, 1, value);
    if (!(value = PyFloat_FromDouble(z /= d))) goto normalise_error;
    PyTuple_SET_ITEM(result, 2, value);
	return result;

normalise_error:
    Py_DECREF(result);
    return NULL;
}

/*
def normalise4(x, y, z, w):
    d = math.sqrt(x*x + y*y + z*z)
    return x/d, y/d, z/d, w/d
*/
static PyObject *normalise4(PyObject *self, PyObject *args) {
	double x, y, z, w, d;
    PyObject *result, *value;

    if (!PyArg_ParseTuple(args, "dddd", &x, &y, &z, &w)) return NULL;

    // get the magnitude
	d = sqrt(x*x + y*y + z*z);

    // construct the result tuple
    if (!(result = PyTuple_New(4))) return NULL;
    if (!(value = PyFloat_FromDouble(x/d))) goto normalise_error;
    PyTuple_SET_ITEM(result, 0, value);
    if (!(value = PyFloat_FromDouble(y/d))) goto normalise_error;
    PyTuple_SET_ITEM(result, 1, value);
    if (!(value = PyFloat_FromDouble(z/d))) goto normalise_error;
    PyTuple_SET_ITEM(result, 2, value);
    if (!(value = PyFloat_FromDouble(w/d))) goto normalise_error;
    PyTuple_SET_ITEM(result, 3, value);
	return result;

normalise_error:
    Py_DECREF(result);
    return NULL;
}

/*
def normal(p1, p2, p3):
    '''    p1
           /\
        p2 -- p3
    '''
    ax, ay, az = p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]
    bx, by, bz = p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]
    return normalise(ay*bz-az*by, az*bx-ax*bz, ax*by-ay*bx)
*/
static PyObject *normal(PyObject *self, PyObject *args) {
	double p10, p11, p12, p20, p21, p22, p30, p31, p32;
    double x, y, z, ax, ay, az, bx, by, bz, d;
    PyObject *result, *value;

	if (!PyArg_ParseTuple(args, "(ddd)(ddd)(ddd)", &p10, &p11, &p12,
        &p20, &p21, &p22, &p30, &p31, &p32)) return NULL;

    // figure the two vectors
    ax = p20 - p10;
    ay = p21 - p11;
    az = p22 - p12;
    bx = p30 - p10;
    by = p31 - p11;
    bz = p32 - p12;

    // cross product
    x = ay*bz - az*by;
    y = az*bx - ax*bz;
    z = ax*by - ay*bx;

    // mag so we can normalise
	d = sqrt(x*x + y*y + z*z);

    // make the resultant tuple
    if (!(result = PyTuple_New(3))) return NULL;
    if (!(value = PyFloat_FromDouble(x/d))) goto normal_error;
    PyTuple_SET_ITEM(result, 0, value);
    if (!(value = PyFloat_FromDouble(y/d))) goto normal_error;
    PyTuple_SET_ITEM(result, 1, value);
    if (!(value = PyFloat_FromDouble(z/d))) goto normal_error;
    PyTuple_SET_ITEM(result, 2, value);
	return result;

normal_error:
    Py_DECREF(result);
    return NULL;
}

/*
def average_normal(*normals):
    x, y, z = 0, 0, 0
    for nx, ny, nz in normals:
        x = x + nx
        y = y + ny
        z = z + nz
    return normalise(x, y, z)
*/
static PyObject *average_normal(PyObject *self, PyObject *args) {
	double x = 0, y = 0, z = 0, d;
    PyObject *list, *normal, *result, *value;
    int i, length;

	if (!PyArg_ParseTuple(args, "O", &list)) return NULL;

    // loop through the list
    length = PySequence_Size(list);
    for (i=0; i<length;i++) {
        if (!(normal = PySequence_GetItem(list, i))) return NULL;
        if (!(value = PyTuple_GET_ITEM(normal, 0))) return NULL;
        x += PyFloat_AsDouble(value);
        if (PyErr_Occurred()) return NULL;
        if (!(value = PyTuple_GET_ITEM(normal, 1))) return NULL;
        y += PyFloat_AsDouble(value);
        if (PyErr_Occurred()) return NULL;
        if (!(value = PyTuple_GET_ITEM(normal, 2))) return NULL;
        z += PyFloat_AsDouble(value);
        if (PyErr_Occurred()) return NULL;
    }

    // mag so we can normalise
	d = sqrt(x*x + y*y + z*z);

    // make the resultant tuple
    if (!(result = PyTuple_New(3))) return NULL;
    if (!(value = PyFloat_FromDouble(x/d))) goto average_normal_error;
    PyTuple_SET_ITEM(result, 0, value);
    if (!(value = PyFloat_FromDouble(y/d))) goto average_normal_error;
    PyTuple_SET_ITEM(result, 1, value);
    if (!(value = PyFloat_FromDouble(z/d))) goto average_normal_error;
    PyTuple_SET_ITEM(result, 2, value);
	return result;

average_normal_error:
    Py_DECREF(result);
    return NULL;
}

/* List of functions defined in the module */
static PyMethodDef util_methods[] = {
	{"normalise",		normalise,		METH_VARARGS},
	{"normalise4",		normalise4,		METH_VARARGS},
	{"normal",		    normal,		    METH_VARARGS},
	{"average_normal",	average_normal,	METH_VARARGS},
	{NULL,		NULL}
};

DL_EXPORT(void) init_util(void) {
	Py_InitModule("_util", util_methods);
}

