#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);
}