/*
UTc! module: src.matrix_c

Purpose
=======
 Provides a centralised location for matrix-related operations.
 
 This is an optional C implementation, since these are all math-heavy routines.
 
Legal
=====
 All code, unless otherwise indicated, is original, and subject to the
 terms of the GPLv3, which is provided in COPYING.
 
 (C) Neil Tallim, 2009
*/
#include <Python.h>
#include <math.h>

#define RADIAN_FACTOR 0.017453292519943295
#define DEGREE_FACTOR 57.295779513082323

char matrix_c_buildIdentityRenderMatrix__doc__[] =
 "Constructs a render matrix that represents an object without any scaling.\n"
 "\n"
 "@type position: sequence(3)\n"
 "@param position: The (x, y, z) co-ordinates of the centre of this object.\n"
 "@rtype: tuple(12)\n"
 "@return: The requested 4x4 render matrix.\n";
static PyObject* matrix_c_buildIdentityRenderMatrix(PyObject* self, PyObject* args){
	float x, y, z;
	
	if (!PyArg_ParseTuple(args, "(fff):buildIdentityRenderMatrix",
	 &x, &y, &z)){
		return NULL;
	}
	
	return Py_BuildValue(
	 "(ffffffffffffffff)",
	 1.0, 0.0, 0.0, 0.0,
	 0.0, 1.0, 0.0, 0.0,
	 0.0, 0.0, 1.0, 0.0,
	 x, y, z, 1.0
	);
}

char matrix_c_buildRenderMatrix__doc__[] =
 "Constructs a render matrix that represents an object with scaling based on\n"
 "the provided rotation matrix.\n"
 "\n"
 "@type rotation_matrix: sequence(9)\n"
 "@param rotation_matrix: The rotation matrix that specifies the scaling to\n"
 "	 apply to the generated render matrix.\n"
 "@type position: sequence(3)\n"
 "@param position: The (x, y, z) co-ordinates of the centre of this object.\n"
 "\n"
 "@rtype: tuple(12)\n"
 "@return: The requested 4x4 render matrix.\n";
static PyObject* matrix_c_buildRenderMatrix(PyObject* self, PyObject* args){
	float m_1_1, m_1_2, m_1_3, m_2_1, m_2_2, m_2_3, m_3_1, m_3_2, m_3_3;
	float x, y, z;
	
	if (!PyArg_ParseTuple(args, "(fffffffff)(fff):buildRenderMatrix",
	 &m_1_1, &m_1_2, &m_1_3,
	 &m_2_1, &m_2_2, &m_2_3,
	 &m_3_1, &m_3_2, &m_3_3,
	 &x, &y, &z)){
		return NULL;
	}
	
	return Py_BuildValue(
	 "(ffffffffffffffff)",
	 m_1_1, m_2_1, m_3_1, 0.0,
	 m_1_2, m_2_2, m_3_2, 0.0,
	 m_1_3, m_2_3, m_3_3, 0.0,
	 x, y, z, 1.0
	);
}

char matrix_c_buildRotationMatrix__doc__[] =
 "Converts (x, y, z) Euler rotations into a rotation matrix.\n"
 "\n"
 "The order of application is roll (Z), yaw (Y), pitch (X).\n"
 "\n"
 "@type x_rot: float\n"
 "@param x_rot: The rotation around the X-axis, in degrees.\n"
 "@type y_rot: float\n"
 "@param y_rot: The rotation around the Y-axis, in degrees.\n"
 "@type z_rot: float\n"
 "@param z_rot: The rotation around the Z-axis, in degrees.\n"
 "\n"
 "@rtype: tuple(9)\n"
 "@return: The 3x3 rotation matrix that represents the given Euler rotations.\n";
static PyObject* matrix_c_buildRotationMatrix(PyObject* self, PyObject* args){
	float x_rot, y_rot, z_rot;
	float rx, ry, rz;
	float cx, cy, cz;
	float sx, sy, sz;
	float sx_sy, cx_sy;
	
	if (!PyArg_ParseTuple(args, "fff:buildRotationMatrix",
	 &x_rot, &y_rot, &z_rot)){
		return NULL;
	}
	
	rx = x_rot * RADIAN_FACTOR;
	ry = y_rot * RADIAN_FACTOR;
	rz = z_rot * RADIAN_FACTOR;
	cx = cos(rx);
	cy = cos(ry);
	cz = cos(rz);
	sx = sin(rx);
	sy = sin(ry);
	sz = sin(rz);
	sx_sy = sx * sy;
	cx_sy = cx * sy;
	
	return Py_BuildValue(
	 "(fffffffff)",
	 cz * cy, cz * sx_sy - sz * cx, cz * cx_sy + sz * sx,
	 sz * cy, sz * sx_sy + cz * cx, sz * cx_sy - cz * sx,
	 -sy, cy * sx, cy * cx
	);
}

char matrix_c_computePosition__doc__[] =
 "Projects an (x, y, z) co-ordinate relative to a given position, based on a\n"
 "given rotation matrix.\n"
 "\n"
 "@type pos_x: float\n"
 "@param pos_x: The X-co-ordinate of this projection's origin.\n"
 "@type pos_y: float\n"
 "@param pos_y: The Y-co-ordinate of this projection's origin.\n"
 "@type pos_z: float\n"
 "@param pos_z: The Z-co-ordinate of this projection's origin.\n"
 "@type offset_x: float\n"
 "@param offset_x: The relative X-offset, based on the rotation matrix.\n"
 "@type offset_y: float\n"
 "@param offset_y: The relative Y-offset, based on the rotation matrix.\n"
 "@type offset_z: float\n"
 "@param offset_z: The relative Z-offset, based on the rotation matrix.\n"
 "@type rotation_matrix: sequence(9)\n"
 "@param rotation_matrix: The rotation matrix to use for the projection.\n"
 "\n"
 "@rtype: tuple(3)\n"
 "@return: The (x, y, z) co-ordinate of the projected point.\n";
static PyObject* matrix_c_computePosition(PyObject* self, PyObject* args){
	float m_1_1, m_1_2, m_1_3, m_2_1, m_2_2, m_2_3, m_3_1, m_3_2, m_3_3;
	float x, y, z;
	float offset_x, offset_y, offset_z;
	
	if (!PyArg_ParseTuple(args, "ffffff(fffffffff):computePosition",
	 &x, &y, &z,
	 &offset_x, &offset_y, &offset_z,
	 &m_1_1, &m_1_2, &m_1_3,
	 &m_2_1, &m_2_2, &m_2_3,
	 &m_3_1, &m_3_2, &m_3_3)){
		return NULL;
	}
	
	return Py_BuildValue(
	 "(fff)",
	 x + (offset_x * m_1_1 + offset_y * m_1_2 + offset_z * m_1_3),
	 y + (offset_x * m_2_1 + offset_y * m_2_2 + offset_z * m_2_3),
	 z + (offset_x * m_3_1 + offset_y * m_3_2 + offset_z * m_3_3)
	);
}

char matrix_c_decomposeRotationMatrix__doc__[] =
 "Determines one possible Euler representation of a rotation matrix.\n"
 "\n"
 "Note: Gimbal lock is addressed by locking X.\n" 
 "\n"
 "@type rotation_matrix: sequence(9)\n"
 "@param rotation_matrix: The rotation matrix to be decomposed.\n"
 "\n"
 "@rtype: tuple(3)\n"
 "@return: The (x, y, z) rotations, in degrees, of the Euler representation.\n";
static PyObject* matrix_c_decomposeRotationMatrix(PyObject* self, PyObject* args){
	float m_1_1, m_1_2, m_1_3, m_2_1, m_2_2, m_2_3, m_3_1, m_3_2, m_3_3;
	float x_rot, y_rot, z_rot;
	
	if (!PyArg_ParseTuple(args, "(fffffffff):decomposeRotationMatrix",
	 &m_1_1, &m_1_2, &m_1_3,
	 &m_2_1, &m_2_2, &m_2_3,
	 &m_3_1, &m_3_2, &m_3_3)){
		return NULL;
	}
	
	y_rot = m_3_1;
	if(abs(y_rot) <= 1.0){
		x_rot = fmod(atan2(m_3_2, m_3_3) * DEGREE_FACTOR, 360.0);
		z_rot = fmod(atan2(m_2_1, m_1_1) * DEGREE_FACTOR, 360.0);
	}else{//Gimbal lock.
		if(y_rot > 0.0){
			y_rot = 1.0;
		}else{
			y_rot = -1.0;
		}
		x_rot = 0.0;
		z_rot = fmod(atan2(m_1_2, m_2_2) * DEGREE_FACTOR, 360.0);
	}
	y_rot = fmod(asin(-y_rot) * DEGREE_FACTOR, 360.0);
	
	if(x_rot < 0.0){
		x_rot += 360.0;
	}
	if(y_rot < 0.0){
		y_rot += 360.0;
	}
	if(z_rot < 0.0){
		z_rot += 360.0;
	}
	
	return Py_BuildValue(
	 "(fff)",
	 x_rot, y_rot, z_rot
	);
}

char matrix_c_multiply3x3Matrices__doc__[] =
 "Multiplies one 3x3 matrix by another 3x3 matrix and returns the resulting\n"
 "3x3 matrix. All matrices are represented as a 1D sequence of nine elements.\n"
 "\n"
 "@type m_1: sequence(9)\n"
 "@param m_1: The first matrix for the multiplication operation.\n"
 "@type m_2: sequence(9)\n"
 "@param m_2: The second matrix for the multiplication operation.\n"
 "\n"
 "@rtype: tuple(9)\n"
 "@return: The product of the matrix multiplication.\n";
static PyObject* matrix_c_multiply3x3Matrices(PyObject* self, PyObject* args){
	float m1_1_1, m1_1_2, m1_1_3, m1_2_1, m1_2_2, m1_2_3, m1_3_1, m1_3_2, m1_3_3;
	float m2_1_1, m2_1_2, m2_1_3, m2_2_1, m2_2_2, m2_2_3, m2_3_1, m2_3_2, m2_3_3;
	
	if (!PyArg_ParseTuple(args, "(fffffffff)(fffffffff):multiply3x3Matrices",
	 &m1_1_1, &m1_1_2, &m1_1_3,
	 &m1_2_1, &m1_2_2, &m1_2_3,
	 &m1_3_1, &m1_3_2, &m1_3_3,
	 &m2_1_1, &m2_1_2, &m2_1_3,
	 &m2_2_1, &m2_2_2, &m2_2_3,
	 &m2_3_1, &m2_3_2, &m2_3_3)){
		return NULL;
	}
	
	return Py_BuildValue(
	 "(fffffffff)",
	 m1_1_1 * m2_1_1 + m1_1_2 * m2_2_1 + m1_1_3 * m2_3_1,
	 m1_1_1 * m2_1_2 + m1_1_2 * m2_2_2 + m1_1_3 * m2_3_2,
	 m1_1_1 * m2_1_3 + m1_1_2 * m2_2_3 + m1_1_3 * m2_3_3,
	 
	 m1_2_1 * m2_1_1 + m1_2_2 * m2_2_1 + m1_2_3 * m2_3_1,
	 m1_2_1 * m2_1_2 + m1_2_2 * m2_2_2 + m1_2_3 * m2_3_2,
	 m1_2_1 * m2_1_3 + m1_2_2 * m2_2_3 + m1_2_3 * m2_3_3,
	 
	 m1_3_1 * m2_1_1 + m1_3_2 * m2_2_1 + m1_3_3 * m2_3_1,
	 m1_3_1 * m2_1_2 + m1_3_2 * m2_2_2 + m1_3_3 * m2_3_2,
	 m1_3_1 * m2_1_3 + m1_3_2 * m2_2_3 + m1_3_3 * m2_3_3
	);
}

char matrix_c_multiply4x4Matrices__doc__[] =
 "Multiplies one 4x4 matrix by another 4x4 matrix and returns the resulting\n"
 "4x4 matrix. All matrices are represented as a 1D sequence of twelve elements.\n"
 "\n"
 "@type m_1: sequence(12)\n"
 "@param m_1: The first matrix for the multiplication operation.\n"
 "@type m_2: sequence(12)\n"
 "@param m_2: The second matrix for the multiplication operation.\n"
 "\n"
 "@rtype: tuple(12)\n"
 "@return: The product of the matrix multiplication.\n";
static PyObject* matrix_c_multiply4x4Matrices(PyObject* self, PyObject* args){
	float m1_1_1, m1_1_2, m1_1_3, m1_1_4, m1_2_1, m1_2_2, m1_2_3, m1_2_4, m1_3_1, m1_3_2, m1_3_3, m1_3_4, m1_4_1, m1_4_2, m1_4_3, m1_4_4;
	float m2_1_1, m2_1_2, m2_1_3, m2_1_4, m2_2_1, m2_2_2, m2_2_3, m2_2_4, m2_3_1, m2_3_2, m2_3_3, m2_3_4, m2_4_1, m2_4_2, m2_4_3, m2_4_4;
	
	if (!PyArg_ParseTuple(args, "(ffffffffffffffff)(ffffffffffffffff):multiply4x4Matrices",
	 &m1_1_1, &m1_1_2, &m1_1_3, &m1_1_4,
	 &m1_2_1, &m1_2_2, &m1_2_3, &m1_2_4,
	 &m1_3_1, &m1_3_2, &m1_3_3, &m1_3_4,
	 &m1_4_1, &m1_4_2, &m1_4_3, &m1_4_4,
	 &m2_1_1, &m2_1_2, &m2_1_3, &m2_1_4,
	 &m2_2_1, &m2_2_2, &m2_2_3, &m2_2_4,
	 &m2_3_1, &m2_3_2, &m2_3_3, &m2_3_4,
	 &m2_4_1, &m2_4_2, &m2_4_3, &m2_4_4)){
		return NULL;
	}
	
	return Py_BuildValue(
	 "(ffffffffffffffff)",
	 m1_1_1 * m2_1_1 + m1_1_2 * m2_2_1 + m1_1_3 * m2_3_1 + m1_1_4 * m2_4_1,
	 m1_1_1 * m2_1_2 + m1_1_2 * m2_2_2 + m1_1_3 * m2_3_2 + m1_1_4 * m2_4_2,
	 m1_1_1 * m2_1_3 + m1_1_2 * m2_2_3 + m1_1_3 * m2_3_3 + m1_1_4 * m2_4_3,
	 m1_1_1 * m2_1_4 + m1_1_2 * m2_2_4 + m1_1_3 * m2_3_4 + m1_1_4 * m2_4_4,
	 
	 m1_2_1 * m2_1_1 + m1_2_2 * m2_2_1 + m1_2_3 * m2_3_1 + m1_2_4 * m2_4_1,
	 m1_2_1 * m2_1_2 + m1_2_2 * m2_2_2 + m1_2_3 * m2_3_2 + m1_2_4 * m2_4_2,
	 m1_2_1 * m2_1_3 + m1_2_2 * m2_2_3 + m1_2_3 * m2_3_3 + m1_2_4 * m2_4_3,
	 m1_2_1 * m2_1_4 + m1_2_2 * m2_2_4 + m1_2_3 * m2_3_4 + m1_2_4 * m2_4_4,
	 
	 m1_3_1 * m2_1_1 + m1_3_2 * m2_2_1 + m1_3_3 * m2_3_1 + m1_3_4 * m2_4_1,
	 m1_3_1 * m2_1_2 + m1_3_2 * m2_2_2 + m1_3_3 * m2_3_2 + m1_3_4 * m2_4_2,
	 m1_3_1 * m2_1_3 + m1_3_2 * m2_2_3 + m1_3_3 * m2_3_3 + m1_3_4 * m2_4_3,
	 m1_3_1 * m2_1_4 + m1_3_2 * m2_2_4 + m1_3_3 * m2_3_4 + m1_3_4 * m2_4_4,
	 
	 m1_4_1 * m2_1_1 + m1_4_2 * m2_2_1 + m1_4_3 * m2_3_1 + m1_4_4 * m2_4_1,
	 m1_4_1 * m2_1_2 + m1_4_2 * m2_2_2 + m1_4_3 * m2_3_2 + m1_4_4 * m2_4_2,
	 m1_4_1 * m2_1_3 + m1_4_2 * m2_2_3 + m1_4_3 * m2_3_3 + m1_4_4 * m2_4_3,
	 m1_4_1 * m2_1_4 + m1_4_2 * m2_2_4 + m1_4_3 * m2_3_4 + m1_4_4 * m2_4_4
	);
}

static PyMethodDef functions[] = {
 {"buildIdentityRenderMatrix",
  (PyCFunction)matrix_c_buildIdentityRenderMatrix,
  METH_VARARGS, matrix_c_buildIdentityRenderMatrix__doc__},
 {"buildRenderMatrix",
  (PyCFunction)matrix_c_buildRenderMatrix,
  METH_VARARGS, matrix_c_buildRenderMatrix__doc__},
 {"buildRotationMatrix",
  (PyCFunction)matrix_c_buildRotationMatrix,
  METH_VARARGS, matrix_c_buildRotationMatrix__doc__},
 {"computePosition",
  (PyCFunction)matrix_c_computePosition,
  METH_VARARGS, matrix_c_computePosition__doc__},
 {"decomposeRotationMatrix",
  (PyCFunction)matrix_c_decomposeRotationMatrix,
  METH_VARARGS, matrix_c_decomposeRotationMatrix__doc__},
 {"multiply3x3Matrices",
  (PyCFunction)matrix_c_multiply3x3Matrices,
  METH_VARARGS, matrix_c_multiply3x3Matrices__doc__},
 {"multiply4x4Matrices",
  (PyCFunction)matrix_c_multiply4x4Matrices,
  METH_VARARGS, matrix_c_multiply4x4Matrices__doc__},
 {NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC initmatrix_c(void){
 Py_InitModule("matrix_c", functions);
}
