#include "Vectors.h"
#include "Quaternions.h"

void RotateQuaternion(Vector * v, float Angle, Quaternion * q)
{
	q->v.x = v->x * sin(Angle/2);
	q->v.y = v->y * sin(Angle/2);
	q->v.z = v->z * sin(Angle/2);
	q->w = cos(Angle/2);
	NormalizeQuaternion(q);
}


/* Not very nice code :/ */
/* Ick. Maybe parentheses would help? Nope, still pretty confusing... */
void MultiplyQuaternion(Quaternion * q1, Quaternion * q2, Quaternion * q3)
{
	q3->v.x = ((q1->w * q2->v.x) + (q1->v.x * q2->w) + (q1->v.y * q2->v.z) - (q1->v.z * q2->v.y)); 
	q3->v.y = ((q1->w * q2->v.y) - (q1->v.x * q2->v.z) + (q1->v.y * q2->w) + (q1->v.z * q2->v.x)); 
	q3->v.z = ((q1->w * q2->v.z) + (q1->v.x * q2->v.y) - (q1->v.y * q2->v.x) + (q1->v.z * q2->w)); 
	q3->w = ((q1->w * q2->w) - (q1->v.x * q2->v.x) - (q1->v.y * q2->v.y) - (q1->v.z * q2->v.z));
}

void Vector2Quaternion(Vector * v, Quaternion * q)
{
	q->v.x = v->x;
	q->v.y = v->y;
	q->v.z = v->z;
	q->w = 0;
}

void Quaternion2Vector(Quaternion * q, Vector * v)
{
	v->x = q->v.x;
	v->y = q->v.y;
	v->z = q->v.z;
}

void ConjugateQuaternion(Quaternion * q) 
{ 
	q->v.x = -q->v.x;
	q->v.y = -q->v.y;
	q->v.z = -q->v.z;
}

float QuaternionMagnitude(Quaternion * q)
{
	return sqrt(q->v.x * q->v.x + q->v.y * q->v.y + q->v.z * q->v.z + q->w * q->w);
}

void NormalizeQuaternion(Quaternion * q)
{
	float Mag = QuaternionMagnitude(q);
	q->v.x /= Mag;
	q->v.y /= Mag;
	q->v.z /= Mag;
	q->w /= Mag;
}

void QuaternionRotateVector(float Angle, float interval, Vector * rotate, 
	Vector * toRotate, Quaternion * ReturnValue)
{
	Quaternion Rotation, QuatView, Result;
	
	RotateQuaternion(rotate, Angle / interval, &Rotation);
	
	Vector2Quaternion(toRotate, &QuatView);
	
	MultiplyQuaternion(&Rotation, &QuatView, &Result);
	ConjugateQuaternion(&Rotation);
	MultiplyQuaternion(&Result, &Rotation, ReturnValue);
}