#include <gtest/gtest.h>
#include <XCEngine/Math/Quaternion.h>
#include <XCEngine/Math/Matrix4.h>
#include <XCEngine/Math/Vector3.h>
#include <XCEngine/Math/Math.h>

using namespace XCEngine::Math;

namespace {

TEST(Math_Quaternion, DefaultConstructor) {
    Quaternion q;
    EXPECT_FLOAT_EQ(q.x, 0.0f);
    EXPECT_FLOAT_EQ(q.y, 0.0f);
    EXPECT_FLOAT_EQ(q.z, 0.0f);
    EXPECT_FLOAT_EQ(q.w, 1.0f);
}

TEST(Math_Quaternion, ParameterConstructor) {
    Quaternion q(1.0f, 2.0f, 3.0f, 4.0f);
    EXPECT_FLOAT_EQ(q.x, 1.0f);
    EXPECT_FLOAT_EQ(q.y, 2.0f);
    EXPECT_FLOAT_EQ(q.z, 3.0f);
    EXPECT_FLOAT_EQ(q.w, 4.0f);
}

TEST(Math_Quaternion, Identity) {
    Quaternion q = Quaternion::Identity();
    EXPECT_FLOAT_EQ(q.x, 0.0f);
    EXPECT_FLOAT_EQ(q.y, 0.0f);
    EXPECT_FLOAT_EQ(q.z, 0.0f);
    EXPECT_FLOAT_EQ(q.w, 1.0f);
}

TEST(Math_Quaternion, FromAxisAngle) {
    Quaternion q = Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f);
    
    float sinHalfAngle = std::sin(PI * 0.25f);
    float cosHalfAngle = std::cos(PI * 0.25f);
    
    EXPECT_NEAR(q.x, 0.0f, 1e-5f);
    EXPECT_NEAR(q.y, sinHalfAngle, 1e-5f);
    EXPECT_NEAR(q.z, 0.0f, 1e-5f);
    EXPECT_NEAR(q.w, cosHalfAngle, 1e-5f);
}

TEST(Math_Quaternion, FromAxisAngle_IdentityRotation) {
    Quaternion q = Quaternion::FromAxisAngle(Vector3::Up(), 0.0f);
    EXPECT_FLOAT_EQ(q.x, 0.0f);
    EXPECT_FLOAT_EQ(q.y, 0.0f);
    EXPECT_FLOAT_EQ(q.z, 0.0f);
    EXPECT_FLOAT_EQ(q.w, 1.0f);
}

TEST(Math_Quaternion, FromEulerAngles) {
    Quaternion q = Quaternion::FromEulerAngles(PI * 0.5f, 0.0f, 0.0f);
    
    float sinHalfPitch = std::sin(PI * 0.25f);
    float cosHalfPitch = std::cos(PI * 0.25f);
    
    EXPECT_NEAR(q.x, sinHalfPitch, 1e-5f);
    EXPECT_NEAR(q.w, cosHalfPitch, 1e-5f);
}

TEST(Math_Quaternion, ToEulerAngles_FromEulerAngles) {
    Vector3 euler(PI * 0.5f, PI * 0.25f, PI * 0.125f);
    Quaternion q = Quaternion::FromEulerAngles(euler.x, euler.y, euler.z);
    Vector3 result = q.ToEulerAngles();
    
    EXPECT_NEAR(result.x, euler.x, 1e-4f);
    EXPECT_NEAR(result.y, euler.y, 1e-4f);
    EXPECT_NEAR(result.z, euler.z, 1e-4f);
}

TEST(Math_Quaternion, ToMatrix4x4_Identity) {
    Quaternion q = Quaternion::Identity();
    Matrix4x4 m = q.ToMatrix4x4();
    
    EXPECT_FLOAT_EQ(m.m[0][0], 1.0f);
    EXPECT_FLOAT_EQ(m.m[1][1], 1.0f);
    EXPECT_FLOAT_EQ(m.m[2][2], 1.0f);
    EXPECT_FLOAT_EQ(m.m[3][3], 1.0f);
}

TEST(Math_Quaternion, ToMatrix4x4_RotationY) {
    Quaternion q = Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f);
    Matrix4x4 m = q.ToMatrix4x4();
    
    EXPECT_NEAR(m.m[0][0], 0.0f, 1e-5f);
    EXPECT_NEAR(m.m[0][2], 1.0f, 1e-5f);
    EXPECT_NEAR(m.m[2][0], -1.0f, 1e-5f);
    EXPECT_NEAR(m.m[2][2], 0.0f, 1e-5f);
}

TEST(Math_Quaternion, FromRotationMatrix) {
    Matrix4x4 m = Matrix4x4::RotationY(PI * 0.5f);
    Quaternion q = Quaternion::FromRotationMatrix(m);
    
    EXPECT_NEAR(std::abs(q.y), 0.707f, 1e-3f);
}

TEST(Math_Quaternion, Slerp_IdentityToIdentity) {
    Quaternion a = Quaternion::Identity();
    Quaternion b = Quaternion::Identity();
    
    Quaternion result = Quaternion::Slerp(a, b, 0.5f);
    
    EXPECT_FLOAT_EQ(result.x, 0.0f);
    EXPECT_FLOAT_EQ(result.y, 0.0f);
    EXPECT_FLOAT_EQ(result.z, 0.0f);
    EXPECT_FLOAT_EQ(result.w, 1.0f);
}

TEST(Math_Quaternion, Slerp_QuarterRotation) {
    Quaternion a = Quaternion::Identity();
    Quaternion b = Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f);
    
    Quaternion result = Quaternion::Slerp(a, b, 0.5f);
    
    float expectedAngle = PI * 0.25f;
    Quaternion expected = Quaternion::FromAxisAngle(Vector3::Up(), expectedAngle);
    
    EXPECT_NEAR(result.x, expected.x, 1e-4f);
    EXPECT_NEAR(result.y, expected.y, 1e-4f);
    EXPECT_NEAR(result.z, expected.z, 1e-4f);
    EXPECT_NEAR(result.w, expected.w, 1e-4f);
}

TEST(Math_Quaternion, Slerp_Start_ReturnsFirst) {
    Quaternion a(0.1f, 0.2f, 0.3f, 0.9f);
    Quaternion b(0.9f, 0.1f, 0.1f, 0.1f);
    
    Quaternion result = Quaternion::Slerp(a, b, 0.0f);
    
    EXPECT_NEAR(result.x, a.x, 1e-5f);
    EXPECT_NEAR(result.y, a.y, 1e-5f);
    EXPECT_NEAR(result.z, a.z, 1e-5f);
    EXPECT_NEAR(result.w, a.w, 1e-5f);
}

TEST(Math_Quaternion, Slerp_End_ReturnsSecond) {
    Quaternion a(0.1f, 0.2f, 0.3f, 0.9f);
    Quaternion b(0.9f, 0.1f, 0.1f, 0.1f);
    
    Quaternion result = Quaternion::Slerp(a, b, 1.0f);
    
    EXPECT_NEAR(result.x, b.x, 1e-4f);
    EXPECT_NEAR(result.y, b.y, 1e-4f);
    EXPECT_NEAR(result.z, b.z, 1e-4f);
    EXPECT_NEAR(result.w, b.w, 1e-4f);
}

TEST(Math_Quaternion, Slerp_ClampsT) {
    Quaternion a = Quaternion::Identity();
    Quaternion b = Quaternion::FromAxisAngle(Vector3::Up(), PI);
    
    Quaternion result1 = Quaternion::Slerp(a, b, -0.5f);
    Quaternion result2 = Quaternion::Slerp(a, b, 1.5f);
    
    EXPECT_NEAR(result1.x, a.x, 1e-5f);
    EXPECT_NEAR(result2.x, b.x, 1e-4f);
}

TEST(Math_Quaternion, LookRotation_Forward) {
    Quaternion q = Quaternion::LookRotation(Vector3::Forward());
    
    EXPECT_NEAR(q.x, 0.0f, 1e-5f);
    EXPECT_NEAR(q.y, 0.0f, 1e-5f);
    EXPECT_NEAR(q.z, 0.0f, 1e-5f);
    EXPECT_NEAR(q.w, 1.0f, 1e-5f);
}

TEST(Math_Quaternion, LookRotation_Up) {
    Quaternion q = Quaternion::LookRotation(Vector3::Up());
    
    EXPECT_NEAR(q.x, 0.707f, 1e-3f);
    EXPECT_NEAR(q.w, 0.707f, 1e-3f);
}

TEST(Math_Quaternion, LookRotation_Right) {
    Quaternion q = Quaternion::LookRotation(Vector3::Right());
    
    EXPECT_NEAR(q.y, -0.707f, 1e-3f);
    EXPECT_NEAR(q.w, 0.707f, 1e-3f);
}

TEST(Math_Quaternion, Multiply_ProducesRotation) {
    Quaternion q1 = Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f);
    Quaternion q2 = Quaternion::FromAxisAngle(Vector3::Right(), PI * 0.5f);
    
    Quaternion result = q1 * q2;
    
    EXPECT_TRUE(result.Magnitude() > 0.0f);
}

TEST(Math_Quaternion, Multiply_Identity) {
    Quaternion q(0.1f, 0.2f, 0.3f, 0.9f);
    Quaternion identity = Quaternion::Identity();
    
    Quaternion result = q * identity;
    
    EXPECT_NEAR(result.x, q.x, 1e-5f);
    EXPECT_NEAR(result.y, q.y, 1e-5f);
    EXPECT_NEAR(result.z, q.z, 1e-5f);
    EXPECT_NEAR(result.w, q.w, 1e-5f);
}

TEST(Math_Quaternion, Multiply_Vector3) {
    Quaternion q = Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f);
    Vector3 v = Vector3::Right();
    
    Vector3 result = q * v;
    
    EXPECT_NEAR(result.x, 0.0f, 1e-5f);
    EXPECT_NEAR(result.z, 1.0f, 1e-5f);
}

TEST(Math_Quaternion, Inverse_Identity) {
    Quaternion q = Quaternion::Identity();
    Quaternion inv = q.Inverse();
    
    EXPECT_FLOAT_EQ(inv.x, 0.0f);
    EXPECT_FLOAT_EQ(inv.y, 0.0f);
    EXPECT_FLOAT_EQ(inv.z, 0.0f);
    EXPECT_FLOAT_EQ(inv.w, 1.0f);
}

TEST(Math_Quaternion, Inverse_Rotation) {
    Quaternion q = Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f);
    Quaternion inv = q.Inverse();
    
    Quaternion product = q * inv;
    
    EXPECT_NEAR(product.x, 0.0f, 1e-5f);
    EXPECT_NEAR(product.y, 0.0f, 1e-5f);
    EXPECT_NEAR(product.z, 0.0f, 1e-5f);
    EXPECT_NEAR(product.w, 1.0f, 1e-5f);
}

TEST(Math_Quaternion, Dot) {
    Quaternion a(1, 0, 0, 0);
    Quaternion b(1, 0, 0, 0);
    
    float result = a.Dot(b);
    
    EXPECT_FLOAT_EQ(result, 1.0f);
}

TEST(Math_Quaternion, Dot_Orthogonal) {
    Quaternion a(1, 0, 0, 0);
    Quaternion b(0, 1, 0, 0);
    
    float result = a.Dot(b);
    
    EXPECT_FLOAT_EQ(result, 0.0f);
}

TEST(Math_Quaternion, Magnitude) {
    Quaternion q(1, 0, 0, 0);
    EXPECT_FLOAT_EQ(q.Magnitude(), 1.0f);
}

TEST(Math_Quaternion, Normalized) {
    Quaternion q(1, 2, 3, 4);
    Quaternion normalized = q.Normalized();
    
    EXPECT_NEAR(normalized.Magnitude(), 1.0f, 1e-5f);
}

TEST(Math_Quaternion, Normalize) {
    Quaternion q(1, 2, 3, 4);
    Quaternion result = Quaternion::Normalize(q);
    
    EXPECT_NEAR(result.Magnitude(), 1.0f, 1e-5f);
}

} // namespace