/*
 *  Copyright (c) 2004-2013 Gert Wollny <gw.fossdev@gmail.com>
 *
 *  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 3 of the License, or
 *  (at your option) any later version.
 *
 *  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.
 *
 *  As an exception to this license, "NEC C&C Research Labs" may use
 *  this software under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation.
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <math.h>

#include <mialm/miaquaternion.h>

/**
 * SECTION:miaquaternion
 * @Short_description: Quaternions to describe rotations in 3D space 
 * @Title: Quaternions
 * 
 * #MiaQuaternion and the related functions describe and handle rotations in 3D space that 
 * are specifically suited for visualization.
 *
 */


#ifndef NDEBUG
static int n_quats = 0; 
void print_quats_left(void)
{
	if (n_quats)
		g_warning("left %d quaternions behind", n_quats);
}
#endif

static void
__mia_quaternion_class_init (gpointer g_class, gpointer g_class_data);

static void
__mia_quaternion_instance_init (GTypeInstance * instance, gpointer g_class);

GType
mia_quaternion_get_type (void)
{
	static GType type = 0;
	if (type == 0)
	{
		static const GTypeInfo info = {
			sizeof (MiaQuaternionClass),
			NULL,	/* base_init */
			NULL,	/* base_finalize */
			__mia_quaternion_class_init,	/* class_init */
			NULL,	/* class_finalize */
			NULL,	/* class_data */
			sizeof (MiaQuaternion),
			0,	/* n_preallocs */
			__mia_quaternion_instance_init, 	/* instance_init */
			NULL /* value_table */
		};
		type = g_type_register_static (MIA_TYPE_VECTOR3D,
					       "MiaQuaternionType", &info, 0);
	}
	return type;
}


/**
 * mia_quaternion_new_from_eulerrot:
 * @axis:  the rotation axis 
 * @angle: the rotation angle 
 * 
 *  Create a quaternion that represents a rotation around the given axis 
 *  by the given angle. The returned quaternion must be destroyed by 
 *  calling g_object_unref()
 * 
 * Returns: a newly created quaternion
 */
MiaQuaternion *
mia_quaternion_new_from_eulerrot (const MiaVector3d * axis, gfloat angle)
{
	MiaQuaternion *obj =
		(MiaQuaternion *) g_object_new (MIA_TYPE_QUATERNION, NULL);
	MiaVector3d *a = MIA_VECTOR3D (obj);
	angle *= 0.5f;
	mia_vector3d_copy (a, axis);
	mia_vector3d_normalize (a);
	mia_vector3d_scale (a, sin (angle));
	obj->r = cos (angle);
	return obj;
}


/**
 * mia_quaternion_new:
 * @x: x-component of rotation axis 
 * @y: y-component of rotation axis 
 * @z: z-component of rotation axis 
 * @w: scalar part of the quaternion 
 *
 * Create a quaternion q = w + x *i + y * j + z * k.
 *
 * Returns: a newly created quaternion representing the rotation 
 */
MiaQuaternion *
mia_quaternion_new (gfloat x, gfloat y, gfloat z, gfloat w)
{
	MiaQuaternion *obj =
		(MiaQuaternion *) g_object_new (MIA_TYPE_QUATERNION, NULL);
	/* add your code here */
	MiaVector3d *v = MIA_VECTOR3D (obj);

	v->x = x;
	v->y = y;
	v->z = z;

	obj->r = w;

	return obj;
}

/**
 * mia_quaternion_equal: 
 * @a:  left hand value for comparison 
 * @b:  right hand value for comparison 
 *
 * Test if two quaternions are exactly equal. 
 *  
 * Returns: TRUE if equal, FALSE otherwise 
 */
gboolean
mia_quaternion_equal (const MiaQuaternion * a, const MiaQuaternion * b)
{
	return mia_vector3d_equal (MIA_VECTOR3D (a), MIA_VECTOR3D (b)) &&
		a->r == b->r;
}

static inline gfloat
_sqr (gfloat x)
{
	return x * x;
}

/**
 * mia_quaternion_norm: 
 * @self: the quaternion  
 *
 * Evaluate the quaternions norm.  
 *  
 * Returns: the Euclidian norm 
 */
gfloat
mia_quaternion_norm (const MiaQuaternion * self)
{
	gfloat norm;
	MiaVector3d *v;
	MIA_IS_QUATERNION (self);
	v = MIA_VECTOR3D (self);

	norm = _sqr (v->x) + _sqr (v->y) + _sqr (v->z) + _sqr (self->r);
	return norm > 0.0f ? sqrt (norm) : 0.0f;
}

/**
 * mia_quaternion_scale: 
 *
 * Multipy a quaternion by a given scalar in place self *= f 
 *  
 * @self: the quaternion 
 * @f: the scaler 
 */
void
mia_quaternion_scale (MiaQuaternion * self, gfloat f)
{
	MIA_IS_QUATERNION (self);
	mia_vector3d_scale (MIA_VECTOR3D (self), f);
	self->r *= f;
}

/**
 * mia_quaternion_normalize: 
 * @self: the quaternion to be normalized 
 *
 * Normalize a quaternion if it's norm is not zero.
 *  
 */
void
mia_quaternion_normalize (MiaQuaternion * self)
{
	gfloat norm = mia_quaternion_norm (self);
	if (norm > 0)
		mia_quaternion_scale (self, 1.0 / norm);
}

/**
 * mia_quaternion_copy: 
 * @dest: destination quaternion to copy to 
 * @src: source quaternion to copy from 
 *
 * Copy one quaternion into another one. The destination quaternion 
 * must already be properly created.
 *
 * Returns: the destination quaternion
 */
MiaQuaternion *
mia_quaternion_copy (MiaQuaternion * dest, const MiaQuaternion * src)
{
	mia_vector3d_copy (MIA_VECTOR3D (dest), MIA_VECTOR3D (src));
	dest->r = src->r;
	return dest;
}

/**
 * mia_quaternion_dup: 
 * @org: a quaterion 
 *
 * Create a duplicate of a quaternion.  It must be destroyed by calling 
 * calling g_object_unref()
 *
 * Returns: a newly created copy of the quaternion. 
 */
MiaQuaternion *
mia_quaternion_dup (const MiaQuaternion * org)
{
	MiaQuaternion *obj =
		(MiaQuaternion *) g_object_new (MIA_TYPE_QUATERNION, NULL);
	return mia_quaternion_copy (obj, org);;
}

/**
 * mia_quaternion_addup: 
 *
 * Add quaternion other to quaternion dest as in dest += other
 *
 * @dest: destination quaternion to add to 
 * @src: source quaternion 
 * Returns: the destination quaternion
 */
MiaQuaternion *
mia_quaternion_addup (MiaQuaternion * self, const MiaQuaternion * other)
{
	g_assert (MIA_IS_QUATERNION (self));
	g_assert (MIA_IS_QUATERNION (other));

	mia_vector3d_addup (MIA_VECTOR3D (self), MIA_VECTOR3D (other));
	self->r += other->r;
	return self;
}

/**
 * mia_quaternion_add: 
 * @self: destination quaternion to add to 
 * @other: source quaternion 
 * @result: if not NULL, then the result will we stored here 
 *
 * Add two quaternions self and other. If the result quaternion is already 
 * created, than the result will be stored in it, otherwise the result 
 * quaternion will be newly created and must be destroyed by calling 
 * g_object_unref()
 * 
 * Returns: the quaternion resulting from the addition of self and other, 
    either a new quaternion (if result is NULL) or result.
 */
MiaQuaternion *
mia_quaternion_add (const MiaQuaternion * self,
		    const MiaQuaternion * other, MiaQuaternion * result)
{
	g_assert (MIA_IS_QUATERNION (self));
	g_assert (MIA_IS_QUATERNION (other));

	if (!result)
	{
		result = mia_quaternion_dup (self);
	}
	else
	{
		g_assert (MIA_IS_QUATERNION (result));
		mia_quaternion_copy (result, self);
	}
	return mia_quaternion_addup (result, other);
}

/**
 * mia_quaternion_multby: 
 * @self: destination quaternion 
 * @other: other quaternion to multiply by 
 *
 * Multiply the quaternion dest by a quaternion other as in dest *= other
 *
 * Returns: the quaternion self
 */
MiaQuaternion *
mia_quaternion_multby (MiaQuaternion * self, MiaQuaternion * other)
{
	gfloat x, y, z, r;
	MiaVector3d *selfv;
	MiaVector3d *otherv;

	g_assert (MIA_IS_QUATERNION (self));
	g_assert (MIA_IS_QUATERNION (other));

	selfv = MIA_VECTOR3D (self);
	otherv = MIA_VECTOR3D (other);

	r = self->r * other->r - mia_vector3d_dot (selfv, otherv);

	x = selfv->x * other->r + self->r * otherv->x +
		selfv->y * otherv->z - selfv->z * otherv->y;

	y = self->r * otherv->y + selfv->y * other->r -
		selfv->x * otherv->z + selfv->z * otherv->y;

	z = self->r * otherv->z + selfv->z * other->r +
		selfv->x * otherv->y - selfv->y * otherv->x;

	mia_vector3d_set (selfv, x, y, z);

	self->r = r;

	return self;
}

/**
 * mia_quaternion_get_rotation: 
 * @self: the quaternion desctibing the rotation  
 * @m: The result will be stored in this matrix. 
 *
 * Evaluate the 4x4 Matrix corresponding to the 3D rotation. 
 *
 */
void
mia_quaternion_get_rotation (MiaQuaternion * self, RotMatrix m)
{
	MiaVector3d *v;
	g_assert (MIA_IS_QUATERNION (self));
	v = MIA_VECTOR3D(self);

	m[0][0] = 1.0 - 2.0 * (v->y * v->y + v->z * v->z);
	m[0][1] = 2.0 * (v->x * v->y - v->z * self->r);
	m[0][2] = 2.0 * (v->z * v->x + v->y * self->r);
	m[0][3] = 0.0;

	m[1][0] = 2.0 * (v->x * v->y + v->z * self->r);
	m[1][1] = 1.0 - 2.0 * (v->z * v->z + v->x * v->x);
	m[1][2] = 2.0 * (v->y * v->z - v->x * self->r);
	m[1][3] = 0.0;

	m[2][0] = 2.0 * (v->z * v->x - v->y * self->r);
	m[2][1] = 2.0 * (v->y * v->z + v->x * self->r);
	m[2][2] = 1.0 - 2.0 * (v->y * v->y + v->x * v->x);
	m[2][3] = 0.0;

	m[3][0] = 0.0;
	m[3][1] = 0.0;
	m[3][2] = 0.0;
	m[3][3] = 1.0;
}

/**
 * mia_quaternion_trackball: 
 *
 * This function is not yet implemented 
 */
MiaQuaternion *
mia_quaternion_trackball (MiaQuaternion * self, float x1, float y1,
			  float x2, float y2, float UNUSED(r))
{
	g_assert (MIA_IS_QUATERNION (self));
	if (x1 == x2 && y1 == y2)
	{
		mia_vector3d_set (MIA_VECTOR3D (self), 0.0f, 0.0f, 0.0f);
		self->r = 1.0;
		return self;
	}
	g_warning ("mia_quaternion_trackball not yet implemented");
	return NULL; 
}

enum
{
	mia_quaternion_prop_r = 1
};

static void
__mia_quaternion_set_property (GObject * object,
			       guint property_id,
			       const GValue * value, GParamSpec * pspec)
{
	MiaQuaternion *self = (MiaQuaternion *) object;
	switch (property_id)
	{
		/* handle your properties here */
	case mia_quaternion_prop_r:
		self->r = g_value_get_float (value);
		break;
	default:
		/* We don't have any other property... */
		G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id,
						   pspec);
	}
}

static void
__mia_quaternion_get_property (GObject * object,
			       guint property_id,
			       GValue * value, GParamSpec * pspec)
{
	MiaQuaternion *self = (MiaQuaternion *) object;

	switch (property_id)
	{
		/* handle your properties here */
	case mia_quaternion_prop_r:
		g_value_set_float (value, self->r);
		break;
	default:
		/* We don't have any other property... */
		G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id,
						   pspec);
	}
}

static void
__mia_quaternion_instance_init (GTypeInstance * UNUSED(instance), gpointer UNUSED(g_class))
{
#ifndef NDEBUG
     n_quats++;
#endif	
}

#ifndef NDEBUG
static void
__mia_quaternion_instance_finalize (GObject *obj)
{
	MiaQuaternionClass *klass;
        GObjectClass *parent_class;
	n_quats--;
	
	klass = MIA_QUATERNION_CLASS(g_type_class_peek
					    (MIA_TYPE_QUATERNION));
	parent_class =	G_OBJECT_CLASS(g_type_class_peek_parent (klass));
	parent_class->finalize(obj); 
}	
#endif	

static void
__mia_quaternion_class_init (gpointer g_class, gpointer UNUSED(g_class_data))
{
	GParamSpec *pspec;
	GObjectClass *gobject_class = (GObjectClass *) (g_class);

	gobject_class->set_property = __mia_quaternion_set_property;
	gobject_class->get_property = __mia_quaternion_get_property;
	
#ifndef NDEBUG	
	gobject_class->finalize = __mia_quaternion_instance_finalize;
#endif	

	/* add your code here (e.g. define properties) */
	pspec = g_param_spec_float ("r", "r", "r value", -HUGE_VAL, HUGE_VAL, 0.0f,
				    G_PARAM_READWRITE);
	g_object_class_install_property (gobject_class,
					 mia_quaternion_prop_r, pspec);

}


/**
 * mia_quaternion_xmlio_read: 
 * @state: XML parser state holding the parent object and the values 
 * @property: property name the quaternion has in the parent class
 *
 * Helper function to read a quaternion from a XML file 
 *
 */
void
mia_quaternion_xmlio_read (ParserState * state, const gchar * property)
{
	gfloat x, y, z, r;
	GValue value = G_VALUE_INIT;
	MiaQuaternion *q;

	GObject *obj = G_OBJECT (state->pss->parent->data);
	g_assert (obj);

	g_value_init (&value, G_TYPE_POINTER);
	sscanf (state->pss->ch->str, "%g %g %g %g", &x, &y, &z, &r);

	q = mia_quaternion_new (x, y, z, r);
	g_value_set_pointer (&value, (gpointer) q);
	g_object_set_property (obj, property, &value);
}

/**
 * mia_quaternion_xmlio_write: 
 * @root: xml node this quaternion will be attached to 
 * @ns: name space 
 * @tag: the xml tag to store this quaternion under 
 * @q: the quaternion 
 *
 * Helper function to write a quaternion to a XML file 
 *
 * Returns: true if the node creation was successfu, otherwise false. 
 */
gboolean
mia_quaternion_xmlio_write (xmlNodePtr root, xmlNsPtr ns, const gchar * tag,
			    const MiaQuaternion * q)
{
	gchar text[1024];
	const MiaVector3d *v = MIA_VECTOR3D (q);
	g_return_val_if_fail (q, TRUE);
	snprintf (text, 1023, "%g %g %g %g", v->x, v->y, v->z, q->r);
	return (xmlNewTextChild (root, ns, (const xmlChar *)tag, (const xmlChar *)text) != NULL);
}


/**
 * mia_quaternion_test: 
 * 
 * Test the quaternion implementation
 *
 * Returns: %TRUE if all tests pass, %FALSE otherwise. 
 */

gboolean
mia_quaternion_test (void)
{
	MiaQuaternion *rx, *ry, *rz, *help;
	rx = mia_quaternion_new (1.0, 0.0, 0.0, 1.0);
	ry = mia_quaternion_new (0.0, 1.0, 0.0, 2.0);
	rz = mia_quaternion_new (0.0, 0.0, 1.0, -1.0);

	g_assert (MIA_IS_QUATERNION (rx));
	g_assert (MIA_IS_QUATERNION (ry));
	g_assert (MIA_IS_QUATERNION (rz));

	help = mia_quaternion_dup (rx);
	g_assert (MIA_IS_QUATERNION (help));

	g_object_unref (G_OBJECT (rz));
	g_object_unref (G_OBJECT (ry));
	g_object_unref (G_OBJECT (rx));
	return TRUE;
}

