/* Frustum object
 */

#include "Python.h"

enum _side {RIGHT = 0, LEFT = 1, BOTTOM = 2, TOP = 3, BACK = 4, FRONT = 5};
enum _plane {A = 0, B = 1, C = 2, D = 3};

static void normalise(double plane[4]) {
    double magnitude;
    magnitude = sqrt(plane[A]*plane[A]+plane[B]*plane[B]+plane[C]*plane[C]);
    plane[A] /= magnitude;
    plane[B] /= magnitude;
    plane[C] /= magnitude;
    plane[D] /= magnitude; 
}

typedef struct {
	PyObject_HEAD
	double planes[6][4];
    PyObject *glGetFloatv;
    PyObject *GL_PROJECTION_MATRIX;
//    PyObject *GL_MODELVIEW_MATRIX;
} FrustumObject;

staticforward PyTypeObject Frustum_Type;

#define FrustumObject_Check(v)	((v)->ob_type == &Frustum_Type)

static FrustumObject *newFrustumObject(PyObject *arg) {
	FrustumObject *self;
    PyObject *module;

	if (!(self = PyObject_New(FrustumObject, &Frustum_Type))) return NULL;

    // get the gl stuff for later
    module = PyImport_ImportModule("OpenGL.GL");
    self->glGetFloatv = PyObject_GetAttrString(module, "glGetFloatv");
    self->GL_PROJECTION_MATRIX = PyObject_GetAttrString(module,
        "GL_PROJECTION_MATRIX");
//    self->GL_MODELVIEW_MATRIX = PyObject_GetAttrString(module,
//        "GL_MODELVIEW_MATRIX");
    Py_DECREF(module);

	return self;
}

/* Frustum methods */
static void Frustum_dealloc(FrustumObject *self) {
    Py_DECREF(self->glGetFloatv);
    Py_DECREF(self->GL_PROJECTION_MATRIX);
//    Py_DECREF(self->GL_MODELVIEW_MATRIX);
	PyObject_Del(self);
}

#define CLIP(x,y) PyFloat_AsDouble(PySequence_GetItem(PySequence_GetItem(c,x),y))
static PyObject *calculate(FrustumObject *self, PyObject *args) {
    PyObject *p=NULL, *m=NULL, *c=NULL;
    double c03, c13, c23, c33;
    double c0n, c1n, c2n, c3n;
    int i;

	if (!PyArg_ParseTuple(args, ":calculate"))
		return NULL;

    // get the matrices
    c = PyObject_CallFunction(self->glGetFloatv, "O",
        self->GL_PROJECTION_MATRIX);
    if (!c) return NULL;
//    m = PyObject_CallFunction(self->glGetFloatv, "O",
//        self->GL_PROJECTION_MATRIX);
//    if (!m) return NULL;

    // matrix mult those two to get the clipping matrix
//    c = p *m

    /* Now we actually want to get the sides of the frustum.
       To do this we take the clipping planes we received above and extract
       the sides from them. We define the normals of the clipping planes
       to be facing into the clipping box. */
    c03 = CLIP(0,3);
    c13 = CLIP(1,3);
    c23 = CLIP(2,3);
    c33 = CLIP(3,3);
    if (PyErr_Occurred()) return NULL;

    // right/left
    c0n = CLIP(0,0);
    c1n = CLIP(1,0);
    c2n = CLIP(2,0);
    c3n = CLIP(3,0);
    if (PyErr_Occurred()) return NULL;
	self->planes[RIGHT][A] = c03 - c0n;
	self->planes[RIGHT][B] = c13 - c1n;
	self->planes[RIGHT][C] = c23 - c2n;
	self->planes[RIGHT][D] = c33 - c3n;
	self->planes[LEFT][A] = c03 + c0n;
	self->planes[LEFT][B] = c13 + c1n;
	self->planes[LEFT][C] = c23 + c2n;
	self->planes[LEFT][D] = c33 + c3n;

    // top/bottom
    c0n = CLIP(0,1);
    c1n = CLIP(1,1);
    c2n = CLIP(2,1);
    c3n = CLIP(3,1);
    if (PyErr_Occurred()) return NULL;
	self->planes[TOP][A] = c03 - c0n;
	self->planes[TOP][B] = c13 - c1n;
	self->planes[TOP][C] = c23 - c2n;
	self->planes[TOP][D] = c33 - c3n;
	self->planes[BOTTOM][A] = c03 + c0n;
	self->planes[BOTTOM][B] = c13 + c1n;
	self->planes[BOTTOM][C] = c23 + c2n;
	self->planes[BOTTOM][D] = c33 + c3n;

    // back/front
    c0n = CLIP(0,2);
    c1n = CLIP(1,2);
    c2n = CLIP(2,2);
    c3n = CLIP(3,2);
    if (PyErr_Occurred()) return NULL;
	self->planes[BACK][A] = c03 - c0n;
	self->planes[BACK][B] = c13 - c1n;
	self->planes[BACK][C] = c23 - c2n;
	self->planes[BACK][D] = c33 - c3n;
	self->planes[FRONT][A] = c03 + c0n;
	self->planes[FRONT][B] = c13 + c1n;
	self->planes[FRONT][C] = c23 + c2n;
	self->planes[FRONT][D] = c33 + c3n;

    for (i=0; i<6; i++) normalise(self->planes[i]);

    if (p) {Py_DECREF(p);}
    if (m) {Py_DECREF(m);}
    if (c) {Py_DECREF(c);}

    PyErr_Clear();
	Py_INCREF(Py_None);
	return Py_None;
}


/*
The plane equations are (A*x + B*y + C*z + D = 0)

If we plug in the point's values, the result of this equation will
be:
    == 0   - point is on the plane
    > 0    - point is in front of the plane
    < 0    - point is behind the plane

"Behind" in this case is outside the clipping box - outside the
viewing area. We say it's outside the frustum if it's outside _any_
of the clipping planes.
*/
static PyObject *isPointInside(FrustumObject *self, PyObject *args) {
    double x, y, z, result;
    int i;

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

    for (i=0; i<6; i++) {
        result = self->planes[i][0]*x;
        result += self->planes[i][1]*y;
        result += self->planes[i][2]*z;
        result += self->planes[i][3];
        if (result <= 0) return PyInt_FromLong(0);
    }
    return PyInt_FromLong(1);
}

/*
Similar to the point method, this one checks against the radius of the sphere.
*/
static PyObject *isSphereInside(FrustumObject *self, PyObject *args) {
    double x, y, z, radius, result;
    int i;

	if (!PyArg_ParseTuple(args, "dddd:isSphereInside", &x, &y, &z, &radius))
		return NULL;
    radius = -radius;

    for (i=0; i<6; i++) {
        result = self->planes[i][0]*x;
        result += self->planes[i][1]*y;
        result += self->planes[i][2]*z;
        result += self->planes[i][3];
        if (result <= radius) return PyInt_FromLong(0);
    }
    return PyInt_FromLong(1);
}

/*
Test a cube at position (x, y, z) with side length size*2 by
just checking all the corners in turn. If all of the corners are
behind any of the frustum planes, then the box is out of the
frustum.

This may return some false positives since we only check one plane
at a time. A more complex method would be needed to determine if,
eg.      /
        /
  Cube /._____
      / | Frustum
     /  |
*/
static PyObject *isCubeInside(FrustumObject *self, PyObject *args) {
    double x, y, z, size;
    double a, b, c, d;
    int i;

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

    for (i=0; i<6; i++) {
        a = self->planes[i][A];
        b = self->planes[i][B];
        c = self->planes[i][C];
        d = self->planes[i][D];

        // test all of the corners of the box in turn
        if (a*(x - size) + b*(y - size) + c*(z - size) + d > 0) continue;
        if (a*(x + size) + b*(y - size) + c*(z - size) + d > 0) continue;
        if (a*(x - size) + b*(y + size) + c*(z - size) + d > 0) continue;
        if (a*(x + size) + b*(y + size) + c*(z - size) + d > 0) continue;
        if (a*(x - size) + b*(y - size) + c*(z + size) + d > 0) continue;
        if (a*(x + size) + b*(y - size) + c*(z + size) + d > 0) continue;
        if (a*(x - size) + b*(y + size) + c*(z + size) + d > 0) continue;
        if (a*(x + size) + b*(y + size) + c*(z + size) + d > 0) continue;
        return PyInt_FromLong(0);
    }
    return PyInt_FromLong(1);
}

/*
Do the isPoint check for each point in l, return true if any are inside.
*/
static PyObject *isPolygonInside(FrustumObject *self, PyObject *args) {
    PyObject *list, *entry, *value;
    double x, y, z, xt=0, yt=0, zt=0, result;
    int i, n, ok, length;

	if (!PyArg_ParseTuple(args, "0|ddd:calculate", &list, &xt, &yt, &zt))
		return NULL;

    length = PySequence_Size(list);
    for (n=0; n<length; n++) {
        // first, extract the point coordinates
        if (!(entry = PySequence_GetItem(list, n))) return NULL;
        if (!(value = PyTuple_GET_ITEM(entry, 0))) return NULL;
        x = PyFloat_AsDouble(value) + xt;
        if (PyErr_Occurred()) return NULL;
        if (!(value = PyTuple_GET_ITEM(entry, 1))) return NULL;
        y = PyFloat_AsDouble(value) + yt;
        if (PyErr_Occurred()) return NULL;
        if (!(value = PyTuple_GET_ITEM(entry, 2))) return NULL;
        z = PyFloat_AsDouble(value) + zt;
        if (PyErr_Occurred()) return NULL;

        // now check the three point coordinates against the planes
        ok = 1;
        for (i=0; i<6; i++) {
            result = self->planes[i][0]*x;
            result += self->planes[i][1]*y;
            result += self->planes[i][2]*z;
            result += self->planes[i][3];
            if (result <= 0) {
                // point is behind this plane, reject
                ok = 0;
                break;
            }
        }
        // this point passed all the plane tests, so the polygon passes
        if (ok) return PyInt_FromLong(1);
    }
    return PyInt_FromLong(0);
}

static PyMethodDef Frustum_methods[] = {
	{"calculate", (PyCFunction)calculate, METH_VARARGS},
	{"isPointInside", (PyCFunction)isPointInside, METH_VARARGS},
	{"isSphereInside", (PyCFunction)isSphereInside, METH_VARARGS},
	{"isCubeInside", (PyCFunction)isCubeInside, METH_VARARGS},
	{"isPolygonInside", (PyCFunction)isPolygonInside, METH_VARARGS},
	{NULL,		NULL}		/* sentinel */
};


static PyObject *Frustum_getattr(FrustumObject *self, char *name) {
	return Py_FindMethod(Frustum_methods, (PyObject *)self, name);
}

statichere PyTypeObject Frustum_Type = {
	PyObject_HEAD_INIT(NULL)
	0,			/*ob_size*/
	"Frustum",			/*tp_name*/
	sizeof(FrustumObject),	/*tp_basicsize*/
	0,			/*tp_itemsize*/
	/* methods */
	(destructor)Frustum_dealloc, /*tp_dealloc*/
	0,			/*tp_print*/
	(getattrfunc)Frustum_getattr, /*tp_getattr*/
	0, /*tp_setattr*/
	0,			/*tp_compare*/
	0,			/*tp_repr*/
	0,			/*tp_as_number*/
	0,			/*tp_as_sequence*/
	0,			/*tp_as_mapping*/
	0,			/*tp_hash*/
};
/* --------------------------------------------------------------------- */

/* Function of no arguments returning new Frustum object */
static PyObject *frustum_new(PyObject *self, PyObject *args) {
	FrustumObject *rv;
	
	if (!PyArg_ParseTuple(args, ":new"))
		return NULL;
	rv = newFrustumObject(args);
	if ( rv == NULL )
	    return NULL;
	return (PyObject *)rv;
}


/* List of functions defined in the module */
static PyMethodDef frustum_methods[] = {
	{"Frustum",		frustum_new,		METH_VARARGS},
	{NULL,		NULL}		/* sentinel */
};


DL_EXPORT(void) init_frustum(void) {
	Frustum_Type.ob_type = &PyType_Type;
	Py_InitModule("_frustum", frustum_methods);
}
