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

using Catch::Matchers::WithinAbs;

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

    SECTION("add") {
        const Vec3 sum = vec3_add(a, b);
        REQUIRE(compare_vec3(sum, {5.0, 7.0, 9.0}, D_TOL));
    }

    SECTION("sub") {
        const Vec3 diff = vec3_sub(b, a);
        REQUIRE(compare_vec3(diff, {3.0, 3.0, 3.0}, D_TOL));
    }

    SECTION("scale") {
        const Vec3 scaled = vec3_scale(a, 2.0);
        REQUIRE(compare_vec3(scaled, {2.0, 4.0, 6.0}, D_TOL));
    }

    SECTION("magnitude") {
        const double mag = vec3_magnitude(a);
        REQUIRE_THAT(mag, WithinAbs(sqrt(14.0), D_TOL));
    }

    SECTION("distance") {
        const double dist = vec3_distance(a, b);
        REQUIRE_THAT(dist, WithinAbs(sqrt(27.0), D_TOL));
    }

    SECTION("normalize") {
        const Vec3 unit = vec3_normalize(a);
        const double actual_mag = vec3_magnitude(unit);
        REQUIRE_THAT(actual_mag, WithinAbs(1.0, D_TOL));
    }

    SECTION("normalize zero vector") {
        const Vec3 zero = {0.0, 0.0, 0.0};
        const Vec3 result = vec3_normalize(zero);
        REQUIRE(compare_vec3(result, zero, D_TOL));
    }

    SECTION("dot product") {
        const double dot = vec3_dot(a, b);
        REQUIRE_THAT(dot, WithinAbs(32.0, D_TOL));
    }

    SECTION("cross product") {
        const Vec3 cross = vec3_cross(a, b);
        REQUIRE(compare_vec3(cross, {-3.0, 6.0, -3.0}, D_TOL));
    }
}

SCENARIO("Acceleration calculation", "[physics][acceleration]") {
    SECTION("F = ma") {
        const Vec3 force = {10.0, 20.0, 30.0};
        const double mass = 5.0;
        const Vec3 accel = calculate_acceleration(force, mass);
        REQUIRE_THAT(accel.x, WithinAbs(2.0, R_TOL));
        REQUIRE_THAT(accel.y, WithinAbs(4.0, R_TOL));
        REQUIRE_THAT(accel.z, WithinAbs(6.0, R_TOL));
    }

    SECTION("zero mass returns zero acceleration") {
        const Vec3 force = {10.0, 20.0, 30.0};
        const Vec3 accel = calculate_acceleration(force, 0.0);
        REQUIRE(compare_vec3(accel, {0.0, 0.0, 0.0}, R_TOL));
    }
}

SCENARIO("Matrix operations", "[physics][utilities][matrix]") {
    const Mat3 A = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0};
    const Vec3 v = {1.0, 2.0, 3.0};

    SECTION("identity matrix") {
        const Mat3 I = mat3_identity();
        const Mat3 result = mat3_multiply(A, I);
        REQUIRE(compare_vec3({result.m00, result.m11, result.m22},
                             {A.m00, A.m11, A.m22}, D_TOL));
    }

    SECTION("multiply vector") {
        const Vec3 result = mat3_multiply_vec3(A, v);
        REQUIRE(compare_vec3(result, {14.0, 32.0, 50.0}, D_TOL));
    }

    SECTION("multiply two matrices") {
        const Mat3 B = {9.0, 8.0, 7.0, 6.0, 5.0, 4.0, 3.0, 2.0, 1.0};
        const Mat3 result = mat3_multiply(A, B);
        REQUIRE_THAT(result.m00, WithinAbs(30.0, D_TOL));
        REQUIRE_THAT(result.m01, WithinAbs(24.0, D_TOL));
        REQUIRE_THAT(result.m02, WithinAbs(18.0, D_TOL));
        REQUIRE_THAT(result.m10, WithinAbs(84.0, D_TOL));
        REQUIRE_THAT(result.m11, WithinAbs(69.0, D_TOL));
        REQUIRE_THAT(result.m12, WithinAbs(54.0, D_TOL));
        REQUIRE_THAT(result.m20, WithinAbs(138.0, D_TOL));
        REQUIRE_THAT(result.m21, WithinAbs(114.0, D_TOL));
        REQUIRE_THAT(result.m22, WithinAbs(90.0, D_TOL));
    }

    SECTION("transpose") {
        const Mat3 T = mat3_transpose(A);
        REQUIRE_THAT(T.m00, WithinAbs(1.0, D_TOL));
        REQUIRE_THAT(T.m01, WithinAbs(4.0, D_TOL));
        REQUIRE_THAT(T.m02, WithinAbs(7.0, D_TOL));
        REQUIRE_THAT(T.m10, WithinAbs(2.0, D_TOL));
        REQUIRE_THAT(T.m11, WithinAbs(5.0, D_TOL));
        REQUIRE_THAT(T.m12, WithinAbs(8.0, D_TOL));
        REQUIRE_THAT(T.m20, WithinAbs(3.0, D_TOL));
        REQUIRE_THAT(T.m21, WithinAbs(6.0, D_TOL));
        REQUIRE_THAT(T.m22, WithinAbs(9.0, D_TOL));
    }

    SECTION("transpose of transpose is original") {
        const Mat3 T2 = mat3_transpose(mat3_transpose(A));
        REQUIRE_THAT(T2.m00, WithinAbs(A.m00, D_TOL));
        REQUIRE_THAT(T2.m01, WithinAbs(A.m01, D_TOL));
        REQUIRE_THAT(T2.m02, WithinAbs(A.m02, D_TOL));
        REQUIRE_THAT(T2.m10, WithinAbs(A.m10, D_TOL));
        REQUIRE_THAT(T2.m11, WithinAbs(A.m11, D_TOL));
        REQUIRE_THAT(T2.m12, WithinAbs(A.m12, D_TOL));
        REQUIRE_THAT(T2.m20, WithinAbs(A.m20, D_TOL));
        REQUIRE_THAT(T2.m21, WithinAbs(A.m21, D_TOL));
        REQUIRE_THAT(T2.m22, WithinAbs(A.m22, D_TOL));
    }
}

SCENARIO("Rotation matrices", "[physics][utilities][rotation]") {
    SECTION("rotate about Z by 90 degrees") {
        const Mat3 Rz = mat3_rotation_z(M_PI / 2);
        const Vec3 v = {1.0, 0.0, 0.0};
        const Vec3 result = mat3_multiply_vec3(Rz, v);
        REQUIRE(compare_vec3(result, {0.0, 1.0, 0.0}, D_TOL));
    }

    SECTION("rotate about X by 90 degrees") {
        const Mat3 Rx = mat3_rotation_x(M_PI / 2);
        const Vec3 v = {0.0, 1.0, 0.0};
        const Vec3 result = mat3_multiply_vec3(Rx, v);
        REQUIRE(compare_vec3(result, {0.0, 0.0, 1.0}, D_TOL));
    }

    SECTION("180 degree rotation") {
        const Mat3 Rz180 = mat3_rotation_z(M_PI);
        const Vec3 v = {1.0, 0.0, 0.0};
        const Vec3 result = mat3_multiply_vec3(Rz180, v);
        REQUIRE(compare_vec3(result, {-1.0, 0.0, 0.0}, D_TOL));
    }

    SECTION("360 degree rotation equals identity") {
        const Mat3 Rz360 = mat3_rotation_z(2.0 * M_PI);
        const Mat3 I = mat3_identity();
        REQUIRE(compare_vec3({Rz360.m00, Rz360.m11, Rz360.m22},
                             {I.m00, I.m11, I.m22}, D_TOL));
    }

    SECTION("negative angle equals equivalent positive rotation") {
        const Mat3 Rz_neg90 = mat3_rotation_z(-M_PI / 2);
        const Mat3 Rz_270 = mat3_rotation_z(3.0 * M_PI / 2);
        REQUIRE_THAT(Rz_neg90.m00, WithinAbs(Rz_270.m00, D_TOL));
        REQUIRE_THAT(Rz_neg90.m01, WithinAbs(Rz_270.m01, D_TOL));
        REQUIRE_THAT(Rz_neg90.m10, WithinAbs(Rz_270.m10, D_TOL));
        REQUIRE_THAT(Rz_neg90.m11, WithinAbs(Rz_270.m11, D_TOL));
    }

    SECTION("combined rotations that cancel") {
        const Mat3 Rz90 = mat3_rotation_z(M_PI / 2);
        const Mat3 Rz_neg90 = mat3_rotation_z(-M_PI / 2);
        const Mat3 combined = mat3_multiply(Rz_neg90, Rz90);
        const Mat3 I = mat3_identity();
        REQUIRE(compare_vec3({combined.m00, combined.m11, combined.m22},
                             {I.m00, I.m11, I.m22}, D_TOL));
    }

    SECTION("rotation matrix orthogonality") {
        const double angle = M_PI / 4;
        const Mat3 Rz = mat3_rotation_z(angle);
        const Mat3 Rz_T = mat3_transpose(Rz);
        const Mat3 product = mat3_multiply(Rz, Rz_T);
        const Mat3 I = mat3_identity();
        REQUIRE_THAT(product.m00, WithinAbs(I.m00, D_TOL));
        REQUIRE_THAT(product.m01, WithinAbs(I.m01, D_TOL));
        REQUIRE_THAT(product.m02, WithinAbs(I.m02, D_TOL));
        REQUIRE_THAT(product.m10, WithinAbs(I.m10, D_TOL));
        REQUIRE_THAT(product.m11, WithinAbs(I.m11, D_TOL));
        REQUIRE_THAT(product.m12, WithinAbs(I.m12, D_TOL));
        REQUIRE_THAT(product.m20, WithinAbs(I.m20, D_TOL));
        REQUIRE_THAT(product.m21, WithinAbs(I.m21, D_TOL));
        REQUIRE_THAT(product.m22, WithinAbs(I.m22, D_TOL));
    }

    SECTION("orbital rotation matrix with 90 deg inclination") {
        const Mat3 R = mat3_rotation_orbital(0.0, M_PI / 2, 0.0);
        const Vec3 v = {1.0, 0.0, 0.0};
        const Vec3 result = mat3_multiply_vec3(R, v);
        REQUIRE(compare_vec3(result, {1.0, 0.0, 0.0}, D_TOL));
    }
}

SCENARIO("compare_vec3 utility", "[physics][utilities][compare]") {
    SECTION("equal vectors within tolerance") {
        const Vec3 a = {1.0, 2.0, 3.0};
        const Vec3 b = {1.0 + 1e-13, 2.0 + 1e-13, 3.0 + 1e-13};
        REQUIRE(compare_vec3(a, b, D_TOL));
    }

    SECTION("equal vectors exactly") {
        const Vec3 a = {1.0, 2.0, 3.0};
        const Vec3 b = {1.0, 2.0, 3.0};
        REQUIRE(compare_vec3(a, b, 0.0));
    }

    SECTION("different vectors outside tolerance") {
        const Vec3 a = {1.0, 2.0, 3.0};
        const Vec3 b = {2.0, 2.0, 3.0};
        REQUIRE(!compare_vec3(a, b, 0.5));
    }
}