//----------------------------------------------------------------------------
// Class definition include
//----------------------------------------------------------------------------

#include "Quaternion.h"

//----------------------------------------------------------------------------
// Class implementation
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
// Builder
//----------------------------------------------------------------------------

Quaternion::Quaternion( )
{
	theta = 0.0;
	w = Vec3(0,0,0);
	r = cos(theta/2);
	double sinHalfTheta = sin(theta/2);
	im = w*sinHalfTheta;
}

Quaternion::Quaternion(Vec3 v1, Vec3 v2)
{
	// Normalize the vectors
	v1.Normalize();
	v2.Normalize();

	// Dot product
	float cosTheta = v1.Dot(v2);
	theta = acos(cosTheta);

	// Cross product
	w = v1.Cross(v2);

	// Normalization
	w.Normalize();

	// Build the quaternion
	r = cos(theta/2);
	double sinHalfTheta = sin(theta/2);
	im = w*sinHalfTheta;
}

Quaternion::Quaternion(float nTheta, Vec3 nW)
{
	theta = nTheta;
	w = nW;
	w.Normalize();

	// Build the quaternion
	r = cos(theta/2);
	double sinHalfTheta = sin(theta/2);
	im = w*sinHalfTheta;
}

Quaternion::Quaternion(float nR, float nI, float nJ, float nK)
{
	r = nR;
	im = Vec3(nI, nJ, nK);

	theta = 2*acos(r);
	double sinHalfTheta = sin(theta/2);
	w = im/sinHalfTheta;
}

//----------------------------------------------------------------------------
// Destructor
//----------------------------------------------------------------------------

Quaternion::~Quaternion( )
{

}
//----------------------------------------------------------------------------
// Methods
//----------------------------------------------------------------------------

float Quaternion::getR()
{
	return r;
}

float Quaternion::getI()
{
	return w[0];
}

float Quaternion::getJ()
{
	return w[1];
}

float Quaternion::getK()
{
	return w[2];
}

Vec3 Quaternion::getW()
{
	return w;
}

Quaternion Quaternion::conjugate( )
{
	return Quaternion(theta, -w);
}

Vec3 Quaternion::rotateVector(Vec3 vector)
{
	// Quaterion representing the vector
	vector.Normalize();
	Quaternion qv(0,vector[0],vector[1],vector[2]);
	Quaternion qthis(r,im[0],im[1],im[2]);
	Quaternion qthis_conj = qthis.conjugate();


	cout << "Qv: [" << qv << "]" << endl;
	cout << "Qthis: [" << qthis << "]" << endl;
	cout << "QthisConj: [" << qthis_conj << "]" << endl;

	// Final quaternion
	Quaternion Qtemp = qthis*qv;
	cout << "Qtemp: [" << Qtemp << "]" << endl;

	Quaternion qf = Qtemp*qthis_conj;

	cout << "Qf: [" << qf << "]" << endl;

	return qf.im;
	//return Vec3(0,0,0);
}

//----------------------------------------------------------------------------
// Operators
//----------------------------------------------------------------------------

const float& Quaternion::operator[](unsigned int i) const
{
	return (i == 0 ? r : (im)[i-1]);
}

Quaternion Quaternion::operator*(Quaternion q)
{
	float a1 = this->r;
	float b1 = this->w[0];
	float c1 = this->w[1];
	float d1 = this->w[2];

	float a2 = q.r;
	float b2 = q.w[0];
	float c2 = q.w[1];
	float d2 = q.w[2];

	float r_n = a1*a2 - b1*b2 - c1*c2 - d1*d2;
	float i_n = a1*b2 + a2*b1 + c1*d2 - c2*d1;
	float j_n = a1*c2 - b1*d2 + a2*c1 + b2*d1;
	float k_n = a1*d2 + b1*c2 - b2*c1 + a2*d1;

	return Quaternion(r_n, i_n, j_n, k_n);
}

std::ostream& operator<<(std::ostream& os, const Quaternion& q)
{
	os << q[0] << " " << q[1] << " " << q[2] << " " << q[3];
	os << " " <<q.theta*180/M_PI << " " << q.w;
	return os;
}