#include <catch2/catch_test_macros.hpp>
#include "../src/physics.h"
#include "../src/test_utilities.h"
#include <cmath>

TEST_CASE("Vector math utilities", "[utilities]") {
    Vec3 a = {1.0, 2.0, 3.0};
    Vec3 b = {4.0, 5.0, 6.0};

    SECTION("Vector addition") {
        Vec3 sum = vec3_add(a, b);
        REQUIRE(compare_double(sum.x, 5.0, 1e-10));
        REQUIRE(compare_double(sum.y, 7.0, 1e-10));
        REQUIRE(compare_double(sum.z, 9.0, 1e-10));
    }

    SECTION("Vector subtraction") {
        Vec3 diff = vec3_sub(b, a);
        REQUIRE(compare_double(diff.x, 3.0, 1e-10));
        REQUIRE(compare_double(diff.y, 3.0, 1e-10));
        REQUIRE(compare_double(diff.z, 3.0, 1e-10));
    }

    SECTION("Vector scaling") {
        Vec3 scaled = vec3_scale(a, 2.0);
        REQUIRE(compare_double(scaled.x, 2.0, 1e-10));
        REQUIRE(compare_double(scaled.y, 4.0, 1e-10));
        REQUIRE(compare_double(scaled.z, 6.0, 1e-10));
    }

    SECTION("Vector magnitude") {
        double mag = vec3_magnitude(a);
        REQUIRE(compare_double(mag, sqrt(14.0), 1e-10));
    }

    SECTION("Vector distance") {
        double dist = vec3_distance(a, b);
        REQUIRE(compare_double(dist, sqrt(27.0), 1e-10));
    }

    SECTION("Vector normalization") {
        Vec3 unit = vec3_normalize(a);
        double expected_mag = 1.0;
        double actual_mag = vec3_magnitude(unit);
        REQUIRE(compare_double(actual_mag, expected_mag, 1e-10));
    }

    SECTION("Vector dot product") {
        double dot = vec3_dot(a, b);
        REQUIRE(compare_double(dot, 32.0, 1e-10));
    }

    SECTION("Vector cross product") {
        Vec3 cross = vec3_cross(a, b);
        REQUIRE(compare_double(cross.x, -3.0, 1e-10));
        REQUIRE(compare_double(cross.y, 6.0, 1e-10));
        REQUIRE(compare_double(cross.z, -3.0, 1e-10));
    }
}

TEST_CASE("Acceleration calculation", "[physics][acceleration]") {
    Vec3 force = {10.0, 20.0, 30.0};
    double mass = 5.0;

    Vec3 accel = calculate_acceleration(force, mass);

    REQUIRE(compare_double(accel.x, 2.0, 1e-10));
    REQUIRE(compare_double(accel.y, 4.0, 1e-10));
    REQUIRE(compare_double(accel.z, 6.0, 1e-10));
}

TEST_CASE("Gravitational acceleration evaluation", "[physics][gravity]") {
    Vec3 position = {1.0, 0.0, 0.0};
    double body_mass = 1000.0;
    double parent_mass = 1e10;

    Vec3 accel = evaluate_acceleration(position, body_mass, parent_mass);

    double expected_magnitude = G * parent_mass / (1.0 * 1.0);
    REQUIRE(compare_double(vec3_magnitude(accel), expected_magnitude, 1e-10));
    REQUIRE(compare_double(accel.x, -expected_magnitude, 1e-10));
}

TEST_CASE("RK4 integration step", "[physics][rk4]") {
    Vec3 position = {0.0, 1.0, 0.0};
    Vec3 velocity = {sqrt(G * 1e10 / 1.0), 0.0, 0.0};
    double dt = 1.0;
    double body_mass = 1.0;
    double parent_mass = 1e10;

    double initial_distance = vec3_magnitude(position);

    rk4_step(&position, &velocity, dt, body_mass, parent_mass);

    double final_distance = vec3_magnitude(position);

    REQUIRE(final_distance > 0.9 * initial_distance);
    REQUIRE(final_distance < 1.1 * initial_distance);
}

TEST_CASE("Matrix identity", "[matrix][identity]") {
    Mat3 I = mat3_identity();

    REQUIRE(compare_double(I.m00, 1.0, 1e-10));
    REQUIRE(compare_double(I.m01, 0.0, 1e-10));
    REQUIRE(compare_double(I.m02, 0.0, 1e-10));
    REQUIRE(compare_double(I.m10, 0.0, 1e-10));
    REQUIRE(compare_double(I.m11, 1.0, 1e-10));
    REQUIRE(compare_double(I.m12, 0.0, 1e-10));
    REQUIRE(compare_double(I.m20, 0.0, 1e-10));
    REQUIRE(compare_double(I.m21, 0.0, 1e-10));
    REQUIRE(compare_double(I.m22, 1.0, 1e-10));
}

TEST_CASE("Matrix-vector multiplication", "[matrix][vector]") {
    Mat3 m = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0};
    Vec3 v = {1.0, 2.0, 3.0};

    Vec3 result = mat3_multiply_vec3(m, v);

    REQUIRE(compare_double(result.x, 14.0, 1e-10));
    REQUIRE(compare_double(result.y, 32.0, 1e-10));
    REQUIRE(compare_double(result.z, 50.0, 1e-10));
}

TEST_CASE("Matrix multiplication with identity", "[matrix][multiply]") {
    Mat3 A = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0};
    Mat3 I = mat3_identity();

    Mat3 result = mat3_multiply(A, I);

    REQUIRE(compare_double(result.m00, A.m00, 1e-10));
    REQUIRE(compare_double(result.m01, A.m01, 1e-10));
    REQUIRE(compare_double(result.m02, A.m02, 1e-10));
    REQUIRE(compare_double(result.m10, A.m10, 1e-10));
    REQUIRE(compare_double(result.m11, A.m11, 1e-10));
    REQUIRE(compare_double(result.m12, A.m12, 1e-10));
    REQUIRE(compare_double(result.m20, A.m20, 1e-10));
    REQUIRE(compare_double(result.m21, A.m21, 1e-10));
    REQUIRE(compare_double(result.m22, A.m22, 1e-10));
}

TEST_CASE("Rotation about Z axis", "[matrix][rotation]") {
    double angle = M_PI / 2;  // 90 degrees
    Mat3 Rz = mat3_rotation_z(angle);
    Vec3 v = {1.0, 0.0, 0.0};

    Vec3 result = mat3_multiply_vec3(Rz, v);

    REQUIRE(compare_double(result.x, 0.0, 1e-10));
    REQUIRE(compare_double(result.y, 1.0, 1e-10));
    REQUIRE(compare_double(result.z, 0.0, 1e-10));
}

TEST_CASE("Rotation about X axis", "[matrix][rotation]") {
    double angle = M_PI / 2;  // 90 degrees
    Mat3 Rx = mat3_rotation_x(angle);
    Vec3 v = {0.0, 1.0, 0.0};

    Vec3 result = mat3_multiply_vec3(Rx, v);

    REQUIRE(compare_double(result.x, 0.0, 1e-10));
    REQUIRE(compare_double(result.y, 0.0, 1e-10));
    REQUIRE(compare_double(result.z, 1.0, 1e-10));
}

TEST_CASE("Rotation edge cases", "[matrix][rotation][edge]") {
    SECTION("180 degree rotation") {
        Mat3 Rz180 = mat3_rotation_z(M_PI);
        Vec3 v = {1.0, 0.0, 0.0};
        Vec3 result = mat3_multiply_vec3(Rz180, v);

        REQUIRE(compare_double(result.x, -1.0, 1e-10));
        REQUIRE(compare_double(result.y, 0.0, 1e-10));
        REQUIRE(compare_double(result.z, 0.0, 1e-10));
    }

    SECTION("360 degree rotation equals identity") {
        Mat3 Rz360 = mat3_rotation_z(2.0 * M_PI);
        Mat3 I = mat3_identity();

        REQUIRE(compare_double(Rz360.m00, I.m00, 1e-10));
        REQUIRE(compare_double(Rz360.m11, I.m11, 1e-10));
        REQUIRE(compare_double(Rz360.m22, I.m22, 1e-10));
    }

    SECTION("Negative angle equals positive rotation") {
        Mat3 Rz_neg90 = mat3_rotation_z(-M_PI / 2);
        Mat3 Rz_270 = mat3_rotation_z(3.0 * M_PI / 2);

        REQUIRE(compare_double(Rz_neg90.m00, Rz_270.m00, 1e-10));
        REQUIRE(compare_double(Rz_neg90.m01, Rz_270.m01, 1e-10));
        REQUIRE(compare_double(Rz_neg90.m10, Rz_270.m10, 1e-10));
        REQUIRE(compare_double(Rz_neg90.m11, Rz_270.m11, 1e-10));
    }

    SECTION("Combined rotations that cancel") {
        Mat3 Rz90 = mat3_rotation_z(M_PI / 2);
        Mat3 Rz_neg90 = mat3_rotation_z(-M_PI / 2);
        Mat3 combined = mat3_multiply(Rz_neg90, Rz90);
        Mat3 I = mat3_identity();

        REQUIRE(compare_double(combined.m00, I.m00, 1e-10));
        REQUIRE(compare_double(combined.m11, I.m11, 1e-10));
        REQUIRE(compare_double(combined.m22, I.m22, 1e-10));
    }
}

TEST_CASE("Rotation matrix orthogonality", "[matrix][rotation][validation]") {
    double angle = M_PI / 4;  // 45 degrees
    Mat3 Rz = mat3_rotation_z(angle);

    Mat3 Rz_T = {Rz.m00, Rz.m10, Rz.m20,
                 Rz.m01, Rz.m11, Rz.m21,
                 Rz.m02, Rz.m12, Rz.m22};

    Mat3 product = mat3_multiply(Rz, Rz_T);
    Mat3 I = mat3_identity();

    REQUIRE(compare_double(product.m00, I.m00, 1e-10));
    REQUIRE(compare_double(product.m01, I.m01, 1e-10));
    REQUIRE(compare_double(product.m02, I.m02, 1e-10));
    REQUIRE(compare_double(product.m10, I.m10, 1e-10));
    REQUIRE(compare_double(product.m11, I.m11, 1e-10));
    REQUIRE(compare_double(product.m12, I.m12, 1e-10));
    REQUIRE(compare_double(product.m20, I.m20, 1e-10));
    REQUIRE(compare_double(product.m21, I.m21, 1e-10));
    REQUIRE(compare_double(product.m22, I.m22, 1e-10));
}

TEST_CASE("Orbital rotation matrix", "[matrix][orbital]") {
    double omega = 0.0;
    double i = M_PI / 2;  // 90 degrees inclination
    double Omega = 0.0;

    Mat3 R = mat3_rotation_orbital(omega, i, Omega);
    Vec3 v = {1.0, 0.0, 0.0};

    Vec3 result = mat3_multiply_vec3(R, v);

    REQUIRE(compare_double(result.x, 1.0, 1e-10));
    REQUIRE(compare_double(result.y, 0.0, 1e-10));
    REQUIRE(compare_double(result.z, 0.0, 1e-10));
}