#ifndef RTS_QUATERNION_H
#define RTS_QUATERNION_H

#include <stim/math/matrix.h>
#include <stim/cuda/cudatools/callable.h>

namespace stim{

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

	CUDA_CALLABLE void normalize(){

		double length=sqrt(w*w + x*x + y*y + z*z);
		w=w/length;
		x=x/length;
		y=y/length;
		z=z/length;
	}

	CUDA_CALLABLE void CreateRotation(T theta, T ux, T uy, T uz){

		vec3<T> u(ux, uy, uz);
		CreateRotation(theta, u);		
	}

	CUDA_CALLABLE void CreateRotation(T theta, vec3<T> u){

		vec3<T> u_hat = u.norm();

		//assign the given Euler rotation to this quaternion
		w = (T)cos(theta/2);
		x = u_hat[0]*(T)sin(theta/2);
		y = u_hat[1]*(T)sin(theta/2);
		z = u_hat[2]*(T)sin(theta/2);
	}

	CUDA_CALLABLE void CreateRotation(vec3<T> from, vec3<T> to){

		from = from.norm();
		to = to.norm();
		vec3<T> r = from.cross(to);			//compute the rotation vector
		T theta = asin(r.len());				//compute the angle of the rotation about r
		//deal with a zero vector (both k and kn point in the same direction)
		if(theta == (T)0){
			return;
		}

		//create a quaternion to capture the rotation
		CreateRotation(theta, r.norm());
	}



	CUDA_CALLABLE quaternion<T> operator *(quaternion<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);

		quaternion<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;
	}
	
	CUDA_CALLABLE matrix<T, 3> toMatrix3(){

		matrix<T, 3> result;


	    T 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 - (yy + zz);
		result(0, 1) = xy - wz;

		result(0, 2) = xz + wy;

		result(1, 0) = xy + wz;
		result(1, 1) = 1 - (xx + zz);

		result(1, 2) = yz - wx;

		result(2, 0) = xz - wy;
		result(2, 1) = yz + wx;

		result(2, 2) = 1 - (xx + yy);

		return result;
	}

	CUDA_CALLABLE matrix<T, 4> toMatrix4(){

		matrix<T, 4> result;
	    T 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 - (yy + zz);
		result(0, 1) = xy - wz;

		result(0, 2) = xz + wy;

		result(1, 0) = xy + wz;
		result(1, 1) = 1 - (xx + zz);

		result(1, 2) = yz - wx;

		result(2, 0) = xz - wy;
		result(2, 1) = yz + wx;

		result(2, 2) = 1 - (xx + yy);

		result(3, 3) = 1;

		return result;
	}


	CUDA_CALLABLE quaternion(){
		w=0; x=0; y=0; z=0;
	}

	CUDA_CALLABLE quaternion(T c, T i, T j, T k){
		w=c;  x=i;  y=j;  z=k;
	}

};

}	//end rts namespace


#endif