/*
 * $Id$
 *
 * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version. The Blender
 * Foundation also sells licenses for use in proprietary software under
 * the Blender License.  See http://www.blender.org/BL/ for information
 * about this.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
 * All rights reserved.
 *
 * 
 * Contributor(s): Joseph Gilbert
 *
 * ***** END GPL/BL DUAL LICENSE BLOCK *****
 */

#include "euler.h"

//doc strings
char Euler_Zero_doc[] = "() - set all values in the euler to 0";
char Euler_Unique_doc[] =
	"() - sets the euler rotation a unique shortest arc rotation - tests for gimbal lock";
char Euler_ToMatrix_doc[] =
	"() - returns a rotation matrix representing the euler rotation";
char Euler_ToQuat_doc[] =
	"() - returns a quaternion representing the euler rotation";

//methods table
struct PyMethodDef Euler_methods[] = {
	{"zero", ( PyCFunction ) Euler_Zero, METH_NOARGS,
	 Euler_Zero_doc},
	{"unique", ( PyCFunction ) Euler_Unique, METH_NOARGS,
	 Euler_Unique_doc},
	{"toMatrix", ( PyCFunction ) Euler_ToMatrix, METH_NOARGS,
	 Euler_ToMatrix_doc},
	{"toQuat", ( PyCFunction ) Euler_ToQuat, METH_NOARGS,
	 Euler_ToQuat_doc},
	{NULL, NULL, 0, NULL}
};

/*****************************/
//    Euler Python Object   
/*****************************/

//euler methods
PyObject *Euler_ToQuat( EulerObject * self )
{
	float *quat;
	int x;

	for( x = 0; x < 3; x++ ) {
		self->eul[x] *= ( float ) ( Py_PI / 180 );
	}
	quat = PyMem_Malloc( 4 * sizeof( float ) );
	EulToQuat( self->eul, quat );
	for( x = 0; x < 3; x++ ) {
		self->eul[x] *= ( float ) ( 180 / Py_PI );
	}
	return ( PyObject * ) newQuaternionObject( quat );
}

PyObject *Euler_ToMatrix( EulerObject * self )
{
	float *mat;
	int x;

	for( x = 0; x < 3; x++ ) {
		self->eul[x] *= ( float ) ( Py_PI / 180 );
	}
	mat = PyMem_Malloc( 3 * 3 * sizeof( float ) );
	EulToMat3( self->eul, ( float ( * )[3] ) mat );
	for( x = 0; x < 3; x++ ) {
		self->eul[x] *= ( float ) ( 180 / Py_PI );
	}
	return ( PyObject * ) newMatrixObject( mat, 3, 3 );
}

PyObject *Euler_Unique( EulerObject * self )
{
	float heading, pitch, bank;
	float pi2 = ( float ) Py_PI * 2.0f;
	float piO2 = ( float ) Py_PI / 2.0f;
	float Opi2 = 1.0f / pi2;

	//radians
	heading = self->eul[0] * ( float ) ( Py_PI / 180 );
	pitch = self->eul[1] * ( float ) ( Py_PI / 180 );
	bank = self->eul[2] * ( float ) ( Py_PI / 180 );

	//wrap heading in +180 / -180
	pitch += ( float ) Py_PI;
	pitch -= ( float ) floor( pitch * Opi2 ) * pi2;
	pitch -= ( float ) Py_PI;


	if( pitch < -piO2 ) {
		pitch = ( float ) -Py_PI - pitch;
		heading += ( float ) Py_PI;
		bank += ( float ) Py_PI;
	} else if( pitch > piO2 ) {
		pitch = ( float ) Py_PI - pitch;
		heading += ( float ) Py_PI;
		bank += ( float ) Py_PI;
	}
	//gimbal lock test
	if( fabs( pitch ) > piO2 - 1e-4 ) {
		heading += bank;
		bank = 0.0f;
	} else {
		bank += ( float ) Py_PI;
		bank -= ( float ) ( floor( bank * Opi2 ) ) * pi2;
		bank -= ( float ) Py_PI;
	}

	heading += ( float ) Py_PI;
	heading -= ( float ) ( floor( heading * Opi2 ) ) * pi2;
	heading -= ( float ) Py_PI;

	//back to degrees
	self->eul[0] = heading * ( float ) ( 180 / Py_PI );
	self->eul[1] = pitch * ( float ) ( 180 / Py_PI );
	self->eul[2] = bank * ( float ) ( 180 / Py_PI );

	return EXPP_incr_ret( Py_None );
}

PyObject *Euler_Zero( EulerObject * self )
{
	self->eul[0] = 0.0;
	self->eul[1] = 0.0;
	self->eul[2] = 0.0;

	return EXPP_incr_ret( Py_None );
}

static void Euler_dealloc( EulerObject * self )
{
	/* since we own this memory... */
	PyMem_Free( self->eul );

	PyObject_DEL( self );
}

static PyObject *Euler_getattr( EulerObject * self, char *name )
{
	if( ELEM3( name[0], 'x', 'y', 'z' ) && name[1] == 0 ) {
		return PyFloat_FromDouble( self->eul[name[0] - 'x'] );
	}
	return Py_FindMethod( Euler_methods, ( PyObject * ) self, name );
}

static int Euler_setattr( EulerObject * self, char *name, PyObject * e )
{
	float val;

	if( !PyArg_Parse( e, "f", &val ) )
		return EXPP_ReturnIntError( PyExc_TypeError,
					    "unable to parse float argument\n" );

	if( ELEM3( name[0], 'x', 'y', 'z' ) && name[1] == 0 ) {
		self->eul[name[0] - 'x'] = val;
		return 0;
	} else
		return -1;
}

/* Eulers Sequence methods */
static PyObject *Euler_item( EulerObject * self, int i )
{
	if( i < 0 || i >= 3 )
		return EXPP_ReturnPyObjError( PyExc_IndexError,
					      "array index out of range\n" );

	return Py_BuildValue( "f", self->eul[i] );
}

static PyObject *Euler_slice( EulerObject * self, int begin, int end )
{
	PyObject *list;
	int count;

	if( begin < 0 )
		begin = 0;
	if( end > 3 )
		end = 3;
	if( begin > end )
		begin = end;

	list = PyList_New( end - begin );

	for( count = begin; count < end; count++ ) {
		PyList_SetItem( list, count - begin,
				PyFloat_FromDouble( self->eul[count] ) );
	}
	return list;
}

static int Euler_ass_item( EulerObject * self, int i, PyObject * ob )
{
	if( i < 0 || i >= 3 )
		return EXPP_ReturnIntError( PyExc_IndexError,
					    "array assignment index out of range\n" );

	if( !PyNumber_Check( ob ) )
		return EXPP_ReturnIntError( PyExc_IndexError,
					    "Euler member must be a number\n" );

	if( !PyFloat_Check( ob ) && !PyInt_Check( ob ) ) {
		return EXPP_ReturnIntError( PyExc_TypeError,
					    "int or float expected\n" );
	} else {
		self->eul[i] = ( float ) PyFloat_AsDouble( ob );
	}
	return 0;
}

static int Euler_ass_slice( EulerObject * self, int begin, int end,
			    PyObject * seq )
{
	int count, z;

	if( begin < 0 )
		begin = 0;
	if( end > 3 )
		end = 3;
	if( begin > end )
		begin = end;

	if( !PySequence_Check( seq ) )
		return EXPP_ReturnIntError( PyExc_TypeError,
					    "illegal argument type for built-in operation\n" );
	if( PySequence_Length( seq ) != ( end - begin ) )
		return EXPP_ReturnIntError( PyExc_TypeError,
					    "size mismatch in slice assignment\n" );

	z = 0;
	for( count = begin; count < end; count++ ) {
		PyObject *ob = PySequence_GetItem( seq, z );
		z++;

		if( !PyFloat_Check( ob ) && !PyInt_Check( ob ) ) {
			Py_DECREF( ob );
			return -1;
		} else {
			if( !PyArg_Parse( ob, "f", &self->eul[count] ) ) {
				Py_DECREF( ob );
				return -1;
			}
		}
	}
	return 0;
}

static PyObject *Euler_repr( EulerObject * self )
{
	int i, maxindex = 3 - 1;
	char ftoa[24];
	PyObject *str1, *str2;

	str1 = PyString_FromString( "[" );

	for( i = 0; i < maxindex; i++ ) {
		sprintf( ftoa, "%.4f, ", self->eul[i] );
		str2 = PyString_FromString( ftoa );
		if( !str1 || !str2 )
			goto error;
		PyString_ConcatAndDel( &str1, str2 );
	}

	sprintf( ftoa, "%.4f]\n", self->eul[maxindex] );
	str2 = PyString_FromString( ftoa );
	if( !str1 || !str2 )
		goto error;
	PyString_ConcatAndDel( &str1, str2 );

	if( str1 )
		return str1;

      error:
	Py_XDECREF( str1 );
	Py_XDECREF( str2 );
	return EXPP_ReturnPyObjError( PyExc_MemoryError,
				      "couldn't create PyString!\n" );
}

static PySequenceMethods Euler_SeqMethods = {
	( inquiry ) 0,		/* sq_length */
	( binaryfunc ) 0,	/* sq_concat */
	( intargfunc ) 0,	/* sq_repeat */
	( intargfunc ) Euler_item,	/* sq_item */
	( intintargfunc ) Euler_slice,	/* sq_slice */
	( intobjargproc ) Euler_ass_item,	/* sq_ass_item */
	( intintobjargproc ) Euler_ass_slice,	/* sq_ass_slice */
};

PyTypeObject euler_Type = {
	PyObject_HEAD_INIT( NULL ) 
	0,	/*ob_size */
	"euler",		/*tp_name */
	sizeof( EulerObject ),	/*tp_basicsize */
	0,			/*tp_itemsize */
	( destructor ) Euler_dealloc,	/*tp_dealloc */
	( printfunc ) 0,	/*tp_print */
	( getattrfunc ) Euler_getattr,	/*tp_getattr */
	( setattrfunc ) Euler_setattr,	/*tp_setattr */
	0,			/*tp_compare */
	( reprfunc ) Euler_repr,	/*tp_repr */
	0,			/*tp_as_number */
	&Euler_SeqMethods,	/*tp_as_sequence */
};

PyObject *newEulerObject( float *eul )
{
	EulerObject *self;
	int x;

	euler_Type.ob_type = &PyType_Type;

	self = PyObject_NEW( EulerObject, &euler_Type );

	/* 
	   we own the self->eul memory and will free it later.
	   if we received an input arg, copy to our internal array
	*/

	self->eul = PyMem_Malloc( 3 * sizeof( float ) );
	if( ! self->eul )
		return EXPP_ReturnPyObjError( PyExc_MemoryError,
					      "newEulerObject:PyMem_Malloc failed" );
	
	if( !eul ) {
		for( x = 0; x < 3; x++ ) {
			self->eul[x] = 0.0f;
		}
	} else{
		for( x = 0; x < 3; x++){
			self->eul[x] = eul[x];
		}
	}

	return ( PyObject * ) self;
}