/*
 * quaternion.cpp
 *
 *  Created on: Jun 17, 2024
 *      Author: Dario
 */

#include <sta/math/quaternion.hpp>

#include <math.h>
#include <sta/debug/profile.hpp>

namespace sta
{
	namespace math
	{
		Quaternion::Quaternion(float w, float x, float y, float z)
	        : x{x}, y{y}, z{z}, w{w}
	    {

	    }

	    Quaternion::Quaternion()
	    	: x{0}, y{0}, z{0}, w{1}
	    {

	    }

	    Quaternion Quaternion::integrate(float dt, float ox, float oy, float oz)
	    {
	    	ox *= (M_PI / 180.0f);
			oy *= (M_PI / 180.0f);
			oz *= (M_PI / 180.0f);

			float norm = sqrt(ox*ox + oy*oy + oz*oz);

			if (norm == 0)
			{
				return *this;
			}

			float dt2 = dt / 2;

			float cosDt2 = cos(norm * dt2);
			float sinDt2 = 1/norm * sin(norm * dt2);

			float qw = cosDt2*w 	+ -sinDt2*ox*x 	+ -sinDt2*oy*y 	+ -sinDt2*oz*z;
			float qx = sinDt2*ox*w 	+  cosDt2*x 	+ sinDt2*oz*y 	+ -sinDt2*oy*z;
			float qy = sinDt2*oy*w 	+ -sinDt2*oz*x 	+  cosDt2*y 	+  sinDt2*ox*z;
			float qz = sinDt2*oz*w 	+  sinDt2*oy*x 	+ -sinDt2*ox*y 	+  cosDt2*z;

			return Quaternion(qw, qx, qy, qz).normalized();
	    }

		matrix Quaternion::toRotationMatrix(){
			
			matrix R = matrix(3, 3);
			R.set(0, 0, 2 *(w*w + x*x) - 1);
			R.set(0, 1, 2 *(x*y - w*z));
			R.set(0, 2, 2 *(x*z + w*y));

			R.set(1, 0, 2 *(x*y + w*z));
			R.set(1, 1, 2 *(w*w + y*y) - 1);
			R.set(1, 2, 2 *(y*z - w*x));

			R.set(2, 0, 2 *(x*z - w*y));
			R.set(2, 1, 2 *(y*z + w*x));
			R.set(2, 2, 2 *(w*w + z*z) - 1);
			return R;
		}

	    Quaternion Quaternion::unit()
	    {
	    	return Quaternion();
	    }

	    float Quaternion::norm()
	    {
	        return sqrtf(x*x + y*y + z*z + w*w);
	    }

	    Quaternion Quaternion::normalized()
	    {
	        float n = norm();

	        return Quaternion(w / n, x / n, y / n, z / n);
	    }

		Quaternion Quaternion::operator*(const Quaternion& quat)
		{
			float rw = w * quat.w - x * quat.x + y * quat.y + z * quat.z;
			float rx = w * quat.x + x * quat.w + y * quat.z - z * quat.y;
			float ry = w * quat.y - x * quat.z + y * quat.w + z * quat.x;
			float rz = w * quat.z - x * quat.y - y * quat.x + z * quat.w;

			return Quaternion(rw, rx, ry, rz);
		}

		Quaternion operator*(const Quaternion& q1, const Quaternion& q2)
		{
			float rw = q1.w * q2.w - q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
			float rx = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
			float ry = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
			float rz = q1.w * q2.z - q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;

			return Quaternion(rw, rx, ry, rz);
		}

	    Quaternion operator+(const Quaternion& q1, const Quaternion& q2)
	    {
	        return Quaternion(q1.w + q2.w, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z);
	    }

	    Quaternion operator*(const Quaternion& quat, float scalar)
		{
			return Quaternion(scalar * quat.w, scalar * quat.x, scalar * quat.y, scalar * quat.z);
		}

		Quaternion operator*(float scalar, const Quaternion& quat)
		{
			return quat * scalar;
		}

		Quaternion operator*(const Quaternion& quat, double scalar)
		{
			return Quaternion(scalar * quat.w, scalar * quat.x, scalar * quat.y, scalar * quat.z);
		}

		Quaternion operator*(double scalar, const Quaternion& quat)
		{
			return quat * scalar;
		}
	}  // namespace math
} // namespace sta