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

using namespace XCEngine::Math;

namespace {

TEST(Math_Matrix4, DefaultConstructor_InitializesToZero) {
    Matrix4x4 m;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            EXPECT_FLOAT_EQ(m.m[i][j], 0.0f);
        }
    }
}

TEST(Math_Matrix4, Identity_IsCorrect) {
    Matrix4x4 id = Matrix4x4::Identity();
    
    EXPECT_FLOAT_EQ(id.m[0][0], 1.0f);
    EXPECT_FLOAT_EQ(id.m[1][1], 1.0f);
    EXPECT_FLOAT_EQ(id.m[2][2], 1.0f);
    EXPECT_FLOAT_EQ(id.m[3][3], 1.0f);
    
    EXPECT_FLOAT_EQ(id.m[0][1], 0.0f);
    EXPECT_FLOAT_EQ(id.m[1][2], 0.0f);
}

TEST(Math_Matrix4, Zero_IsCorrect) {
    Matrix4x4 zero = Matrix4x4::Zero();
    
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            EXPECT_FLOAT_EQ(zero.m[i][j], 0.0f);
        }
    }
}

TEST(Math_Matrix4, Translation) {
    Vector3 translation(1.0f, 2.0f, 3.0f);
    Matrix4x4 m = Matrix4x4::Translation(translation);
    
    EXPECT_FLOAT_EQ(m.m[0][3], 1.0f);
    EXPECT_FLOAT_EQ(m.m[1][3], 2.0f);
    EXPECT_FLOAT_EQ(m.m[2][3], 3.0f);
    EXPECT_FLOAT_EQ(m.m[3][3], 1.0f);
}

TEST(Math_Matrix4, Translation_TransformsPoint) {
    Matrix4x4 m = Matrix4x4::Translation(Vector3(1, 2, 3));
    Vector3 point(0, 0, 0);
    
    Vector3 result = m.MultiplyPoint(point);
    EXPECT_FLOAT_EQ(result.x, 1.0f);
    EXPECT_FLOAT_EQ(result.y, 2.0f);
    EXPECT_FLOAT_EQ(result.z, 3.0f);
}

TEST(Math_Matrix4, Scale) {
    Vector3 scale(2.0f, 3.0f, 4.0f);
    Matrix4x4 m = Matrix4x4::Scale(scale);
    
    EXPECT_FLOAT_EQ(m.m[0][0], 2.0f);
    EXPECT_FLOAT_EQ(m.m[1][1], 3.0f);
    EXPECT_FLOAT_EQ(m.m[2][2], 4.0f);
    EXPECT_FLOAT_EQ(m.m[3][3], 1.0f);
}

TEST(Math_Matrix4, Scale_TransformsPoint) {
    Matrix4x4 m = Matrix4x4::Scale(Vector3(2, 3, 4));
    Vector3 point(1, 1, 1);
    
    Vector3 result = m.MultiplyPoint(point);
    EXPECT_FLOAT_EQ(result.x, 2.0f);
    EXPECT_FLOAT_EQ(result.y, 3.0f);
    EXPECT_FLOAT_EQ(result.z, 4.0f);
}

TEST(Math_Matrix4, RotationX) {
    Matrix4x4 m = Matrix4x4::RotationX(PI * 0.5f);
    
    EXPECT_NEAR(m.m[1][1], 0.0f, 1e-5f);
    EXPECT_NEAR(m.m[1][2], -1.0f, 1e-5f);
    EXPECT_NEAR(m.m[2][1], 1.0f, 1e-5f);
    EXPECT_NEAR(m.m[2][2], 0.0f, 1e-5f);
}

TEST(Math_Matrix4, RotationY) {
    Matrix4x4 m = Matrix4x4::RotationY(PI * 0.5f);
    
    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_Matrix4, RotationZ) {
    Matrix4x4 m = Matrix4x4::RotationZ(PI * 0.5f);
    
    EXPECT_NEAR(m.m[0][0], 0.0f, 1e-5f);
    EXPECT_NEAR(m.m[0][1], -1.0f, 1e-5f);
    EXPECT_NEAR(m.m[1][0], 1.0f, 1e-5f);
    EXPECT_NEAR(m.m[1][1], 0.0f, 1e-5f);
}

TEST(Math_Matrix4, TRS) {
    Vector3 translation(1, 2, 3);
    Quaternion rotation = Quaternion::Identity();
    Vector3 scale(2, 2, 2);
    
    Matrix4x4 m = Matrix4x4::TRS(translation, rotation, scale);
    
    EXPECT_FLOAT_EQ(m.m[0][3], 1.0f);
    EXPECT_FLOAT_EQ(m.m[1][3], 2.0f);
    EXPECT_FLOAT_EQ(m.m[2][3], 3.0f);
}

TEST(Math_Matrix4, LookAt) {
    Vector3 eye(0, 0, 5);
    Vector3 target(0, 0, 0);
    Vector3 up(0, 1, 0);
    
    Matrix4x4 m = Matrix4x4::LookAt(eye, target, up);
    
    EXPECT_FLOAT_EQ(m.m[0][3], 0.0f);
    EXPECT_FLOAT_EQ(m.m[1][3], 0.0f);
    EXPECT_FLOAT_EQ(m.m[2][3], -5.0f);
}

TEST(Math_Matrix4, Perspective) {
    float fov = PI * 0.25f;
    float aspect = 16.0f / 9.0f;
    float nearPlane = 0.1f;
    float farPlane = 100.0f;
    
    Matrix4x4 m = Matrix4x4::Perspective(fov, aspect, nearPlane, farPlane);
    
    EXPECT_FLOAT_EQ(m.m[0][0], 1.0f / (aspect * std::tan(fov * 0.5f)));
    EXPECT_FLOAT_EQ(m.m[1][1], 1.0f / std::tan(fov * 0.5f));
    EXPECT_FLOAT_EQ(m.m[2][2], farPlane / (farPlane - nearPlane));
    EXPECT_FLOAT_EQ(m.m[2][3], -farPlane * nearPlane / (farPlane - nearPlane));
    EXPECT_FLOAT_EQ(m.m[3][2], 1.0f);
}

TEST(Math_Matrix4, Orthographic) {
    float left = -10.0f, right = 10.0f;
    float bottom = -10.0f, top = 10.0f;
    float nearPlane = 0.1f, farPlane = 100.0f;
    
    Matrix4x4 m = Matrix4x4::Orthographic(left, right, bottom, top, nearPlane, farPlane);
    
    EXPECT_FLOAT_EQ(m.m[0][0], 0.1f);
    EXPECT_FLOAT_EQ(m.m[1][1], 0.1f);
    EXPECT_FLOAT_EQ(m.m[2][2], 1.0f / (farPlane - nearPlane));
}

TEST(Math_Matrix4, Multiply_MatrixWithMatrix) {
    Matrix4x4 a = Matrix4x4::Identity();
    Matrix4x4 b = Matrix4x4::Translation(Vector3(1, 2, 3));
    
    Matrix4x4 result = a * b;
    
    EXPECT_FLOAT_EQ(result.m[0][3], 1.0f);
    EXPECT_FLOAT_EQ(result.m[1][3], 2.0f);
    EXPECT_FLOAT_EQ(result.m[2][3], 3.0f);
}

TEST(Math_Matrix4, Multiply_MatrixWithVector4) {
    Matrix4x4 m = Matrix4x4::Translation(Vector3(1, 2, 3));
    Vector4 v(1, 1, 1, 1);
    
    Vector4 result = m * v;
    
    EXPECT_FLOAT_EQ(result.x, 2.0f);
    EXPECT_FLOAT_EQ(result.y, 3.0f);
    EXPECT_FLOAT_EQ(result.z, 4.0f);
}

TEST(Math_Matrix4, MultiplyPoint) {
    Matrix4x4 m = Matrix4x4::Translation(Vector3(1, 2, 3));
    Vector3 p(0, 0, 0);
    
    Vector3 result = m.MultiplyPoint(p);
    
    EXPECT_FLOAT_EQ(result.x, 1.0f);
    EXPECT_FLOAT_EQ(result.y, 2.0f);
    EXPECT_FLOAT_EQ(result.z, 3.0f);
}

TEST(Math_Matrix4, MultiplyVector) {
    Matrix4x4 m = Matrix4x4::Scale(Vector3(2, 2, 2));
    Vector3 v(1, 1, 1);
    
    Vector3 result = m.MultiplyVector(v);
    
    EXPECT_FLOAT_EQ(result.x, 2.0f);
    EXPECT_FLOAT_EQ(result.y, 2.0f);
    EXPECT_FLOAT_EQ(result.z, 2.0f);
}

TEST(Math_Matrix4, Transpose) {
    Matrix4x4 m;
    m.m[0][1] = 1.0f;
    m.m[1][2] = 2.0f;
    m.m[2][3] = 3.0f;
    
    Matrix4x4 t = m.Transpose();
    
    EXPECT_FLOAT_EQ(t.m[1][0], 1.0f);
    EXPECT_FLOAT_EQ(t.m[2][1], 2.0f);
    EXPECT_FLOAT_EQ(t.m[3][2], 3.0f);
}

TEST(Math_Matrix4, Inverse_Identity_ReturnsIdentity) {
    Matrix4x4 id = Matrix4x4::Identity();
    Matrix4x4 inv = id.Inverse();
    
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            EXPECT_NEAR(inv.m[i][j], id.m[i][j], 1e-5f);
        }
    }
}

TEST(Math_Matrix4, Inverse_Translation) {
    Matrix4x4 m = Matrix4x4::Translation(Vector3(1, 2, 3));
    Matrix4x4 inv = m.Inverse();
    
    Vector3 p(0, 0, 0);
    Vector3 transformed = m.MultiplyPoint(p);
    Vector3 recovered = inv.MultiplyPoint(transformed);
    
    EXPECT_NEAR(recovered.x, 0.0f, 1e-5f);
    EXPECT_NEAR(recovered.y, 0.0f, 1e-5f);
    EXPECT_NEAR(recovered.z, 0.0f, 1e-5f);
}

TEST(Math_Matrix4, Inverse_OfInverse_EqualsOriginal) {
    Matrix4x4 original = Matrix4x4::TRS(
        Vector3(1, 2, 3),
        Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f),
        Vector3(2, 2, 2)
    );
    
    Matrix4x4 inverted = original.Inverse();
    Matrix4x4 recovered = inverted.Inverse();
    
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            EXPECT_NEAR(original.m[i][j], recovered.m[i][j], 1e-4f);
        }
    }
}

TEST(Math_Matrix4, Determinant_Identity_EqualsOne) {
    Matrix4x4 id = Matrix4x4::Identity();
    EXPECT_FLOAT_EQ(id.Determinant(), 1.0f);
}

TEST(Math_Matrix4, GetTranslation) {
    Vector3 translation(1, 2, 3);
    Matrix4x4 m = Matrix4x4::Translation(translation);
    
    Vector3 result = m.GetTranslation();
    
    EXPECT_FLOAT_EQ(result.x, 1.0f);
    EXPECT_FLOAT_EQ(result.y, 2.0f);
    EXPECT_FLOAT_EQ(result.z, 3.0f);
}

TEST(Math_Matrix4, GetScale) {
    Vector3 scale(2, 3, 4);
    Matrix4x4 m = Matrix4x4::Scale(scale);
    
    Vector3 result = m.GetScale();
    
    EXPECT_NEAR(result.x, 2.0f, 1e-5f);
    EXPECT_NEAR(result.y, 3.0f, 1e-5f);
    EXPECT_NEAR(result.z, 4.0f, 1e-5f);
}

TEST(Math_Matrix4, Decompose_TRS) {
    Vector3 originalPos(1, 2, 3);
    Quaternion originalRot = Quaternion::FromAxisAngle(Vector3::Up(), PI * 0.5f);
    Vector3 originalScale(2, 2, 2);
    
    Matrix4x4 m = Matrix4x4::TRS(originalPos, originalRot, originalScale);
    
    Vector3 pos, scale;
    Quaternion rot;
    m.Decompose(pos, rot, scale);
    
    EXPECT_NEAR(pos.x, originalPos.x, 1e-4f);
    EXPECT_NEAR(pos.y, originalPos.y, 1e-4f);
    EXPECT_NEAR(pos.z, originalPos.z, 1e-4f);
    
    EXPECT_NEAR(scale.x, originalScale.x, 1e-4f);
    EXPECT_NEAR(scale.y, originalScale.y, 1e-4f);
    EXPECT_NEAR(scale.z, originalScale.z, 1e-4f);
}

TEST(Math_Matrix4, IndexOperator) {
    Matrix4x4 m;
    m[0][0] = 1.0f;
    
    EXPECT_FLOAT_EQ(m[0][0], 1.0f);
    EXPECT_FLOAT_EQ(m.m[0][0], 1.0f);
}

} // namespace