#include <catch2/catch_test_macros.hpp>
#include "../src/physics.h"
#include "../src/orbital_mechanics.h"
#include "../src/simulation.h"
#include "../src/config_loader.h"
#include <cmath>
#include <limits>

const double ELEMENT_TOLERANCE = 1.0e-6;
const double VELOCITY_TOLERANCE = 1.0e-3;

TEST_CASE("Perfect circle (e=0)", "[precision][boundary][circle]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 3, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_precision_boundaries.toml"));

    Spacecraft* circle = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

    INFO("Testing circular orbit: e=" << circle->orbit.eccentricity);

    Vec3 pos_1;
    Vec3 vel_1;
    orbital_elements_to_cartesian(circle->orbit, earth->mass, &pos_1, &vel_1);

    double r_1 = vec3_magnitude(pos_1);
    double v_1 = vec3_magnitude(vel_1);

    INFO("Radius: " << r_1 << " m");
    INFO("Velocity: " << v_1 << " m/s");

    double expected_r = circle->orbit.semi_major_axis;
    double mu = G * earth->mass;
    double expected_v = sqrt(mu / expected_r);

    double r_error = fabs(r_1 - expected_r);
    double v_error = fabs(v_1 - expected_v);

    INFO("Expected radius: " << expected_r << " m");
    INFO("Expected velocity: " << expected_v << " m/s");
    INFO("Radius error: " << r_error << " m");
    INFO("Velocity error: " << v_error << " m/s");

    REQUIRE(r_error < fabs(expected_r) * ELEMENT_TOLERANCE);
    REQUIRE(v_error < VELOCITY_TOLERANCE);

    double vis_viva = sqrt(mu * (2.0 / r_1 - 1.0 / circle->orbit.semi_major_axis));
    double vis_viva_error = fabs(v_1 - vis_viva);

    INFO("Vis-viva velocity: " << vis_viva << " m/s");
    INFO("Vis-viva error: " << vis_viva_error << " m/s");

    REQUIRE(vis_viva_error < VELOCITY_TOLERANCE);

    destroy_simulation(sim);
}

TEST_CASE("Polar orbit (i=π/2)", "[precision][boundary][polar]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 3, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_precision_boundaries.toml"));

    Spacecraft* polar = &sim->spacecraft[1];
    CelestialBody* earth = &sim->bodies[0];

    INFO("Testing polar orbit: i=" << polar->orbit.inclination << " rad (" << polar->orbit.inclination * 180.0 / M_PI << "°)");

    Vec3 pos;
    Vec3 vel;
    orbital_elements_to_cartesian(polar->orbit, earth->mass, &pos, &vel);

    INFO("Position: (" << pos.x << ", " << pos.y << ", " << pos.z << ") m");
    INFO("Velocity: (" << vel.x << ", " << vel.y << ", " << vel.z << ") m/s");

    double r = vec3_magnitude(pos);
    double v = vec3_magnitude(vel);

    double expected_r = polar->orbit.semi_major_axis * (1.0 - polar->orbit.eccentricity * polar->orbit.eccentricity) / (1.0 + polar->orbit.eccentricity);

    INFO("Expected radius: " << expected_r << " m");
    INFO("Actual radius: " << r << " m");

    double r_error = fabs(r - expected_r);

    REQUIRE(r_error < fabs(expected_r) * ELEMENT_TOLERANCE);

    destroy_simulation(sim);
}

TEST_CASE("Retrograde orbit (i=π)", "[precision][boundary][retrograde]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 3, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_precision_boundaries.toml"));

    Spacecraft* retrograde = &sim->spacecraft[2];
    CelestialBody* earth = &sim->bodies[0];

    INFO("Testing retrograde orbit: i=" << retrograde->orbit.inclination << " rad (" << retrograde->orbit.inclination * 180.0 / M_PI << "°)");

    Vec3 pos;
    Vec3 vel;
    orbital_elements_to_cartesian(retrograde->orbit, earth->mass, &pos, &vel);

    double r = vec3_magnitude(pos);
    double v = vec3_magnitude(vel);

    INFO("Radius: " << r << " m");
    INFO("Velocity: " << v << " m/s");

    double expected_r = retrograde->orbit.semi_major_axis * (1.0 - retrograde->orbit.eccentricity * retrograde->orbit.eccentricity) / (1.0 + retrograde->orbit.eccentricity);
    double mu = G * earth->mass;
    double expected_v = sqrt(mu * (2.0 / r - 1.0 / retrograde->orbit.semi_major_axis));

    double r_error = fabs(r - expected_r);
    double v_error = fabs(v - expected_v);

    INFO("Expected radius: " << expected_r << " m");
    INFO("Expected velocity: " << expected_v << " m/s");
    INFO("Radius error: " << r_error << " m");
    INFO("Velocity error: " << v_error << " m/s");

    REQUIRE(r_error < fabs(expected_r) * ELEMENT_TOLERANCE);
    REQUIRE(v_error < VELOCITY_TOLERANCE);

    destroy_simulation(sim);
}

TEST_CASE("Inclination at 0°, 90°, 180°", "[precision][boundary][inclination]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 3, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_precision_boundaries.toml"));

    double expected_inclinations[] = {0.0, M_PI / 2.0, M_PI};

    for (int i = 0; i < 3; i++) {
        Spacecraft* craft = &sim->spacecraft[i];
        double expected_i = expected_inclinations[i];

        INFO("Spacecraft " << i << ": i=" << craft->orbit.inclination << " rad (" << craft->orbit.inclination * 180.0 / M_PI << "°)");

        double i_error = fabs(craft->orbit.inclination - expected_i);

        INFO("  Expected inclination: " << expected_i << " rad");
        INFO("  Inclination error: " << i_error << " rad");

        REQUIRE(i_error < ELEMENT_TOLERANCE);
    }

    destroy_simulation(sim);
}

TEST_CASE("Semi-major axis sign change", "[precision][boundary][semi_major]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 3, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_precision_boundaries.toml"));

    for (int i = 0; i < 3; i++) {
        Spacecraft* craft = &sim->spacecraft[i];
        double e = craft->orbit.eccentricity;
        double a = craft->orbit.semi_major_axis;

        INFO("Spacecraft " << i << ": e=" << e << ", a=" << a << " m");

        if (e < 1.0) {
            INFO("  Elliptical orbit: a > 0");
            REQUIRE(a > 0.0);
        } else if (fabs(e - 1.0) < 1.0e-6) {
            INFO("  Parabolic orbit: near-circular");
        } else {
            INFO("  Hyperbolic orbit: a < 0");
            REQUIRE(a < 0.0);
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Angular momentum conservation", "[precision][boundary][angular_momentum]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 3, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_precision_boundaries.toml"));

    Spacecraft* craft = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

    double true_anomalies[] = {0.0, M_PI / 4.0, M_PI / 2.0, 3.0 * M_PI / 4.0, M_PI};

    Vec3 initial_h = {0.0, 0.0, 0.0};

    for (int i = 0; i < 5; i++) {
        double nu = true_anomalies[i];
        craft->orbit.true_anomaly = nu;

        Vec3 pos;
        Vec3 vel;
        orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos, &vel);

        Vec3 h = vec3_cross(pos, vel);
        double h_mag = vec3_magnitude(h);

        INFO("ν=" << nu << " rad: |h|=" << h_mag << " m²/s");

        if (i == 0) {
            initial_h = h;
        } else {
            double h_error = vec3_distance(h, initial_h);
            double relative_error = h_error / h_mag;

            INFO("  Angular momentum error: " << h_error << " m²/s (" << relative_error * 100.0 << "%)");

            REQUIRE(relative_error < ELEMENT_TOLERANCE);
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Zero/very small radius or velocity", "[precision][boundary][zero]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 3, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_precision_boundaries.toml"));

    Spacecraft* craft = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

    Vec3 pos;
    Vec3 vel;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos, &vel);

    double r = vec3_magnitude(pos);
    double v = vec3_magnitude(vel);

    INFO("Radius: " << r << " m");
    INFO("Velocity: " << v << " m/s");

    REQUIRE(r > earth->radius);
    REQUIRE(v > 0.0);

    Vec3 r_vec = vec3_normalize(pos);
    Vec3 v_vec = vec3_normalize(vel);

    double r_dot_v = vec3_dot(r_vec, v_vec);

    INFO("r̂ · v̂: " << r_dot_v);

    REQUIRE(r_dot_v > -1.0);
    REQUIRE(r_dot_v < 1.0);

    destroy_simulation(sim);
}