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

using Catch::Matchers::WithinAbs;

SCENARIO("Molniya orbit position at multiple true anomalies",
         "[inclined][molniya][position]") {
    const double TIME_STEP = 60.0;
    const double SEMI_MAJOR_AXIS = 26540000.0;
    const double ECCENTRICITY = 0.74;

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_inclined_orbits.toml"));

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

    auto check_radius_at_nu = [&](double nu, double expected_r) {
        molniya->orbit.true_anomaly = nu;
        initialize_orbital_objects(sim);

        double actual_r = vec3_magnitude(vec3_sub(molniya->global_position, earth->global_position));
        INFO("nu: " << nu << " rad, expected r: " << expected_r << " m, actual r: " << actual_r << " m");
        REQUIRE_THAT(actual_r, WithinAbs(expected_r, 10000.0));
    };

    SECTION("Perigee (nu = 0)") {
        check_radius_at_nu(0.0, SEMI_MAJOR_AXIS * (1.0 - ECCENTRICITY));
    }
    SECTION("90 degrees (nu = pi/2)") {
        double expected_r = SEMI_MAJOR_AXIS * (1.0 - ECCENTRICITY * ECCENTRICITY) /
                            (1.0 + ECCENTRICITY * cos(M_PI / 2.0));
        check_radius_at_nu(M_PI / 2.0, expected_r);
    }
    SECTION("Apogee (nu = pi)") {
        check_radius_at_nu(M_PI, SEMI_MAJOR_AXIS * (1.0 + ECCENTRICITY));
    }
    SECTION("270 degrees (nu = 3pi/2)") {
        double expected_r = SEMI_MAJOR_AXIS * (1.0 - ECCENTRICITY * ECCENTRICITY) /
                            (1.0 + ECCENTRICITY * cos(3.0 * M_PI / 2.0));
        check_radius_at_nu(3.0 * M_PI / 2.0, expected_r);
    }

    destroy_simulation(sim);
}

SCENARIO("Molniya orbit propagation to apogee",
         "[inclined][molniya][propagation]") {
    const double TIME_STEP = 60.0;
    const double G_CONST = 6.67430e-11;
    const double EARTH_MASS = 5.972e24;
    const double MU = G_CONST * EARTH_MASS;

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_inclined_orbits.toml"));

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

    const double a = molniya->orbit.semi_major_axis;
    const double expected_apogee_r = a * (1.0 + molniya->orbit.eccentricity);
    const double theoretical_half_period = M_PI * sqrt(a * a * a / MU);

    INFO("Theoretical half period: " << theoretical_half_period << " s");
    INFO("Expected apogee radius: " << expected_apogee_r << " m");

    auto propagate_to_half_period = [&]() -> double {
        double target_time = theoretical_half_period;
        while (sim->time < target_time) {
            update_simulation(sim);
        }
        return vec3_magnitude(vec3_sub(molniya->global_position, earth->global_position));
    };

    SECTION("After half period, craft reaches apogee") {
        const double actual_r = propagate_to_half_period();
        INFO("Actual radius at half period: " << actual_r << " m");
        REQUIRE_THAT(actual_r, WithinAbs(expected_apogee_r, 100000.0));
    }

    destroy_simulation(sim);
}

SCENARIO("Generic inclined orbit - z-coordinate and radius sanity",
         "[inclined][generic]") {
    const double TIME_STEP = 60.0;
    const double SEMI_MAJOR_AXIS = 10000000.0;
    const double ECCENTRICITY = 0.5;
    const double INCLINATION_DEG = 45.0;
    const double INCLINATION_RAD = INCLINATION_DEG * M_PI / 180.0;
    const double ARGUMENT_OF_PERIAPSIS = M_PI / 2.0;

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_inclined_orbits.toml"));

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

    craft->orbit.semi_major_axis = SEMI_MAJOR_AXIS;
    craft->orbit.eccentricity = ECCENTRICITY;
    craft->orbit.true_anomaly = 0.0;
    craft->orbit.inclination = INCLINATION_RAD;
    craft->orbit.longitude_of_ascending_node = 0.0;
    craft->orbit.argument_of_periapsis = ARGUMENT_OF_PERIAPSIS;
    initialize_orbital_objects(sim);

    auto check_z_nonzero = [&]() {
        double z = craft->global_position.z;
        INFO("Z-coordinate: " << z << " m");
        REQUIRE_THAT(z, !WithinAbs(0.0, 0.001));
    };

    auto check_radius = [&]() {
        double orbital_radius = vec3_magnitude(vec3_sub(craft->global_position, earth->global_position));
        double position_mag = vec3_magnitude(craft->global_position);
        double error = fabs(position_mag - orbital_radius);
        INFO("Position magnitude: " << position_mag << " m, orbital radius: " << orbital_radius << " m, error: " << error << " m");
        REQUIRE_THAT(error, WithinAbs(0.0, 10000.0));
    };

    SECTION("Z-coordinate is non-zero for inclined orbit") { check_z_nonzero(); }
    SECTION("Position magnitude matches orbital radius") { check_radius(); }

    destroy_simulation(sim);
}

SCENARIO("Inclination parameter preserved through config loading",
         "[inclined][config]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_inclined_orbits.toml"));

    Spacecraft* molniya = &sim->spacecraft[0];

    INFO("Loaded inclination: " << (molniya->orbit.inclination * 180.0 / M_PI) << " degrees");
    REQUIRE_THAT(molniya->orbit.inclination, WithinAbs(1.107, 0.01));

    destroy_simulation(sim);
}