#ifndef BLUECORE_QUATERNION_H
#define BLUECORE_QUATERNION_H

#include "Vector.h"

namespace BlueCore
{
	template <class T>
	class QuaternionTemplate
	{
	public:
		T w, x, y, z;


		/**
		 * constructor
		 */
		inline QuaternionTemplate()
		{
			w = static_cast<T>(1.0);
			x = static_cast<T>(0.0);
			y = static_cast<T>(0.0);
			z = static_cast<T>(0.0);
		}
		

		/**
		 * contructor
		 */
		template <class S>
		inline QuaternionTemplate( S w, S x, S y, S z )
		{
			this->w = static_cast<T>(w);
			this->x = static_cast<T>(x);
			this->y = static_cast<T>(y);
			this->z = static_cast<T>(z);
		}


		/**
		 * contructor
		 */
		template <class S>
		inline QuaternionTemplate( Vector3Template<S> axis, S angle )
		{
			T half_angle = static_cast<T>(angle)/static_cast<T>(2.0);
			T sin_half_angle = static_cast<T>( sin( half_angle ) );
			
			w = static_cast<T>( cos( half_angle ) );
			x = sin_half_angle * static_cast<T>(axis.x);
			y = sin_half_angle * static_cast<T>(axis.y);
			z = sin_half_angle * static_cast<T>(axis.z);
		}


		/**
		 * contructor
		 */
		template <class S>
		inline QuaternionTemplate( S h, S a, S b )
		{
			T c1 = static_cast<T>( cos(h / 2.0) );
			T c2 = static_cast<T>( cos(a / 2.0) );
			T c3 = static_cast<T>( cos(b / 2.0) );
			T s1 = static_cast<T>( sin(h / 2.0) );
			T s2 = static_cast<T>( sin(a / 2.0) );
			T s3 = static_cast<T>( sin(b / 2.0) );
			
			w = c1 * c2 * c3 - s1 * s2 * s3;
			x = s1 * s2 * c3 + c1 * c2 * s3;
			y = s1 * c2 * c3 + c1 * s2 * s3;
			z = c1 * s2 * c3 - s1 * c2 * s3;
		}


		/**
		 * identity
		 */
		inline void identity()
		{
			w = static_cast<T>(1.0);
			x = static_cast<T>(0.0);
			y = static_cast<T>(0.0);
			z = static_cast<T>(0.0);
		}	
		

		/**
		 * operator +
		 */
		template <class S>
		inline QuaternionTemplate<T> operator + (
			const QuaternionTemplate<S> &a ) const
		{
			return QuaternionTemplate<T>(
				w + static_cast<T>(a.w),
				x + static_cast<T>(a.x),
				y + static_cast<T>(a.y),
				z + static_cast<T>(a.z) );
		}


		/**
		 * operator +=
		 */
		template <class S>
		inline QuaternionTemplate<T> &operator += (
			const QuaternionTemplate<S> &a )
		{
			w += static_cast<T>(a.w);
			x += static_cast<T>(a.x);
			y += static_cast<T>(a.y);
			z += static_cast<T>(a.z);
	
			return *this;
		}	


		/**
		 * operator -
		 */
		template <class S>
		inline QuaternionTemplate<T> operator - (
			const QuaternionTemplate<S> &a ) const
		{
			return QuaternionTemplate<T>(
				w - static_cast<T>(a.w),
				x - static_cast<T>(a.x),
				y - static_cast<T>(a.y),
				z - static_cast<T>(a.z) );
		}
		

		/**
		 * operator *
		 */
		template <class S>
		inline QuaternionTemplate<T> operator * (
			const QuaternionTemplate<S> &a ) const
		{
			return QuaternionTemplate<T>(
				w * static_cast<T>(a.w) -
				x * static_cast<T>(a.x) -
				y * static_cast<T>(a.y) -
				z * static_cast<T>(a.z), 
				w * static_cast<T>(a.x) +
				x * static_cast<T>(a.w) +
				y * static_cast<T>(a.z) -
				z * static_cast<T>(a.y),
				w * static_cast<T>(a.y) -
				x * static_cast<T>(a.z) +
				y * static_cast<T>(a.w) +
				z * static_cast<T>(a.x),
				w * static_cast<T>(a.z) +
				x * static_cast<T>(a.y) -
				y * static_cast<T>(a.x) +
				z * static_cast<T>(a.w) );
		}		
		

		/**
		 * operator *=
		 */
		template <class S>
		inline QuaternionTemplate<T> &operator *=(
			const QuaternionTemplate<S> &a )
		{
			w = w * static_cast<T>(a.w) - x * static_cast<T>(a.x) -
				y * static_cast<T>(a.y) - z * static_cast<T>(a.z);
			x = w * static_cast<T>(a.x) + x * static_cast<T>(a.w) +
				y * static_cast<T>(a.z) - z * static_cast<T>(a.y);
			y = w * static_cast<T>(a.y) - x * static_cast<T>(a.z) +
				y * static_cast<T>(a.w) + z * static_cast<T>(a.x);
			z = w * static_cast<T>(a.z) + x * static_cast<T>(a.y) -
				y * static_cast<T>(a.x) + z * static_cast<T>(a.w);
			
			return *this;
		}	


		/**
		 * operator -=
		 */
		template <class S>
		inline QuaternionTemplate<T> &operator -= (
			const QuaternionTemplate<S> &a )
		{
			w -= static_cast<T>(a.w);
			x -= static_cast<T>(a.x);
			y -= static_cast<T>(a.y);
			z -= static_cast<T>(a.z);
	
			return *this;
		}	


		/**
		 * operator =
		 */
		template <class S>
		inline QuaternionTemplate<T> &operator = (
			const QuaternionTemplate<S> &a )
		{
			w = static_cast<T>(a.w);
			x = static_cast<T>(a.x);
			y = static_cast<T>(a.y);
			z = static_cast<T>(a.z);
	
			return *this;
		}


		/**
		 * unit
		 */
		inline QuaternionTemplate<T> unit() const
		{
			T d = 1/sqrt( w*w + x*x + y*y + z*z );
			return QuaternionTemplate<T>( w * d, x * d, y * d, z * d );
		}			


		/**
		 * inversed
		 */
		inline QuaternionTemplate<T> inversed() const
		{
			return QuaternionTemplate<T>( w, -x, -y, -z );
		}			

		/**
		 * inversed
		 */
		inline void inverse()
		{
			x = -x;
			y = -y;
			z = -z;
		}			


		/**
		 * normalize
		 */
		inline QuaternionTemplate<T> &normalize()
		{
			T d = 1/sqrt( w*w + x*x + y*y + z*z );
			w *= d;
			x *= d;
			y *= d;
			z *= d;
	
			return *this;
		}
		

		/**
		 * apply
		 */
		template <class S>
		inline const Vector3Template<T> apply(
			const Vector3Template<S> &a ) const
		{
			T	xx = x*x, xy = x*y, xz = x*z, xw = x*w,
				yy = y*y, yz = y*z, yw = y*w,
				zz = z*z, zw = z*w;
				
			return Vector3Template<T>(
				2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
				static_cast<T>(a.y) * ( xy - zw ) +
				static_cast<T>(a.z) * ( xz + yw ) ),
				
				2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
				static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
				static_cast<T>(a.z) * ( yz - xw ) ),
				
				2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
				static_cast<T>(a.y) * ( yz + xw ) +
				static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
		}


		/**
		 * apply
		 */
		template <class S>
		inline const Vector3Template<T> operator * (
			const Vector3Template<S> &a ) const
		{
			T	xx = x*x, xy = x*y, xz = x*z, xw = x*w,
				yy = y*y, yz = y*z, yw = y*w,
				zz = z*z, zw = z*w;
				
			return Vector3Template<T>(
				2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
				static_cast<T>(a.y) * ( xy - zw ) +
				static_cast<T>(a.z) * ( xz + yw ) ),
				
				2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
				static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
				static_cast<T>(a.z) * ( yz - xw ) ),
				
				2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
				static_cast<T>(a.y) * ( yz + xw ) +
				static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
		}


		/**
		 * applyInversed
		 */
		template <class S>
		inline Vector3Template<T> applyInversed(
			const Vector3Template<S> &a ) const
		{
			T	xx = x*x, xy = x*y, xz = x*z, xw = -x*w,
				yy = y*y, yz = y*z, yw = -y*w,
				zz = z*z, zw = -z*w;
				
			return Vector3Template<T>(
				2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
				static_cast<T>(a.y) * ( xy - zw ) +
				static_cast<T>(a.z) * ( xz + yw ) ),
				
				2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
				static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
				static_cast<T>(a.z) * ( yz - xw ) ),
				
				2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
				static_cast<T>(a.y) * ( yz + xw ) +
				static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
		}
		
		/**
		 * transform from ode to gl coodinates
		 */
		inline QuaternionTemplate<T> toGl() const
		{
			return QuaternionTemplate<T>( w, x, z, -y );
		}

		/**
		 * transform from gl to ode coodinates
		 */
		inline QuaternionTemplate<T> toOde() const
		{
			return QuaternionTemplate<T>( w, x, -z, y );
		}
		
		inline QuaternionTemplate<T> slerp( const QuaternionTemplate<T> &q, const Scalar &t )
		{
			Scalar phi = acos(w*q.w + x*q.x + y*q.y + z*q.z);
			Scalar s = 1 / sin(phi);
			Scalar a = sin(phi*(1-t)) * s; 
			Scalar b = sin(phi*t) * s;

			return QuaternionTemplate<T>( a*w+b*q.w, a*x+b*q.x, a*y+b*q.y, a*z+b*q.z );
		}
	};
	
	typedef QuaternionTemplate<float> QuaternionFloat;
	typedef QuaternionTemplate<double> QuaternionDouble;
	typedef QuaternionTemplate<Scalar> Quaternion;
}

#endif