#include "rtsMatrix4x4.h"

#ifndef RTS_QUATERNION_H
#define RTS_QUATERNION_H

template<typename T>
class rtsQuaternion
{
public:
	T w;
	T x;
	T y;
	T z;

	void normalize();
	void CreateRotation(T theta, T axis_x, T axis_y, T axis_z);
	rtsQuaternion<T> operator*(rtsQuaternion<T> &rhs);
	matrix4x4<T> toMatrix();
	

	rtsQuaternion();
	rtsQuaternion(T w, T x, T y, T z);

};

template<typename T>
void rtsQuaternion<T>::normalize()
{
	double length=sqrt(w*w + x*x + y*y + z*z);
	w=w/length;
	x=x/length;
	y=y/length;
	z=z/length;
}

template<typename T>
void rtsQuaternion<T>::CreateRotation(T theta, T axis_x, T axis_y, T axis_z)
{
	//assign the given Euler rotation to this quaternion
	w = cos(theta/2.0);
	x = axis_x*sin(theta/2.0);
	y = axis_y*sin(theta/2.0);
	z = axis_z*sin(theta/2.0);


}

template<typename T>
rtsQuaternion<T> rtsQuaternion<T>::operator *(rtsQuaternion<T> &param)
{
	float A, B, C, D, E, F, G, H;


	A = (w + x)*(param.w + param.x);
	B = (z - y)*(param.y - param.z);
	C = (w - x)*(param.y + param.z); 
	D = (y + z)*(param.w - param.x);
	E = (x + z)*(param.x + param.y);
	F = (x - z)*(param.x - param.y);
	G = (w + y)*(param.w - param.z);
	H = (w - y)*(param.w + param.z);

	rtsQuaternion<T> result;
	result.w = B + (-E - F + G + H) /2;
	result.x = A - (E + F + G + H)/2; 
	result.y = C + (E - F + G - H)/2; 
	result.z = D + (E - F - G + H)/2;

	return result;
}

template<typename T>
matrix4x4<T> rtsQuaternion<T>::toMatrix()
{
	matrix4x4<T> result;


    double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;


    // calculate coefficients
    x2 = x + x; y2 = y + y;
    z2 = z + z;
    xx = x * x2; xy = x * y2; xz = x * z2;
    yy = y * y2; yz = y * z2; zz = z * z2;
    wx = w * x2; wy = w * y2; wz = w * z2;

	result(0, 0) = 1.0 - (yy + zz);
	result(0, 1) = xy - wz;
    //m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;

	result(0, 2) = xz + wy;
	result(0, 3) = 0.0;
    //m[2][0] = xz + wy; m[3][0] = 0.0;

	result(1, 0) = xy + wz;
	result(1, 1) = 1.0 - (xx + zz);
    //m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);

	result(1, 2) = yz - wx;
	result(1, 3) = 0.0;
    //m[2][1] = yz - wx; m[3][1] = 0.0;

	result(2, 0) = xz - wy;
	result(2, 1) = yz + wx;
    //m[0][2] = xz - wy; m[1][2] = yz + wx;

	result(2, 2) = 1.0 - (xx + yy);
	result(3, 2) = 0.0;
    //m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;

	result(3, 0) = 0.0;
	result(3, 1) = 0.0;
	result(3, 2) = 0.0;
	result(3, 3) = 1.0;
    //m[0][3] = 0; m[1][3] = 0;
    //m[2][3] = 0; m[3][3] = 1;
	/*
	double* orientationmatrix=(double*)m;
	char c;


	double* result=new double[16];
	double* array=(double*)m;
	for(int i=0; i<16; i++)
		result[i]=array[i];
	*/

	return result;
}

template<typename T>
rtsQuaternion<T>::rtsQuaternion()
{
	w=0.0; x=0.0; y=0.0; z=0.0;
}

template<typename T>
rtsQuaternion<T>::rtsQuaternion(T c, T i, T j, T k)
{
	w=c;  x=i;  y=j;  z=k;
}

#endif