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

const double CONVERGENCE_TOLERANCE = 1.0e-10;
const int MAX_ITERATIONS = 50;

double calculate_orbital_period(double semi_major_axis, double parent_mass) {
    double mu = G * parent_mass;
    return 2.0 * M_PI * sqrt(pow(semi_major_axis, 3.0) / mu);
}

double calculate_orbital_energy(const Vec3& position, const Vec3& velocity, double parent_mass, double craft_mass) {
    double r = vec3_magnitude(position);
    double v_squared = velocity.x * velocity.x + velocity.y * velocity.y + velocity.z * velocity.z;
    double kinetic = 0.5 * craft_mass * v_squared;
    double potential = -G * craft_mass * parent_mass / r;
    return kinetic + potential;
}

TEST_CASE("Fast orbit - LEO (period ~92 minutes)", "[extreme][timescales][fast]") {
    const double TIME_STEP = 10.0;
    const int NUM_ORBITS = 10;

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

    const int CRAFT_INDEX = 0;
    const int PARENT_INDEX = 0;
    Spacecraft* craft = &sim->spacecraft[CRAFT_INDEX];
    CelestialBody* parent = &sim->bodies[PARENT_INDEX];

    double expected_period = calculate_orbital_period(craft->orbit.semi_major_axis, parent->mass);

    INFO("Expected LEO period: " << expected_period << " s (" << (expected_period / 60.0) << " minutes)");

    Vec3 initial_pos, initial_vel;
    orbital_elements_to_cartesian(craft->orbit, parent->mass, &initial_pos, &initial_vel);
    double initial_energy = calculate_orbital_energy(initial_pos, initial_vel, parent->mass, craft->mass);

    for (int orbit = 0; orbit < NUM_ORBITS; orbit++) {
        double orbit_start_time = sim->time;

        OrbitalElements propagated = craft->orbit;
        while (sim->time < orbit_start_time + expected_period) {
            propagated = propagate_orbital_elements(propagated, TIME_STEP, parent->mass);
            sim->time += TIME_STEP;
        }

        Vec3 final_pos, final_vel;
        orbital_elements_to_cartesian(propagated, parent->mass, &final_pos, &final_vel);

        double final_energy = calculate_orbital_energy(final_pos, final_vel, parent->mass, craft->mass);

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

        INFO("Orbit " << orbit << " energy error: " << energy_error);
        INFO("Orbit " << orbit << " position error: " << pos_error << " m");

        REQUIRE_THAT(energy_error, Catch::Matchers::WithinAbs(0.0, 1e-9));
    }

    destroy_simulation(sim);
}

TEST_CASE("Fast orbit - Mercury-like (period ~88 days)", "[extreme][timescales][fast]") {
    const double TIME_STEP = 3600.0;

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

    const int CRAFT_INDEX = 1;
    const int PARENT_INDEX = 1;
    Spacecraft* craft = &sim->spacecraft[CRAFT_INDEX];
    CelestialBody* parent = &sim->bodies[PARENT_INDEX];

    double expected_period = calculate_orbital_period(craft->orbit.semi_major_axis, parent->mass);

    INFO("Expected Mercury-like period: " << expected_period << " s (" << (expected_period / 86400.0) << " days)");

    Vec3 initial_pos, initial_vel;
    orbital_elements_to_cartesian(craft->orbit, parent->mass, &initial_pos, &initial_vel);
    double initial_energy = calculate_orbital_energy(initial_pos, initial_vel, parent->mass, craft->mass);

    const int NUM_ORBITS = 5;
    for (int orbit = 0; orbit < NUM_ORBITS; orbit++) {
        OrbitalElements propagated = craft->orbit;
        for (int step = 0; step < (int)(expected_period / TIME_STEP); step++) {
            propagated = propagate_orbital_elements(propagated, TIME_STEP, parent->mass);
        }

        Vec3 final_pos, final_vel;
        orbital_elements_to_cartesian(propagated, parent->mass, &final_pos, &final_vel);

        double final_energy = calculate_orbital_energy(final_pos, final_vel, parent->mass, craft->mass);

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

        INFO("Orbit " << orbit << " energy error: " << energy_error);
        INFO("Orbit " << orbit << " position error: " << pos_error << " m");

        REQUIRE_THAT(energy_error, Catch::Matchers::WithinAbs(0.0, 1e-9));
    }

    destroy_simulation(sim);
}

TEST_CASE("Long period orbit - Jupiter-like (period ~12 years)", "[extreme][timescales][long]") {
    const double TIME_STEP = 86400.0;

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

    const int CRAFT_INDEX = 2;
    const int PARENT_INDEX = 1;
    Spacecraft* craft = &sim->spacecraft[CRAFT_INDEX];
    CelestialBody* parent = &sim->bodies[PARENT_INDEX];

    double expected_period = calculate_orbital_period(craft->orbit.semi_major_axis, parent->mass);

    INFO("Expected long period: " << expected_period << " s (" << (expected_period / (86400.0 * 365.0)) << " years)");

    Vec3 initial_pos, initial_vel;
    orbital_elements_to_cartesian(craft->orbit, parent->mass, &initial_pos, &initial_vel);
    double initial_energy = calculate_orbital_energy(initial_pos, initial_vel, parent->mass, craft->mass);

    const double PROPAGATION_TIME = 2.0 * 365.0 * 86400.0;

    OrbitalElements propagated = craft->orbit;
    int num_steps = (int)(PROPAGATION_TIME / TIME_STEP);
    for (int step = 0; step < num_steps; step++) {
        propagated = propagate_orbital_elements(propagated, TIME_STEP, parent->mass);
    }

    Vec3 final_pos, final_vel;
    orbital_elements_to_cartesian(propagated, parent->mass, &final_pos, &final_vel);

    double final_energy = calculate_orbital_energy(final_pos, final_vel, parent->mass, craft->mass);

    double energy_error = fabs(final_energy - initial_energy) / fabs(initial_energy);

    INFO("After " << (PROPAGATION_TIME / (86400.0 * 365.0)) << " years:");
    INFO("Energy error: " << energy_error);

    REQUIRE_THAT(energy_error, Catch::Matchers::WithinAbs(0.0, 1e-9));

    destroy_simulation(sim);
}

TEST_CASE("Low altitude orbit (~100 km)", "[extreme][timescales][low]") {
    const double TIME_STEP = 10.0;

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

    const int CRAFT_INDEX = 3;
    const int PARENT_INDEX = 0;
    Spacecraft* craft = &sim->spacecraft[CRAFT_INDEX];
    CelestialBody* parent = &sim->bodies[PARENT_INDEX];

    double expected_period = calculate_orbital_period(craft->orbit.semi_major_axis, parent->mass);

    INFO("Expected low altitude period: " << expected_period << " s (" << (expected_period / 60.0) << " minutes)");

    const int NUM_ORBITS = 10;
    for (int orbit = 0; orbit < NUM_ORBITS; orbit++) {
        OrbitalElements propagated = craft->orbit;
        for (int step = 0; step < (int)(expected_period / TIME_STEP); step++) {
            propagated = propagate_orbital_elements(propagated, TIME_STEP, parent->mass);
        }

        Vec3 pos, vel;
        orbital_elements_to_cartesian(propagated, parent->mass, &pos, &vel);

        double r = vec3_magnitude(pos);

        INFO("Orbit " << orbit << " radius: " << r << " m");
        INFO("Parent radius: " << parent->radius << " m");
        INFO("Altitude: " << (r - parent->radius) << " m");

        REQUIRE(r > parent->radius);
    }

    destroy_simulation(sim);
}

TEST_CASE("Super-synchronous orbit (period > 24 hours)", "[extreme][timescales][super_sync]") {
    const double TIME_STEP = 3600.0;
    const double TARGET_PERIOD = 24.0 * 3600.0;

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

    const int CRAFT_INDEX = 4;
    const int PARENT_INDEX = 0;
    Spacecraft* craft = &sim->spacecraft[CRAFT_INDEX];
    CelestialBody* parent = &sim->bodies[PARENT_INDEX];

    double period = calculate_orbital_period(craft->orbit.semi_major_axis, parent->mass);

    INFO("Super-synchronous period: " << period << " s (" << (period / 3600.0) << " hours)");
    INFO("One Earth day: " << TARGET_PERIOD << " s (" << (TARGET_PERIOD / 3600.0) << " hours)");

    REQUIRE(period > TARGET_PERIOD);

    Vec3 initial_pos, initial_vel;
    orbital_elements_to_cartesian(craft->orbit, parent->mass, &initial_pos, &initial_vel);
    double initial_energy = calculate_orbital_energy(initial_pos, initial_vel, parent->mass, craft->mass);

    const double PROPAGATION_TIME = 3.0 * TARGET_PERIOD;

    OrbitalElements propagated = craft->orbit;
    int num_steps = (int)(PROPAGATION_TIME / TIME_STEP);
    for (int step = 0; step < num_steps; step++) {
        propagated = propagate_orbital_elements(propagated, TIME_STEP, parent->mass);
    }

    Vec3 final_pos, final_vel;
    orbital_elements_to_cartesian(propagated, parent->mass, &final_pos, &final_vel);

    double final_energy = calculate_orbital_energy(final_pos, final_vel, parent->mass, craft->mass);
    double energy_error = fabs(final_energy - initial_energy) / fabs(initial_energy);

    INFO("After 3 Earth days, energy error: " << energy_error);

    REQUIRE_THAT(energy_error, Catch::Matchers::WithinAbs(0.0, 1e-9));

    destroy_simulation(sim);
}

TEST_CASE("Geosynchronous orbit (period = sidereal day)", "[extreme][timescales][geosync]") {
    const double SIDEREAL_DAY_HOURS = 23.93447;

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

    const int CRAFT_INDEX = 5;
    const int PARENT_INDEX = 0;
    Spacecraft* craft = &sim->spacecraft[CRAFT_INDEX];
    CelestialBody* parent = &sim->bodies[PARENT_INDEX];

    double period = calculate_orbital_period(craft->orbit.semi_major_axis, parent->mass);
    double period_hours = period / 3600.0;
    double period_error_hours = fabs(period_hours - SIDEREAL_DAY_HOURS);

    INFO("Calculated period: " << period << " s (" << period_hours << " hours)");
    INFO("Sidereal day: " << SIDEREAL_DAY_HOURS << " hours");
    INFO("Period error: " << period_error_hours << " hours (" << (period_error_hours * 3600.0) << " s)");

    REQUIRE_THAT(period_hours, Catch::Matchers::WithinAbs(SIDEREAL_DAY_HOURS, 0.0002));

    Vec3 initial_pos, initial_vel;
    orbital_elements_to_cartesian(craft->orbit, parent->mass, &initial_pos, &initial_vel);

    OrbitalElements propagated = craft->orbit;
    propagated = propagate_orbital_elements(propagated, period, parent->mass);

    Vec3 final_pos, final_vel;
    orbital_elements_to_cartesian(propagated, parent->mass, &final_pos, &final_vel);

    double pos_error = vec3_magnitude(vec3_sub(final_pos, initial_pos));

    INFO("Position error after one period: " << pos_error << " m");

    REQUIRE_THAT(pos_error, Catch::Matchers::WithinAbs(0.0, 1e-3));

    destroy_simulation(sim);
}

TEST_CASE("Period consistency across different true anomalies", "[extreme][timescales][consistency]") {
    const double TIME_STEP = 3600.0;

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

    const int CRAFT_INDEX = 1;
    const int PARENT_INDEX = 1;
    Spacecraft* craft = &sim->spacecraft[CRAFT_INDEX];
    CelestialBody* parent = &sim->bodies[PARENT_INDEX];

    double period = calculate_orbital_period(craft->orbit.semi_major_axis, parent->mass);

    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 = craft->orbit;
        test_orbit.true_anomaly = test_anomalies[i];

        OrbitalElements propagated = test_orbit;
        propagated = propagate_orbital_elements(propagated, period, parent->mass);

        Vec3 initial_pos, initial_vel;
        Vec3 final_pos, final_vel;
        orbital_elements_to_cartesian(test_orbit, parent->mass, &initial_pos, &initial_vel);
        orbital_elements_to_cartesian(propagated, parent->mass, &final_pos, &final_vel);

        double pos_error = vec3_magnitude(vec3_sub(final_pos, initial_pos));
        double vel_error = vec3_magnitude(vec3_sub(final_vel, initial_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, Catch::Matchers::WithinAbs(0.0, 1e-3));
        REQUIRE_THAT(vel_error, Catch::Matchers::WithinAbs(0.0, 1e-6));
    }

    destroy_simulation(sim);
}

TEST_CASE("Energy conservation across all timescales", "[extreme][timescales][energy]") {
    const double TIME_STEP = 3600.0;

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

    struct EnergyTest {
        int craft_index;
        int parent_index;
        const char* name;
    };

    EnergyTest tests[] = {
        {0, 0, "LEO (fast)"},
        {1, 1, "Mercury-like (fast)"},
        {2, 1, "Jupiter-like (long)"},
        {3, 0, "Low altitude (low)"},
        {4, 0, "Super-synchronous"},
        {5, 0, "Geosynchronous"}
    };

    for (int t = 0; t < 6; t++) {
        EnergyTest test = tests[t];
        Spacecraft* craft = &sim->spacecraft[test.craft_index];
        CelestialBody* parent = &sim->bodies[test.parent_index];

        Vec3 initial_pos, initial_vel;
        orbital_elements_to_cartesian(craft->orbit, parent->mass, &initial_pos, &initial_vel);
        double initial_energy = calculate_orbital_energy(initial_pos, initial_vel, parent->mass, craft->mass);

        double period = calculate_orbital_period(craft->orbit.semi_major_axis, parent->mass);
        double propagation_time = period * 2.0;

        OrbitalElements propagated = craft->orbit;
        int num_steps = (int)(propagation_time / TIME_STEP);
        for (int step = 0; step < num_steps; step++) {
            propagated = propagate_orbital_elements(propagated, TIME_STEP, parent->mass);
        }

        Vec3 final_pos, final_vel;
        orbital_elements_to_cartesian(propagated, parent->mass, &final_pos, &final_vel);
        double final_energy = calculate_orbital_energy(final_pos, final_vel, parent->mass, craft->mass);

        double energy_error = fabs(final_energy - initial_energy) / fabs(initial_energy);

        INFO(test.name << ":");
        INFO("  Initial energy: " << initial_energy << " J");
        INFO("  Final energy: " << final_energy << " J");
        INFO("  Relative error: " << energy_error);

        REQUIRE_THAT(energy_error, Catch::Matchers::WithinAbs(0.0, 1e-9));
    }

    destroy_simulation(sim);
}

TEST_CASE("Mean anomaly accumulation for very long periods", "[extreme][timescales][mean_anomaly]") {
    const double TIME_STEP = 86400.0;

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

    const int CRAFT_INDEX = 2;
    const int PARENT_INDEX = 1;
    Spacecraft* craft = &sim->spacecraft[CRAFT_INDEX];
    CelestialBody* parent = &sim->bodies[PARENT_INDEX];

    double mu = G * parent->mass;
    double a = craft->orbit.semi_major_axis;
    double e = craft->orbit.eccentricity;
    double n = sqrt(mu / pow(a, 3.0));

    const double PROPAGATION_TIME = 10.0 * 365.0 * 86400.0;
    double expected_mean_anomaly = n * PROPAGATION_TIME;
    double expected_orbits = expected_mean_anomaly / (2.0 * M_PI);

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

    OrbitalElements propagated = craft->orbit;
    int num_steps = (int)(PROPAGATION_TIME / TIME_STEP);
    for (int step = 0; step < num_steps; step++) {
        propagated = propagate_orbital_elements(propagated, TIME_STEP, parent->mass);
    }

    Vec3 final_pos, final_vel;
    orbital_elements_to_cartesian(propagated, parent->mass, &final_pos, &final_vel);

    double true_anomaly_change = propagated.true_anomaly - craft->orbit.true_anomaly;
    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(propagated.eccentricity - e), Catch::Matchers::WithinAbs(0.0, 1e-10));
    REQUIRE_THAT(fabs(propagated.semi_major_axis - a), Catch::Matchers::WithinAbs(0.0, 1e-6));

    destroy_simulation(sim);
}