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

using Catch::Matchers::WithinAbs;

// Helper: propagate orbit for N full periods, return final pos/vel
static void propagate_n_periods(SimulationState* sim, int craft_idx, int parent_idx,
                                 int num_periods, double dt,
                                 Vec3& out_pos, Vec3& out_vel) {
    const double parent_mass = sim->bodies[parent_idx].mass;
    OrbitalElements current = sim->spacecraft[craft_idx].orbit;
    double period = 2.0 * M_PI * sqrt(pow(current.semi_major_axis, 3.0) / (G * parent_mass));
    double total_time = num_periods * period;
    int steps = (int)(total_time / dt);

    for (int s = 0; s < steps; s++) {
        current = propagate_orbital_elements(current, dt, parent_mass);
    }
    orbital_elements_to_cartesian(current, parent_mass, &out_pos, &out_vel);
}

// Helper: compute orbital energy from state vectors
static double compute_energy(const Vec3& pos, const Vec3& vel,
                              double craft_mass, double parent_mass) {
    double r = vec3_magnitude(pos);
    double v2 = vel.x * vel.x + vel.y * vel.y + vel.z * vel.z;
    return 0.5 * craft_mass * v2 - G * craft_mass * parent_mass / r;
}

// Helper: compute orbital period
static double compute_period(double semi_major_axis, double parent_mass) {
    return 2.0 * M_PI * sqrt(pow(semi_major_axis, 3.0) / (G * parent_mass));
}

SCENARIO("Analytical propagation preserves energy across extreme timescales",
         "[extreme][timescales]") {
    const double TIME_STEP = 3600.0;

    const double PERIOD_HOURS_TOL = 0.0002;
    const double PROP_POS_TOL = 1e-4;

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

    // --- Fixture: LEO spacecraft ---
    const int LEO_IDX = 0;
    const int PARENT_EARTH = 0;
    Spacecraft* leo_craft = &sim->spacecraft[LEO_IDX];
    CelestialBody* earth = &sim->bodies[PARENT_EARTH];
    const double leo_period = compute_period(leo_craft->orbit.semi_major_axis, earth->mass);
    INFO("LEO period: " << leo_period << " s (" << leo_period / 60.0 << " min)");

    SECTION("LEO energy conservation over 10 orbits") {
        Vec3 pos, vel;
        orbital_elements_to_cartesian(leo_craft->orbit, earth->mass, &pos, &vel);
        const double initial_energy = compute_energy(pos, vel, leo_craft->mass, earth->mass);

        Vec3 final_pos, final_vel;
        propagate_n_periods(sim, LEO_IDX, PARENT_EARTH, 10, 10.0, final_pos, final_vel);
        const double final_energy = compute_energy(final_pos, final_vel, leo_craft->mass, earth->mass);

        const double energy_error = fabs(final_energy - initial_energy) / fabs(initial_energy);
        const double pos_error = vec3_magnitude(vec3_sub(final_pos, pos));

        INFO("Energy relative error: " << energy_error);
        INFO("Position error after 10 orbits: " << pos_error << " m");

        REQUIRE_THAT(energy_error, WithinAbs(0.0, REL_TOL));
    }

    // --- Fixture: Mercury-like spacecraft ---
    const int MERCURY_IDX = 1;
    const int PARENT_SUN = 1;
    Spacecraft* mercury_craft = &sim->spacecraft[MERCURY_IDX];
    CelestialBody* sun = &sim->bodies[PARENT_SUN];
    const double mercury_period = compute_period(mercury_craft->orbit.semi_major_axis, sun->mass);
    INFO("Mercury-like period: " << mercury_period << " s (" << mercury_period / 86400.0 << " days)");

    SECTION("Mercury-like energy conservation over 5 orbits") {
        Vec3 pos, vel;
        orbital_elements_to_cartesian(mercury_craft->orbit, sun->mass, &pos, &vel);
        const double initial_energy = compute_energy(pos, vel, mercury_craft->mass, sun->mass);

        Vec3 final_pos, final_vel;
        propagate_n_periods(sim, MERCURY_IDX, PARENT_SUN, 5, 3600.0, final_pos, final_vel);
        const double final_energy = compute_energy(final_pos, final_vel, mercury_craft->mass, sun->mass);

        const double energy_error = fabs(final_energy - initial_energy) / fabs(initial_energy);
        const double pos_error = vec3_magnitude(vec3_sub(final_pos, pos));

        INFO("Energy relative error: " << energy_error);
        INFO("Position error after 5 orbits: " << pos_error << " m");

        REQUIRE_THAT(energy_error, WithinAbs(0.0, REL_TOL));
    }

    // --- Fixture: Jupiter-like spacecraft ---
    const int JUPITER_IDX = 2;
    Spacecraft* jupiter_craft = &sim->spacecraft[JUPITER_IDX];
    const double jupiter_period = compute_period(jupiter_craft->orbit.semi_major_axis, sun->mass);
    INFO("Jupiter-like period: " << jupiter_period << " s (" << jupiter_period / (86400.0 * 365.0) << " years)");

    SECTION("Jupiter-like energy conservation over 2 years") {
        const double prop_time = 2.0 * 365.0 * 86400.0;
        const double parent_mass = sun->mass;
        OrbitalElements current = jupiter_craft->orbit;
        int steps = (int)(prop_time / TIME_STEP);
        for (int s = 0; s < steps; s++) {
            current = propagate_orbital_elements(current, TIME_STEP, parent_mass);
        }
        Vec3 final_pos, final_vel;
        orbital_elements_to_cartesian(current, parent_mass, &final_pos, &final_vel);

        Vec3 init_pos, init_vel;
        orbital_elements_to_cartesian(jupiter_craft->orbit, parent_mass, &init_pos, &init_vel);
        const double initial_energy = compute_energy(init_pos, init_vel, jupiter_craft->mass, parent_mass);
        const double final_energy = compute_energy(final_pos, final_vel, jupiter_craft->mass, parent_mass);
        const double energy_error = fabs(final_energy - initial_energy) / fabs(initial_energy);

        INFO("After 2 years, energy relative error: " << energy_error);
        REQUIRE_THAT(energy_error, WithinAbs(0.0, REL_TOL));
    }

    // --- Low altitude orbit ---
    const int LOW_ALT_IDX = 3;
    Spacecraft* low_alt_craft = &sim->spacecraft[LOW_ALT_IDX];
    const double low_alt_period = compute_period(low_alt_craft->orbit.semi_major_axis, earth->mass);
    INFO("Low altitude period: " << low_alt_period << " s (" << low_alt_period / 60.0 << " min)");

    SECTION("Low altitude orbit stays above surface (100 km)") {
        const double parent_radius = earth->radius;
        OrbitalElements current = low_alt_craft->orbit;
        for (int orbit = 0; orbit < 10; orbit++) {
            current = propagate_orbital_elements(current, 10.0, earth->mass);
            Vec3 pos, vel;
            orbital_elements_to_cartesian(current, earth->mass, &pos, &vel);
            const double r = vec3_magnitude(pos);
            const double altitude = r - parent_radius;
            INFO("Orbit " << orbit << " radius: " << r << " m, altitude: " << altitude << " m");
            REQUIRE_THAT(altitude, WithinAbs(100000.0, R_TOL));
        }
    }

    // --- Super-synchronous orbit ---
    const int SUPER_SYNC_IDX = 4;
    Spacecraft* super_sync_craft = &sim->spacecraft[SUPER_SYNC_IDX];
    const double super_sync_period = compute_period(super_sync_craft->orbit.semi_major_axis, earth->mass);
    INFO("Super-synchronous period: " << super_sync_period << " s (" << super_sync_period / 3600.0 << " hours)");

    SECTION("Super-synchronous period exceeds 24 hours") {
        REQUIRE_THAT(super_sync_period, WithinAbs(95002.684566, M_TOL));
    }

    SECTION("Super-synchronous energy conservation over 3 days") {
        const double prop_time = 3.0 * 24.0 * 3600.0;
        const double parent_mass = earth->mass;
        OrbitalElements current = super_sync_craft->orbit;
        int steps = (int)(prop_time / TIME_STEP);
        for (int s = 0; s < steps; s++) {
            current = propagate_orbital_elements(current, TIME_STEP, parent_mass);
        }
        Vec3 final_pos, final_vel;
        orbital_elements_to_cartesian(current, parent_mass, &final_pos, &final_vel);

        Vec3 init_pos, init_vel;
        orbital_elements_to_cartesian(super_sync_craft->orbit, parent_mass, &init_pos, &init_vel);
        const double initial_energy = compute_energy(init_pos, init_vel, super_sync_craft->mass, parent_mass);
        const double final_energy = compute_energy(final_pos, final_vel, super_sync_craft->mass, parent_mass);
        const double energy_error = fabs(final_energy - initial_energy) / fabs(initial_energy);

        INFO("After 3 days, energy relative error: " << energy_error);
        REQUIRE_THAT(energy_error, WithinAbs(0.0, REL_TOL));
    }

    // --- Geosynchronous orbit ---
    const int GEO_IDX = 5;
    Spacecraft* geo_craft = &sim->spacecraft[GEO_IDX];
    const double geo_period = compute_period(geo_craft->orbit.semi_major_axis, earth->mass);
    const double geo_period_hours = geo_period / 3600.0;
    const double SIDEREAL_DAY_HOURS = 23.93447;

    SECTION("Geosynchronous period matches sidereal day") {
        const double period_error_hours = fabs(geo_period_hours - SIDEREAL_DAY_HOURS);
        INFO("Calculated period: " << geo_period_hours << " hours");
        INFO("Sidereal day: " << SIDEREAL_DAY_HOURS << " hours");
        INFO("Period error: " << period_error_hours << " hours");
        REQUIRE_THAT(geo_period_hours, WithinAbs(SIDEREAL_DAY_HOURS, PERIOD_HOURS_TOL));
    }

    SECTION("Geosynchronous one-period roundtrip") {
        const double parent_mass = earth->mass;
        OrbitalElements propagated = geo_craft->orbit;
        propagated = propagate_orbital_elements(propagated, geo_period, parent_mass);

        Vec3 init_pos, init_vel, final_pos, final_vel;
        orbital_elements_to_cartesian(geo_craft->orbit, parent_mass, &init_pos, &init_vel);
        orbital_elements_to_cartesian(propagated, parent_mass, &final_pos, &final_vel);
        const double pos_error = vec3_magnitude(vec3_sub(final_pos, init_pos));

        INFO("Position error after one period: " << pos_error << " m");
        REQUIRE_THAT(pos_error, WithinAbs(0.0, R_TOL));
    }

    // --- Period consistency from different true anomalies ---
    SECTION("Period consistency across different starting true anomalies") {
        const double parent_mass = sun->mass;
        const double period = mercury_period;
        const double test_anomalies[] = {0.0, M_PI / 2.0, M_PI, 3.0 * M_PI / 2.0};

        for (int i = 0; i < 4; i++) {
            OrbitalElements test_orbit = mercury_craft->orbit;
            test_orbit.true_anomaly = test_anomalies[i];

            OrbitalElements propagated = test_orbit;
            propagated = propagate_orbital_elements(propagated, period, parent_mass);

            Vec3 init_pos, init_vel, final_pos, final_vel;
            orbital_elements_to_cartesian(test_orbit, parent_mass, &init_pos, &init_vel);
            orbital_elements_to_cartesian(propagated, parent_mass, &final_pos, &final_vel);

            const double pos_error = vec3_magnitude(vec3_sub(final_pos, init_pos));
            const double vel_error = vec3_magnitude(vec3_sub(final_vel, init_vel));

            INFO("True anomaly: " << test_anomalies[i] << " rad");
            INFO("Position error: " << pos_error << " m");
            INFO("Velocity error: " << vel_error << " m/s");

            REQUIRE_THAT(pos_error, WithinAbs(0.0, PROP_POS_TOL));
            REQUIRE_THAT(vel_error, WithinAbs(0.0, V_TOL));
        }
    }

    // --- Combined energy test for all spacecraft ---
    struct EnergyTest {
        int craft_index;
        int parent_index;
        const char* name;
        int num_periods;
    };

    EnergyTest all_tests[] = {
        {0, 0, "LEO", 10},
        {1, 1, "Mercury-like", 5},
        {2, 1, "Jupiter-like", 2},
        {3, 0, "Low altitude", 10},
        {4, 0, "Super-synchronous", 3},
        {5, 0, "Geosynchronous", 1},
    };

    SECTION("Energy conservation across all timescales") {
        for (const auto& t : all_tests) {
            Spacecraft* craft = &sim->spacecraft[t.craft_index];
            CelestialBody* parent = &sim->bodies[t.parent_index];

            Vec3 init_pos, init_vel;
            orbital_elements_to_cartesian(craft->orbit, parent->mass, &init_pos, &init_vel);
            const double initial_energy = compute_energy(init_pos, init_vel, craft->mass, parent->mass);

            double period = compute_period(craft->orbit.semi_major_axis, parent->mass);
            double prop_time;
            if (t.num_periods == 2) {
                prop_time = 2.0 * 365.0 * 86400.0;  // 2 years for Jupiter
            } else if (t.num_periods == 3) {
                prop_time = 3.0 * 24.0 * 3600.0;     // 3 days for super-sync
            } else {
                prop_time = t.num_periods * period;
            }

            OrbitalElements current = craft->orbit;
            int steps = (int)(prop_time / TIME_STEP);
            for (int s = 0; s < steps; s++) {
                current = propagate_orbital_elements(current, TIME_STEP, parent->mass);
            }

            Vec3 final_pos, final_vel;
            orbital_elements_to_cartesian(current, parent->mass, &final_pos, &final_vel);
            const double final_energy = compute_energy(final_pos, final_vel, craft->mass, parent->mass);
            const double energy_error = fabs(final_energy - initial_energy) / fabs(initial_energy);

            INFO(t.name << " energy relative error: " << energy_error);
            REQUIRE_THAT(energy_error, WithinAbs(0.0, REL_TOL));
        }
    }

    // --- Mean anomaly accumulation ---
    SECTION("Mean anomaly accumulation over 10 years") {
        const double parent_mass = sun->mass;
        const double a = jupiter_craft->orbit.semi_major_axis;
        const double e = jupiter_craft->orbit.eccentricity;
        const double mu = G * parent_mass;
        const double n = sqrt(mu / pow(a, 3.0));
        const double prop_time = 10.0 * 365.0 * 86400.0;
        const double expected_mean_anomaly = n * prop_time;
        const double expected_orbits = expected_mean_anomaly / (2.0 * M_PI);

        INFO("Expected mean anomaly after 10 years: " << expected_mean_anomaly << " rad");
        INFO("Expected orbits: " << expected_orbits);

        OrbitalElements current = jupiter_craft->orbit;
        int steps = (int)(prop_time / TIME_STEP);
        for (int s = 0; s < steps; s++) {
            current = propagate_orbital_elements(current, TIME_STEP, parent_mass);
        }

        Vec3 final_pos, final_vel;
        orbital_elements_to_cartesian(current, parent_mass, &final_pos, &final_vel);

        const double true_anomaly_change = current.true_anomaly - jupiter_craft->orbit.true_anomaly;
        const double expected_true_anomaly_change = fmod(expected_mean_anomaly, 2.0 * M_PI);

        INFO("True anomaly change: " << true_anomaly_change << " rad");
        INFO("Expected true anomaly change: " << expected_true_anomaly_change << " rad");

        REQUIRE_THAT(fabs(current.eccentricity - e), WithinAbs(0.0, E_TOL));
        REQUIRE_THAT(fabs(current.semi_major_axis - a), WithinAbs(0.0, A_TOL));
    }

    destroy_simulation(sim);
}