#include "Math/Quaternion.h"
#include "Math/Matrix4.h"
#include "Math/Vector3.h"

namespace XCEngine {
namespace Math {

Quaternion Quaternion::FromAxisAngle(const Vector3& axis, float radians) {
    float halfAngle = radians * 0.5f;
    float s = std::sin(halfAngle);
    return Quaternion(
        axis.x * s,
        axis.y * s,
        axis.z * s,
        std::cos(halfAngle)
    );
}

Quaternion Quaternion::FromEulerAngles(float pitch, float yaw, float roll) {
    float halfPitch = pitch * 0.5f;
    float halfYaw = yaw * 0.5f;
    float halfRoll = roll * 0.5f;

    float sinPitch = std::sin(halfPitch);
    float cosPitch = std::cos(halfPitch);
    float sinYaw = std::sin(halfYaw);
    float cosYaw = std::cos(halfYaw);
    float sinRoll = std::sin(halfRoll);
    float cosRoll = std::cos(halfRoll);

    return Quaternion(
        cosYaw * sinPitch * cosRoll + sinYaw * cosPitch * sinRoll,
        sinYaw * cosPitch * cosRoll - cosYaw * sinPitch * sinRoll,
        cosYaw * cosPitch * sinRoll - sinYaw * sinPitch * cosRoll,
        cosYaw * cosPitch * cosRoll + sinYaw * sinPitch * sinRoll
    );
}

Quaternion Quaternion::FromEulerAngles(const Vector3& euler) {
    return FromEulerAngles(euler.x, euler.y, euler.z);
}

Quaternion Quaternion::FromRotationMatrix(const Matrix4& matrix) {
    float trace = matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2];

    Quaternion q;

    if (trace > 0.0f) {
        float s = std::sqrt(trace + 1.0f) * 2.0f;
        q.w = 0.25f * s;
        q.x = (matrix.m[2][1] - matrix.m[1][2]) / s;
        q.y = (matrix.m[0][2] - matrix.m[2][0]) / s;
        q.z = (matrix.m[1][0] - matrix.m[0][1]) / s;
    } else if (matrix.m[0][0] > matrix.m[1][1] && matrix.m[0][0] > matrix.m[2][2]) {
        float s = std::sqrt(1.0f + matrix.m[0][0] - matrix.m[1][1] - matrix.m[2][2]) * 2.0f;
        q.w = (matrix.m[2][1] - matrix.m[1][2]) / s;
        q.x = 0.25f * s;
        q.y = (matrix.m[0][1] + matrix.m[1][0]) / s;
        q.z = (matrix.m[0][2] + matrix.m[2][0]) / s;
    } else if (matrix.m[1][1] > matrix.m[2][2]) {
        float s = std::sqrt(1.0f + matrix.m[1][1] - matrix.m[0][0] - matrix.m[2][2]) * 2.0f;
        q.w = (matrix.m[0][2] - matrix.m[2][0]) / s;
        q.x = (matrix.m[0][1] + matrix.m[1][0]) / s;
        q.y = 0.25f * s;
        q.z = (matrix.m[1][2] + matrix.m[2][1]) / s;
    } else {
        float s = std::sqrt(1.0f + matrix.m[2][2] - matrix.m[0][0] - matrix.m[1][1]) * 2.0f;
        q.w = (matrix.m[1][0] - matrix.m[0][1]) / s;
        q.x = (matrix.m[0][2] + matrix.m[2][0]) / s;
        q.y = (matrix.m[1][2] + matrix.m[2][1]) / s;
        q.z = 0.25f * s;
    }

    return q.Normalized();
}

Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, float t) {
    t = std::clamp(t, 0.0f, 1.0f);
    float cosHalfTheta = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;

    if (cosHalfTheta < 0.0f) {
        return Slerp(Quaternion(-b.x, -b.y, -b.z, -b.w), Quaternion(a.x, a.y, a.z, a.w), 1.0f - t);
    }

    if (std::abs(cosHalfTheta) >= 1.0f) {
        return a;
    }

    float halfTheta = std::acos(cosHalfTheta);
    float sinHalfTheta = std::sqrt(1.0f - cosHalfTheta * cosHalfTheta);

    if (std::abs(sinHalfTheta) < EPSILON) {
        return Quaternion(
            a.w * 0.5f + b.w * 0.5f,
            a.x * 0.5f + b.x * 0.5f,
            a.y * 0.5f + b.y * 0.5f,
            a.z * 0.5f + b.z * 0.5f
        );
    }

    float ratioA = std::sin((1.0f - t) * halfTheta) / sinHalfTheta;
    float ratioB = std::sin(t * halfTheta) / sinHalfTheta;

    return Quaternion(
        a.x * ratioA + b.x * ratioB,
        a.y * ratioA + b.y * ratioB,
        a.z * ratioA + b.z * ratioB,
        a.w * ratioA + b.w * ratioB
    );
}

Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
    Vector3 f = Vector3::Normalize(forward);
    Vector3 r = Vector3::Normalize(Vector3::Cross(up, f));
    Vector3 u = Vector3::Cross(f, r);

    Matrix4 m;
    m.m[0][0] = r.x; m.m[0][1] = r.y; m.m[0][2] = r.z;
    m.m[1][0] = u.x; m.m[1][1] = u.y; m.m[1][2] = u.z;
    m.m[2][0] = f.x; m.m[2][1] = f.y; m.m[2][2] = f.z;
    m.m[0][3] = m[1][3] = m[2][3] = 0.0f;
    m.m[3][0] = m[3][1] = m[3][2] = 0.0f;
    m.m[3][3] = 1.0f;

    return FromRotationMatrix(m);
}

Vector3 Quaternion::ToEulerAngles() const {
    float sinrCosp = 2.0f * (w * x + y * z);
    float cosrCosp = 1.0f - 2.0f * (x * x + y * y);
    float roll = std::atan2(sinrCosp, cosrCosp);

    float sinp = 2.0f * (w * y - z * x);
    float pitch;
    if (std::abs(sinp) >= 1.0f) {
        pitch = std::copysign(PI * 0.5f, sinp);
    } else {
        pitch = std::asin(sinp);
    }

    float sinyCosp = 2.0f * (w * z + x * y);
    float cosyCosp = 1.0f - 2.0f * (y * y + z * z);
    float yaw = std::atan2(sinyCosp, cosyCosp);

    return Vector3(pitch, yaw, roll);
}

Matrix4 Quaternion::ToMatrix4x4() const {
    float xx = x * x;
    float yy = y * y;
    float zz = z * z;
    float xy = x * y;
    float xz = x * z;
    float yz = y * z;
    float wx = w * x;
    float wy = w * y;
    float wz = w * z;

    Matrix4 result;
    result.m[0][0] = 1.0f - 2.0f * (yy + zz);
    result.m[0][1] = 2.0f * (xy - wz);
    result.m[0][2] = 2.0f * (xz + wy);
    
    result.m[1][0] = 2.0f * (xy + wz);
    result.m[1][1] = 1.0f - 2.0f * (xx + zz);
    result.m[1][2] = 2.0f * (yz - wx);
    
    result.m[2][0] = 2.0f * (xz - wy);
    result.m[2][1] = 2.0f * (yz + wx);
    result.m[2][2] = 1.0f - 2.0f * (xx + yy);
    
    result.m[3][3] = 1.0f;

    return result;
}

Quaternion Quaternion::operator*(const Quaternion& other) const {
    return Quaternion(
        w * other.x + x * other.w + y * other.z - z * other.y,
        w * other.y - x * other.z + y * other.w + z * other.x,
        w * other.z + x * other.y - y * other.x + z * other.w,
        w * other.w - x * other.x - y * other.y - z * other.z
    );
}

Quaternion Quaternion::Inverse() const {
    float norm = x * x + y * y + z * z + w * w;
    if (norm > 0.0f) {
        float invNorm = 1.0f / norm;
        return Quaternion(-x * invNorm, -y * invNorm, -z * invNorm, w * invNorm);
    }
    return Identity();
}

float Quaternion::Dot(const Quaternion& other) const {
    return x * other.x + y * other.y + z * other.z + w * other.w;
}

float Quaternion::Magnitude() const {
    return std::sqrt(x * x + y * y + z * z + w * w);
}

Quaternion Quaternion::Normalized() const {
    float mag = Magnitude();
    if (mag > EPSILON) {
        return Quaternion(x / mag, y / mag, z / mag, w / mag);
    }
    return Identity();
}

Quaternion Quaternion::Normalize(const Quaternion& q) {
    return q.Normalized();
}

Vector3 operator*(const Quaternion& q, const Vector3& v) {
    Vector3 qv(q.x, q.y, q.z);
    Vector3 uv = Vector3::Cross(qv, v);
    Vector3 uuv = Vector3::Cross(qv, uv);
    return v + (uv * q.w + uuv) * 2.0f;
}

} // namespace Math
} // namespace XCEngine