#include "Mix/Quaternion.h"

using namespace Mix;

Quaternion::Quaternion( void )
{
	x = 0.0f;
	y = 0.0f;
	z = 0.0f;
	w = 1.0f;
}

Quaternion::Quaternion( const Quaternion& q )
{
	x = q.x;
	y = q.y;
	z = q.z;
	w = q.w;
}

Quaternion::Quaternion( Float32 qx, Float32 qy, Float32 qz, Float32 qw )
{
	x = qx;
	y = qy;
	z = qz;
	w = qw;
}

Quaternion::Quaternion( const Mix::Vector3& axis, Float32 angle )
{
	SetRotationAxis( axis, angle );
}

Quaternion::~Quaternion( void )
{
}

void Quaternion::SetRotationAxis( const Mix::Vector3& axis, Float32 angle )
{
	Float32 ha;
	Float32 s;

	ha = ( angle * 0.5f );
	s = ::sinf( ha );

	x = axis.x * s;
	y = axis.y * s;
	z = axis.z * s;
	w = ::cosf( ha );
}

void Quaternion::RotationAxis( const Mix::Vector3& axis, Float32 angle )
{
	Float32 ha = ( angle * 0.5f );
	Float32 s = ::sinf( ha );

	Float32 rx = axis.x * s;
	Float32 ry = axis.y * s;
	Float32 rz = axis.z * s;
	Float32 rw = ::cosf( ha );

	Float32 tx = x;
	Float32 ty = y;
	Float32 tz = z;
	Float32 tw = w;

	x = rw * tx + rx * tw + ry * tz - rz * ty;
	y = rw * ty - rx * tz + ry * tw + rz * tx;
	z = rw * tz + rx * ty - ry * tx + rz * tw;
	w = rw * tw - rx * tx - ry * ty - rz * tz;
}

Mix::Vector3 Quaternion::GetAxis( void ) const
{
	Float32 sSqr = 1.0f - w * w;

	if( MIX_FLOAT_IS_ZERO( sSqr ) == True )
	{
		return Mix::Vector3( 1.0f, 0.0f, 0.0f );
	}

	Float32 s = 1.0f / sSqr;

	return Mix::Vector3( x * s, y * s, z * s );
}

Float32 Quaternion::GetAngle( void ) const
{
	return ::acosf( w ) * 2.0f;
}

void Quaternion::Normalize( void )
{
	Float32 sq;

	sq = ::sqrtf( x * x + y * y + z * z + w * w );

	if( MIX_FLOAT_IS_ZERO( sq ) == False )
	{
		Float32 mag = ( 1.0f / sq );

		x *= mag; 
		y *= mag; 
		z *= mag; 
		w *= mag; 
	}
	else
	{
		x = 0.0f;
		y = 0.0f;
		z = 0.0f;
		w = 0.0f;
	}
}

Quaternion Quaternion::ToNormalize( void ) const
{
	Quaternion ret = *this;

	ret.Normalize();

	return ret;
}

void Quaternion::Inverse( void )
{
	Float32 s = MIX_FLOAT_RECIPROCAL( x * x + y * y + z * z + w * w );

	x = -x * s;
	y = -y * s;
	z = -z * s;
	w =  w * s;
}

Quaternion Quaternion::ToInverse( void ) const
{
	Quaternion ret = *this;

	ret.Inverse();

	return ret;
}

Boolean Quaternion::operator == ( const Quaternion& q ) const
{
	if( ( MIX_FLOAT_EQUAL( x, q.x ) == False ) ||
		( MIX_FLOAT_EQUAL( y, q.y ) == False ) ||
		( MIX_FLOAT_EQUAL( z, q.z ) == False ) ||
		( MIX_FLOAT_EQUAL( w, q.w ) == False ) )
	{
		return False;
	}

	return True;
}

Boolean Quaternion::operator != ( const Quaternion& q ) const
{
	if( ( MIX_FLOAT_EQUAL( x, q.x ) == False ) ||
		( MIX_FLOAT_EQUAL( y, q.y ) == False ) ||
		( MIX_FLOAT_EQUAL( z, q.z ) == False ) ||
		( MIX_FLOAT_EQUAL( w, q.w ) == False ) )
	{
		return True;
	}

	return False;
}

Quaternion Quaternion::operator + ( const Quaternion& q ) const
{
	Quaternion quat( *this );

	quat += q;

	return quat;
}

Quaternion Quaternion::operator - ( const Quaternion& q ) const
{
	Quaternion quat( *this );

	quat -= q;

	return quat;
}

Quaternion Quaternion::operator * ( const Quaternion& q ) const
{
	Quaternion quat( *this );

	quat *= q;

	return quat;
}

Quaternion Quaternion::operator * ( Float32 v ) const
{
	Quaternion quat( *this );

	quat *= v;

	return quat;
}

Quaternion& Quaternion::operator = ( const Quaternion& q )
{
	x = q.x;
	y = q.y;
	z = q.z;
	w = q.w;

	return *this;
}

Quaternion& Quaternion::operator += ( const Quaternion& q )
{
	x += q.x;
	y += q.y;
	z += q.z;
	w += q.w;

	return *this;
}

Quaternion& Quaternion::operator -= ( const Quaternion& q )
{
	x -= q.x;
	y -= q.y;
	z -= q.z;
	w -= q.w;

	return *this;
}

Quaternion& Quaternion::operator *= ( const Quaternion& q )
{
	Float32 tx = x;
	Float32 ty = y;
	Float32 tz = z;
	Float32 tw = w;

	x = q.w * tx + q.x * tw + q.y * tz - q.z * ty;
	y = q.w * ty - q.x * tz + q.y * tw + q.z * tx;
	z = q.w * tz + q.x * ty - q.y * tx + q.z * tw;
	w = q.w * tw - q.x * tx - q.y * ty - q.z * tz;

	return *this;
}

Quaternion& Quaternion::operator *= ( Float32 v )
{
	x *= v;
	y *= v;
	z *= v;
	w *= v;

	return *this;
}

Quaternion Quaternion::operator -( void ) const
{
	Quaternion ret;

	ret.x = -x;
	ret.y = -y;
	ret.z = -z;
	ret.w = -w;

	return ret;
}

Float32 Quaternion::Dot( const Mix::Quaternion& l, const Mix::Quaternion& r )
{
	return ( l.x * r.x ) + ( l.y * r.y ) + ( l.z * r.z ) + ( l.w * r.w );
}

Mix::Quaternion Quaternion::Cross( const Mix::Quaternion& l, const Mix::Quaternion& r )
{
	return Mix::Quaternion();
}

Mix::Quaternion Quaternion::Difference( const Mix::Quaternion& from, const Mix::Quaternion& to )
{
	return to * from.ToInverse();
}

Quaternion Quaternion::Slerp( const Mix::Quaternion& qs, const Mix::Quaternion& qd, Float32 t )
{
	Float32 d = Mix::Quaternion::Dot( qs, qd );
	Float32 e = ( d < 0.0f )? -1.0f : 1.0f;

	return qs * ( 1.0f - t ) + qd * ( e * t );
}
