#include "quaternion.h"
#include <math.h>

#define PI		3.14159265359f

Quaternion QuatNormalize(const Quaternion* q, const float gain)
{
  Quaternion res = {0.0f, 0.0f, 0.0f, 0.0f};
	
	float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
	
	if (norm > 1e-6f)
	{
		norm = gain / norm;
		
		res.x = q->x * norm;
		res.y = q->y * norm;
		res.z = q->z * norm;
		res.w = q->w * norm;
		
		return res;
	}
	
	return res;
}

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 = {q1->x + q2->x, q1->y + q2->y, q1->z + q2->z, 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 = {q->x * value, q->y * value, q->z * value, q->w * value};
	return res;
}

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

Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW)
{
	Quaternion v = {vec->x, vec->y, 0, 0};
	Quaternion h = {0, 0, CCW ? q->z : -q->z, q->w};
	h = QuatNormalize(&h, 1.0f);

	Quaternion vhprod = QuatProd(&v, &h);
	h = QuatProd(&vhprod, &h);
	
	Vector3 res = {h.x, h.y, vec->z};
	
	return res;
}

Quaternion QuatCreateFromEuler(const Vector3* eulerAngels)
{
	Quaternion res;
	
	float h_r = 0.5f * eulerAngels->y;
	float h_p = 0.5f * eulerAngels->x;
	float h_y = 0.5f * eulerAngels->z;
	
	float c_r = cosf(h_r), s_r = sinf(h_r);
	float c_p = cosf(h_p), s_p = sinf(h_p);
	float c_y = cosf(h_y), s_y = sinf(h_y);
	
	res.x = c_r * s_p * c_y - s_r * c_p * s_y;	// Был +, стал -
	res.y = s_r * c_p * c_y + c_r * s_p * s_y;	// Был -, стал +
	res.z = -c_r * c_p * s_y - s_r * s_p * c_y;	// Первое слагаемое стало отрицательным
	res.w = c_r * c_p * c_y - s_r * s_p * s_y;	// Был +, стал -
	
	return res;
}

Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay)
{
  	Quaternion error = {
	current->w * target->x + current->x * target->w + current->y * target->z - current->z * target->y,
	current->w * target->y + current->x * target->z + current->y * target->w - current->z * target->x,
	current->w * target->z + current->x * target->y + current->y * target->x - current->z * target->w,
	current->w * target->w + current->x * target->x + current->y * target->y - current->z * target->z
	};
	
	if (fastWay && error.w < 0.0f) return QuatNegate(&error);
	
	return error;
}

Vector3 QuatToEuler(const Quaternion* q)
{
	Vector3 e;

    e.x = atan2f(2*(q->w*q->x + q->y*q->z),
                 1 - 2*(q->x*q->x + q->y*q->y));

    e.y = asinf(2*(q->w*q->y - q->z*q->x));

    e.z = atan2f(2*(q->w*q->z + q->x*q->y),
                 1 - 2*(q->y*q->y + q->z*q->z));

    e.x *= 180.0f / PI;
    e.y *= 180.0f / PI;
    e.z *= 180.0f / PI;

    return e;
}