#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>

const double VELOCITY_TOLERANCE = 1.0e-6;
const double POSITION_TOLERANCE = 1.0e3;

TEST_CASE("Highly eccentric orbit (e=0.99)", "[extreme][eccentricity][high]") {
    const double TIME_STEP = 60.0;

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

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

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

    INFO("Testing spacecraft with e=" << high_e->orbit.eccentricity);

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

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

    double expected_r_perigee = high_e->orbit.semi_major_axis * (1.0 - high_e->orbit.eccentricity);
    double expected_r_apogee = high_e->orbit.semi_major_axis * (1.0 + high_e->orbit.eccentricity);

    INFO("Semi-major axis: " << high_e->orbit.semi_major_axis << " m");
    INFO("Eccentricity: " << high_e->orbit.eccentricity);
    INFO("Radius: " << r << " m");
    INFO("Velocity: " << v << " m/s");
    INFO("Expected perigee: " << expected_r_perigee << " m");
    INFO("Expected apogee: " << expected_r_apogee << " m");

    REQUIRE(r >= expected_r_perigee * 0.9);
    REQUIRE(r <= expected_r_apogee * 1.1);
    REQUIRE(v > 0.0);

    destroy_simulation(sim);
}

TEST_CASE("Near-parabolic orbit (e=0.9999)", "[extreme][eccentricity][near_parabolic]") {
    const double TIME_STEP = 60.0;

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

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

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

    INFO("Testing spacecraft with e=" << near_parabolic->orbit.eccentricity);

    Vec3 pos_perigee;
    Vec3 vel_perigee;
    near_parabolic->orbit.true_anomaly = 0.0;
    orbital_elements_to_cartesian(near_parabolic->orbit, earth->mass, &pos_perigee, &vel_perigee);

    double r_perigee = vec3_magnitude(pos_perigee);
    double v_perigee = vec3_magnitude(vel_perigee);

    Vec3 pos_apogee;
    Vec3 vel_apogee;
    near_parabolic->orbit.true_anomaly = M_PI;
    orbital_elements_to_cartesian(near_parabolic->orbit, earth->mass, &pos_apogee, &vel_apogee);

    double r_apogee = vec3_magnitude(pos_apogee);
    double v_apogee = vec3_magnitude(vel_apogee);

    double expected_r_perigee = near_parabolic->orbit.semi_major_axis * (1.0 - near_parabolic->orbit.eccentricity);
    double expected_r_apogee = near_parabolic->orbit.semi_major_axis * (1.0 + near_parabolic->orbit.eccentricity);

    INFO("Perigee:");
    INFO("  Radius: " << r_perigee << " m (expected: " << expected_r_perigee << " m)");
    INFO("  Velocity: " << v_perigee << " m/s");

    INFO("Apogee:");
    INFO("  Radius: " << r_apogee << " m (expected: " << expected_r_apogee << " m)");
    INFO("  Velocity: " << v_apogee << " m/s");

    double r_perigee_error = fabs(r_perigee - expected_r_perigee);
    double r_apogee_error = fabs(r_apogee - expected_r_apogee);

    REQUIRE(r_perigee_error < POSITION_TOLERANCE);
    REQUIRE(r_apogee_error < POSITION_TOLERANCE);
    REQUIRE(v_perigee > v_apogee);
    REQUIRE(r_apogee > r_perigee);

    destroy_simulation(sim);
}

TEST_CASE("Near-parabolic boundary (e=1.0001)", "[extreme][eccentricity][boundary]") {
    const double TIME_STEP = 60.0;

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

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

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

    INFO("Testing spacecraft with e=" << hyperbolic->orbit.eccentricity);

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

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

    double mu = G * earth->mass;
    double a = hyperbolic->orbit.semi_major_axis;
    double escape_velocity = sqrt(2.0 * mu / r);
    double circular_velocity = sqrt(mu / r);

    INFO("Radius: " << r << " m");
    INFO("Velocity: " << v << " m/s");
    INFO("Escape velocity: " << escape_velocity << " m/s");
    INFO("Circular velocity: " << circular_velocity << " m/s");
    INFO("Semi-major axis: " << a << " m");

    double expected_v_squared = mu * (2.0 / r - 1.0 / a);
    double expected_v = sqrt(expected_v_squared);

    double v_error = fabs(v - expected_v);
    double relative_error = v_error / expected_v;

    INFO("Expected velocity: " << expected_v << " m/s");
    INFO("Velocity error: " << v_error << " m/s (" << relative_error * 100.0 << "%)");

    REQUIRE(relative_error < VELOCITY_TOLERANCE);
    REQUIRE(v > escape_velocity * 0.9);
    REQUIRE(a < 0.0);

    destroy_simulation(sim);
}

TEST_CASE("Velocity magnitude accuracy for extreme eccentricities", "[extreme][eccentricity][velocity]") {
    const double TIME_STEP = 60.0;

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

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

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

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

        INFO("Spacecraft " << i << ": e=" << craft->orbit.eccentricity);

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

        for (int j = 0; j < 4; j++) {
            double nu = true_anomalies[j];

            // For hyperbolic orbits (e > 1), skip invalid true anomalies
            // Valid range: |ν| < arccos(-1/e)
            if (craft->orbit.eccentricity > 1.0) {
                double max_nu = acos(-1.0 / craft->orbit.eccentricity);
                if (fabs(nu) >= max_nu) {
                    INFO("  ν=" << nu << " rad: skipped (exceeds hyperbolic limit ±" << max_nu << " rad)");
                    continue;
                }
            }

            craft->orbit.true_anomaly = nu;

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

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

            double a = craft->orbit.semi_major_axis;
            double mu = G * earth->mass;

            double expected_v_squared = mu * (2.0 / r - 1.0 / a);

            if (expected_v_squared > 0.0) {
                double expected_v = sqrt(expected_v_squared);
                double v_error = fabs(v - expected_v);
                double relative_error = v_error / expected_v;

                INFO("  ν=" << nu << " rad: v=" << v << " m/s, error=" << relative_error * 100.0 << "%");

                REQUIRE(relative_error < VELOCITY_TOLERANCE * 10.0);
            }
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Period calculation (or lack thereof) for e≥1", "[extreme][eccentricity][period]") {
    const double TIME_STEP = 60.0;

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

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

    Spacecraft* high_e = &sim->spacecraft[0];
    Spacecraft* near_parabolic = &sim->spacecraft[1];
    Spacecraft* hyperbolic = &sim->spacecraft[2];

    double a_e = high_e->orbit.semi_major_axis;
    double a_near = near_parabolic->orbit.semi_major_axis;
    double a_h = hyperbolic->orbit.semi_major_axis;

    INFO("Highly eccentric (e=0.99): a=" << a_e << " m");
    INFO("Near-parabolic (e=0.9999): a=" << a_near << " m");
    INFO("Hyperbolic (e=1.0001): a=" << a_h << " m");

    REQUIRE(a_e > 0.0);
    REQUIRE(a_near > 0.0);
    REQUIRE(a_h < 0.0);

    destroy_simulation(sim);
}