#include "quaternion.h"

Quaternion QuatConjugate(const Quaternion* q)
{
  Quaternion res = {.x = -q->x, .y = -q->y, .z = -q->z, .w = q->w};
  return res;
}

Quaternion QuatInvert(const Quaternion* q)
{
	Quaternion res;
	float nsq = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
	
	if (nsq > 1e-6f)
	{
		nsq = 1.0f / nsq;
		
		res.x = q->x * nsq;
		res.y = q->y * nsq;
		res.z = q->z * nsq;
		res.w = q->w * nsq;
		
		return res;
	}
	
	return *q;
}

Quaternion QuatNegate(const Quaternion* q)
{
	Quaternion res = {.x = -q->x, .y = -q->y, .z = -q->z, .w = -q->w};
	return res;
}

Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2)
{
	Quaternion res = {.x = q1->x + q2->x, .y = q1->y + q2->y, .z = q1->z + q2->z, .w = q1->w + q2->w};
	return res;
}

Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2)
{
	Quaternion res = {.x = q1->x - q2->x, .y = q1->y - q2->y, .z = q1->z - q2->z, .w = q1->w - q2->w};
	return res;
}

Quaternion QuatConstProd(const Quaternion* q, const float value)
{
	Quaternion res = {.x = q->x * value, .y = q->y * value, .z = q->z * value, .w = q->w * value};
	return res;
}

Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
{
	Quaternion res = {
	.x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
	.y = q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
	.z = q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
	.w = q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
	};
	
	
	return res;
}