// __author__ = Juan Carlos De Abreu (jcabreur@gmail.com)

#ifndef MATH_QUATERNION_INL_H__
#define MATH_QUATERNION_INL_H__

#include "Quaternion.h"

BEGIN_TEG_NAMESPACE();

namespace math {

// Operator de casting entre dos quaternions
template<class _T>
void Quaternion<_T>::operator = (const Quaternion<_T>& q) {
	memcpy(values, q.values, sizeof(values));
}

// Operator producto entre dos quaternions
template<class _T>
Quaternion<_T> Quaternion<_T>::operator * (const Quaternion<_T>& q) {
	return Quaternion(w * q.x + x * q.w + y * q.z - z * q.y,
		w * q.y - x * q.z + y * q.w + z * q.x,
		w * q.z + x * q.y - y * q.x + z * q.w,
		w * q.w - x * q.x - y * q.y - z * q.z);
}
/*
//operador suma entre dos quaternions
template<class _T>
Quaternion<_T> Quaternion<_T>::operator + (const Quaternion<_T>& q) {
	Quaternion result;
	
	Vec4f v1, v2, crossV, v3;
	v1.set(x, y, z); 
	v2.set(q.x, q.y, q.z);

	crossV = cross(v2,v1);

	v1 *= q.w;
	v2 *= w;

	v3 = v1 + v2;
	v3 = crossV + v3;

	v3.w = w * q.w - (x * q.x + y * q.y + z * q.z);

	result.x = v3.x; result.y = v3.y; result.z = v3.z; result.w = v3.w;

	return result;
}
*/
//funcion para crear el quaternion a partir de los ejes
template<class _T>
Quaternion<_T> createQuatFromAxis(Vec4<_T> a, const double& radians) {
	a.normalize();
	const double radiansDiv2 = radians * 0.5;
	const double sine = sin(radians);

	return Quaternion<_T>(
			static_cast<_T>(a.x * sine),
			static_cast<_T>(a.y * sine),
			static_cast<_T>(a.z * sine),
			static_cast<_T>(cos(radians)));
}

//funcion para rotar el quaternion con los angulos en radianes
template<class _T>
Quaternion<_T> rotationRadiansf(const double& x_, const double& y_, const double& z_) {
	const double sinX = sin(x_ * 0.5);
	const double cosX = cos(x_ * 0.5);
	
	const double sinY = sin(y_ * 0.5);
	const double cosY = cos(y_ * 0.5);
	
	const double sinZ = sin(z_ * 0.5);
	const double cosZ = cos(z_ * 0.5);

	const double cosXY = cosX * cosY;
	const double sinXY = sinX * sinY;

	return Quaternion<_T>(
		static_cast<float>(sinX * cosY * cosZ - cosX * sinY * sinZ),
		y = static_cast<float>(cosX * sinY * cosZ + sinX * cosY * sinZ),
		z = static_cast<float>(cosXY * sinZ - sinXY * cosZ),
		w = static_cast<float>(cosXY * cosZ + sinXY * sinZ)); 
}

//funcion para obtener la matrix de rotacion asociada al quaternion
/*template<class _T>
void Quaternion<_T>::createMatrix(Mat4<_T> &m) {
	// Se calcula la primera fila.
	m[0]  = 1.0f - 2.0f * (y * y + z * z); 
	m[1]  = 2.0f * (x * y + z * w);
	m[2]  = 2.0f * (x * z - y * w);
	m[3]  = 0.0f;  

	// Se calcula la segunda fila
	m[4]  = 2.0f * (x * y - z * w);  
	m[5]  = 1.0f - 2.0f * (x * x + z * z); 
	m[6]  = 2.0f * (z * y + x * w);  
	m[7]  = 0.0f;  

	// Se calcula la tercera fila
	m[8]  = 2.0f * (x * z + y * w);
	m[9]  = 2.0f * (y * z - x * w);
	m[10] = 1.0f - 2.0f * (x * x + y * y);  
	m[11] = 0.0f;  

	// Se ajustan los valores de la cuarta columna
	m[12] = 0;  
	m[13] = 0;  
	m[14] = 0;  
	m[15] = 1.0f;
}
*/

template<class _T>
Mat4<_T> Quaternion<_T>::createMatrix() const {
return Mat4<_T> (
    1 - 2 * (y * y + z * z), 2 * (x * y + z * w), 2 * (x * z - y * w), 0,
    2 * (x * y - z * w), 1 - 2 * (x * x + z * z), 2 * (z * y + x * w), 0,
    2 * (x * z + y * w), 2 * (y * z - x * w), 1 - 2 * (x * x + y * y), 0,
    0, 0, 0, 1);
}

template<class _T>
void Quaternion<_T>::set(const _T& x_, const _T& y_, const _T& z_, const _T& w_) {
	x = x_; y = y_; z = z_; w = w_;
}

// Method para obtener la conjugada del quaternion (es la misma inversa)
template<class _T>
inline Quaternion<_T> Quaternion<_T>::conjugate() {
	return Quaternion<_T>(-x, -y, -z, w);
}

// Method para obtener la magnitid del quaternion
template<class _T>
_T Quaternion<_T>::module() {
	return static_cast<_T>(sqrt(x * x + y * y + z * z + w * w));
}

// Method para normalizar el quaternion
template<class _T>
void Quaternion<_T>::normalize() {
	const _T mod = 1.0f / module();
	x *= mod; y *= mod; z *= mod; w *= mod;
}

// Method ...
template<class _T>
Quaternion<_T> cross(const Quaternion<_T>& q1, const Quaternion<_T>& q2) {
	return Quaternion(q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y,
		q1.w*q2.y + q1.x*q2.z + q1.y*q2.w - q1.z*q2.x,
		q1.w*q2.z + q1.x*q2.y + q1.y*q2.x - q1.z*q2.w,
		q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z);
}

//funcion para rotar el quaternion sobre un eje
template<class _T>
Quaternion<_T> rotate(float amount, Vec3<_T> axis) {
	// Se normaliza ...
	axis.normalize();

	// se convierte el angulo de grados a radianes
	// Se calcula el seno una sola vez para optimizacion
	const _T angle = static_cast<_T> (GET_RADIANS(amount) * 0.5);
	const double sine = sin(angle);

	// Se crea el quaternion
	return Quaternion<_T>(
		static_cast<_T>(axis.x * sine),
		static_cast<_T>(axis.y * sine),
		static_cast<_T>(axis.z * sine),
		static_cast<_T>(cos(angle)));
}

//funcion para obtener la interpolacion lineal entre dos quaternions
template<class _T>
Quaternion<_T> slerp(const Quaternion<_T>& q1, const Quaternion<_T>& q2, float t) {
	Quaternion<_T> tmp = q2;
	float sinTheta = 0.0f;	
	float cosTheta = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;

	if (cosTheta < 0.0f) {
		// Flip sigh if so.
		q2.set(-tmp.x, -tmp.y, -tmp.z, -tmp.w);
		cosTheta = -cosTheta;
	}

	float beta = 1.0f - t;

	if (1.0f - cosTheta > 0.001f) {
		// We are using spherical Interpolation
		cosTheta = (float)acos(cosTheta);
		sinTheta = 1.0f / (float)sin(cosTheta);
		beta = (float)sin(cosTheta * beta) * sinTheta;
		t = (float)sin(cosTheta * t) * sinTheta;
	}

	// Interpolation
	return Quaternion<_T>(beta * q1.x + t * tmp.x,
		beta * q1.y + t * tmp.y,
		beta * q1.z + t * tmp.z,
		beta * q1.w + t * tmp.w);
}

};	// Math namespace

END_TEG_NAMESPACE();

#endif  // MATH_QUATERNION_INL_H__
