#include "Quaternion.hpp"
#include "Vector4.hpp"
#include "Vector3.hpp"
#include "MathF.hpp"
#include <cmath>

TSE::Quaternion::Quaternion() { }

TSE::Quaternion::Quaternion(float _x, float _y, float _z, float _w)
{
    x = _x;
    y = _y;
    z = _z;
    w = _w;
}

TSE::Quaternion::Quaternion(const Quaternion &other)
{
    x = other.x;
    y = other.y;
    z = other.z;
    w = other.w;
}

TSE::Quaternion::Quaternion(const Vector4 &other)
{
    x = other.x;
    y = other.y;
    z = other.z;
    w = other.w;
}

TSE::Vector3 TSE::Quaternion::ToEulerAngles() const
{
    Vector3 euler;

    float sinr_cosp = 2.0f * (w * x + y * z);
    float cosr_cosp = 1.0f - 2.0f * (x * x + y * y);
    euler.x = Rad2Deg(std::atan2(sinr_cosp, cosr_cosp));

    float sinp = 2.0f * (w * y - z * x);
    if (std::abs(sinp) >= 1)
        euler.y = Rad2Deg(std::copysign(TSE_PI / 2.0f, sinp));
    else
        euler.y = Rad2Deg(std::asin(sinp));

    float siny_cosp = 2.0f * (w * z + x * y);
    float cosy_cosp = 1.0f - 2.0f * (y * y + z * z);
    euler.z = Rad2Deg(std::atan2(siny_cosp, cosy_cosp));

    return euler;
}

TSE::Vector3 TSE::Quaternion::Rotate(const Vector3 &v) const
{
    Vector3 qv(x, y, z);
    Vector3 t = Vector3::Cross(qv, v) * 2.0f;
    return v + t * w + Vector3::Cross(qv, t);
}

void TSE::Quaternion::Normalize()
{
    float lensq = w*w + x*x + y*y + z*z;
    if(lensq <= TSE_EPSILON) return;
    const float inv = 1.0f / std::sqrt(lensq);
    x *= inv; y *= inv; z *= inv; w *= inv;
}

TSE::Quaternion TSE::Quaternion::FromAngleAxis(float angleRad, const Vector3 &axis)
{
    Vector3 a = Vector3::Normalize(axis);
    float half = angleRad * 0.5f;
    float s = sin(half);

    Quaternion q;
    q.w = std::cos(half);
    q.x = a.x * s;
    q.y = a.y * s;
    q.z = a.z * s;

    q.Normalize();

    return q;
}

TSE::Quaternion TSE::Quaternion::FromEulerAngles(const Vector3 &euler)
{
    float roll  = Deg2Rad(euler.x);
    float pitch = Deg2Rad(euler.y);
    float yaw   = Deg2Rad(euler.z);

    float cr = std::cos(roll * 0.5f);
    float sr = std::sin(roll * 0.5f);
    float cp = std::cos(pitch * 0.5f);
    float sp = std::sin(pitch * 0.5f);
    float cy = std::cos(yaw * 0.5f);
    float sy = std::sin(yaw * 0.5f);

    return Quaternion(
        sr * cp * cy - cr * sp * sy,
        cr * sp * cy + sr * cp * sy,
        cr * cp * sy - sr * sp * cy,
        cr * cp * cy + sr * sp * sy
    );
}

TSE::Vector3 TSE::Quaternion::ToEulerAngles(const Quaternion &q)
{
    return q.ToEulerAngles();
}

TSE::Quaternion TSE::Quaternion::operator*(const Quaternion &other) const
{
    Quaternion b = *this;
    Quaternion r;
    r.w = b.w*other.w - b.x*other.x - b.y*other.y - b.z*other.z;
    r.x = b.w*other.x + b.x*other.w + b.y*other.z - b.z*other.y;
    r.y = b.w*other.y - b.x*other.z + b.y*other.w + b.z*other.x;
    r.z = b.w*other.z + b.x*other.y - b.y*other.x + b.z*other.w;
    return r;
}

TSE::Quaternion TSE::Quaternion::operator*=(const Quaternion &other)
{
    *this = *this * other;
    return *this;
}

bool TSE::Quaternion::operator==(const Quaternion &other) const
{
    return x == other.x && y == other.y && z == other.z && w == other.w;
}

bool TSE::Quaternion::operator!=(const Quaternion &other) const
{
    return !(*this == other);
}
