#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 "../src/test_utilities.h"
#include <cmath>

const double VELOCITY_TOLERANCE_APSIDES = 1.0;
const double POSITION_TOLERANCE_APSIDES = 1.0e3;
const double VELOCITY_TOLERANCE_TIMESTEP = 10.0;
const double POSITION_TOLERANCE_TIMESTEP = 1.0e4;

TEST_CASE("Propagation through perigee (velocity maximum)", "[analytical][propagation][perigee]") {
    const double TIME_STEP = 60.0;

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

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

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

    Vec3 pos_before;
    Vec3 vel_before;

    craft->orbit.true_anomaly = M_PI / 4.0;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos_before, &vel_before);

    double v_before = vec3_magnitude(vel_before);
    double r_before = vec3_magnitude(pos_before);

    INFO("Before perigee:");
    INFO("  Position: (" << pos_before.x << ", " << pos_before.y << ", " << pos_before.z << ") m");
    INFO("  Velocity: (" << vel_before.x << ", " << vel_before.y << ", " << vel_before.z << ") m/s");
    INFO("  Velocity magnitude: " << v_before << " m/s");
    INFO("  Radius: " << r_before << " m");

    Vec3 pos_perigee;
    Vec3 vel_perigee;

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

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

    INFO("At perigee (v=0):");
    INFO("  Position: (" << pos_perigee.x << ", " << pos_perigee.y << ", " << pos_perigee.z << ") m");
    INFO("  Velocity: (" << vel_perigee.x << ", " << vel_perigee.y << ", " << vel_perigee.z << ") m/s");
    INFO("  Velocity magnitude: " << v_perigee << " m/s");
    INFO("  Radius: " << r_perigee << " m");

    double expected_r_perigee = craft->orbit.semi_major_axis * (1.0 - craft->orbit.eccentricity);
    INFO("Expected radius at perigee: " << expected_r_perigee << " m");

    double r_error = fabs(r_perigee - expected_r_perigee);
    INFO("Radius error: " << r_error << " m");

    REQUIRE(r_error < POSITION_TOLERANCE_APSIDES);
    REQUIRE(v_perigee > v_before);

    destroy_simulation(sim);
}

TEST_CASE("Propagation through apogee (velocity minimum)", "[analytical][propagation][apogee]") {
    const double TIME_STEP = 60.0;

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

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

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

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

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

    INFO("At perigee:");
    INFO("  Velocity magnitude: " << v_perigee << " m/s");
    INFO("  Radius: " << r_perigee << " m");

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

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

    INFO("At apogee (v=π):");
    INFO("  Position: (" << pos_apogee.x << ", " << pos_apogee.y << ", " << pos_apogee.z << ") m");
    INFO("  Velocity: (" << vel_apogee.x << ", " << vel_apogee.y << ", " << vel_apogee.z << ") m/s");
    INFO("  Velocity magnitude: " << v_apogee << " m/s");
    INFO("  Radius: " << r_apogee << " m");

    double expected_r_apogee = craft->orbit.semi_major_axis * (1.0 + craft->orbit.eccentricity);
    INFO("Expected radius at apogee: " << expected_r_apogee << " m");

    double r_error = fabs(r_apogee - expected_r_apogee);
    INFO("Radius error: " << r_error << " m");

    REQUIRE(r_error < POSITION_TOLERANCE_APSIDES);
    REQUIRE(v_apogee < v_perigee);
    REQUIRE(r_apogee > r_perigee);

    destroy_simulation(sim);
}

TEST_CASE("Propagation returns to initial state after one orbital period", "[analytical][propagation][period]") {
    const double TIME_STEP = 60.0;

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

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

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

    double a = craft->orbit.semi_major_axis;
    double mu = G * earth->mass;
    double period_seconds = 2.0 * M_PI * sqrt(pow(a, 3.0) / mu);

    INFO("Semi-major axis: " << a << " m");
    INFO("Orbital period: " << period_seconds << " s (" << period_seconds / 3600.0 << " hours)");

    Vec3 pos_initial;
    Vec3 vel_initial;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos_initial, &vel_initial);

    INFO("Initial position: (" << pos_initial.x << ", " << pos_initial.y << ", " << pos_initial.z << ") m");
    INFO("Initial velocity: (" << vel_initial.x << ", " << vel_initial.y << ", " << vel_initial.z << ") m/s");

    OrbitalElements final_elements = propagate_orbital_elements(craft->orbit, period_seconds, earth->mass);

    Vec3 pos_final;
    Vec3 vel_final;
    orbital_elements_to_cartesian(final_elements, earth->mass, &pos_final, &vel_final);

    INFO("Final position: (" << pos_final.x << ", " << pos_final.y << ", " << pos_final.z << ") m");
    INFO("Final velocity: (" << vel_final.x << ", " << vel_final.y << ", " << vel_final.z << ") m/s");

    double pos_error = vec3_distance(pos_initial, pos_final);
    double vel_error = vec3_distance(vel_initial, vel_final);

    INFO("Position error after one period: " << pos_error << " m");
    INFO("Velocity error after one period: " << vel_error << " m/s");

    double r_initial = vec3_magnitude(pos_initial);
    double r_final = vec3_magnitude(pos_final);
    double relative_pos_error = pos_error / r_initial * 100.0;

    double v_initial = vec3_magnitude(vel_initial);
    double v_final = vec3_magnitude(vel_final);
    double relative_vel_error = vel_error / v_initial * 100.0;

    INFO("Relative position error: " << relative_pos_error << "%");
    INFO("Relative velocity error: " << relative_vel_error << "%");

    REQUIRE(relative_pos_error < 0.1);
    REQUIRE(relative_vel_error < 0.1);

    destroy_simulation(sim);
}

TEST_CASE("True anomaly accuracy after full orbit", "[analytical][propagation][true_anomaly]") {
    const double TIME_STEP = 60.0;

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

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

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

    double initial_true_anomaly = craft->orbit.true_anomaly;

    INFO("Initial true anomaly: " << initial_true_anomaly << " rad (" << initial_true_anomaly * 180.0 / M_PI << "°)");

    double a = craft->orbit.semi_major_axis;
    double mu = G * earth->mass;
    double period_seconds = 2.0 * M_PI * sqrt(pow(a, 3.0) / mu);

    OrbitalElements final_elements = propagate_orbital_elements(craft->orbit, period_seconds, earth->mass);

    double final_true_anomaly = final_elements.true_anomaly;

    INFO("Final true anomaly: " << final_true_anomaly << " rad (" << final_true_anomaly * 180.0 / M_PI << "°)");

    double expected_true_anomaly = fmod(initial_true_anomaly + 2.0 * M_PI, 2.0 * M_PI);
    double anomaly_error = fabs(final_true_anomaly - expected_true_anomaly);

    INFO("Expected true anomaly: " << expected_true_anomaly << " rad");
    INFO("True anomaly error: " << anomaly_error << " rad (" << anomaly_error * 180.0 / M_PI << "°)");

    REQUIRE(anomaly_error < 1.0e-6);

    destroy_simulation(sim);
}

TEST_CASE("Vis-viva equation holds at multiple points in orbit", "[analytical][propagation][vis_viva]") {
    const double TIME_STEP = 60.0;

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

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

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

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

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

    for (int i = 0; i < 5; i++) {
        double nu = true_anomalies[i];
        INFO("Testing at true anomaly: " << nu << " rad (" << nu * 180.0 / M_PI << "°)");

        craft->orbit.true_anomaly = nu;

        Vec3 position;
        Vec3 velocity;
        orbital_elements_to_cartesian(craft->orbit, earth->mass, &position, &velocity);

        double r = vec3_magnitude(position);
        double v = vec3_magnitude(velocity);

        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 * 100.0;

        INFO("  Radius: " << r << " m");
        INFO("  Actual velocity: " << v << " m/s");
        INFO("  Expected velocity: " << expected_v << " m/s");
        INFO("  Error: " << v_error << " m/s (" << relative_error << "%)");

        REQUIRE(relative_error < 0.01);
    }

    destroy_simulation(sim);
}

TEST_CASE("Large timestep - dt greater than orbital period", "[analytical][timestep][large]") {
    const double TIME_STEP = 60.0;

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

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

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

    double a = craft->orbit.semi_major_axis;
    double mu = G * earth->mass;
    double period_seconds = 2.0 * M_PI * sqrt(pow(a, 3.0) / mu);

    INFO("Orbital period: " << period_seconds << " s (" << period_seconds / 3600.0 << " hours)");

    double large_dt = period_seconds * 2.0;
    INFO("Timestep: " << large_dt << " s (2x orbital period)");

    Vec3 pos_before;
    Vec3 vel_before;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos_before, &vel_before);

    OrbitalElements propagated = propagate_orbital_elements(craft->orbit, large_dt, earth->mass);

    Vec3 pos_after;
    Vec3 vel_after;
    orbital_elements_to_cartesian(propagated, earth->mass, &pos_after, &vel_after);

    double r_before = vec3_magnitude(pos_before);
    double r_after = vec3_magnitude(pos_after);
    double v_before = vec3_magnitude(vel_before);
    double v_after = vec3_magnitude(vel_after);

    INFO("Before propagation:");
    INFO("  Radius: " << r_before << " m");
    INFO("  Velocity: " << v_before << " m/s");

    INFO("After 2 periods:");
    INFO("  Radius: " << r_after << " m");
    INFO("  Velocity: " << v_after << " m/s");

    double r_error = fabs(r_after - r_before);
    double v_error = fabs(v_after - v_before);
    double relative_r_error = r_error / r_before * 100.0;
    double relative_v_error = v_error / v_before * 100.0;

    INFO("Radius error: " << r_error << " m (" << relative_r_error << "%)");
    INFO("Velocity error: " << v_error << " m/s (" << relative_v_error << "%)");

    REQUIRE(relative_r_error < 0.1);
    REQUIRE(relative_v_error < 0.1);

    destroy_simulation(sim);
}

TEST_CASE("Very small timestep - dt less than 1 second", "[analytical][timestep][small]") {
    const double TIME_STEP = 60.0;

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

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

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

    Vec3 pos_before;
    Vec3 vel_before;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos_before, &vel_before);

    double small_dt = 0.1;
    INFO("Timestep: " << small_dt << " s");

    OrbitalElements propagated = propagate_orbital_elements(craft->orbit, small_dt, earth->mass);

    Vec3 pos_after;
    Vec3 vel_after;
    orbital_elements_to_cartesian(propagated, earth->mass, &pos_after, &vel_after);

    double pos_change = vec3_distance(pos_before, pos_after);
    double vel_change = vec3_distance(vel_before, vel_after);

    INFO("Position change: " << pos_change << " m");
    INFO("Velocity change: " << vel_change << " m/s");

    double v_before_mag = vec3_magnitude(vel_before);
    double expected_pos_change = v_before_mag * small_dt;
    double pos_error = fabs(pos_change - expected_pos_change);

    INFO("Expected position change (v·dt): " << expected_pos_change << " m");
    INFO("Position error: " << pos_error << " m");
    INFO("Relative position error: " << (pos_error / expected_pos_change * 100.0) << "%");

    REQUIRE(pos_error < VELOCITY_TOLERANCE_TIMESTEP * small_dt * 10.0);
    REQUIRE(vel_change < VELOCITY_TOLERANCE_TIMESTEP);

    destroy_simulation(sim);
}

TEST_CASE("Accuracy vs timestep size relationship", "[analytical][timestep][accuracy]") {
    const double TIME_STEP = 60.0;

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

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

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

    double a = craft->orbit.semi_major_axis;
    double mu = G * earth->mass;
    double period_seconds = 2.0 * M_PI * sqrt(pow(a, 3.0) / mu);

    double dt_ratios[] = {0.01, 0.1, 1.0, 10.0};

    Vec3 pos_initial;
    Vec3 vel_initial;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos_initial, &vel_initial);

    for (int i = 0; i < 4; i++) {
        double dt = period_seconds * dt_ratios[i];
        INFO("Testing dt = " << dt << " s (" << dt_ratios[i] << "x period)");

        OrbitalElements propagated = propagate_orbital_elements(craft->orbit, dt, earth->mass);

        Vec3 pos_final;
        Vec3 vel_final;
        orbital_elements_to_cartesian(propagated, earth->mass, &pos_final, &vel_final);

        double pos_error = vec3_distance(pos_initial, pos_final);
        double vel_error = vec3_distance(vel_initial, vel_final);

        double num_periods = dt / period_seconds;
        double expected_num_orbits = round(num_periods);

        double fractional_phase = num_periods - expected_num_orbits;
        double expected_pos_error = fractional_phase * 2.0 * M_PI * a;

        INFO("  Position error: " << pos_error << " m");
        INFO("  Expected error (phase): " << expected_pos_error << " m");
        INFO("  Number of periods: " << num_periods);

        if (expected_num_orbits > 0 && expected_pos_error > 1.0e-6) {
            double relative_error = pos_error / expected_pos_error;

            INFO("  Relative error: " << relative_error);

            REQUIRE(relative_error < 0.5);
        } else if (expected_num_orbits > 0) {
            INFO("  Expected error is zero, skipping relative error check");
            REQUIRE(pos_error < 1.0e-3);
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Mean anomaly accumulation over long propagation", "[analytical][timestep][accumulation]") {
    const double TIME_STEP = 60.0;

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

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

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

    double a = craft->orbit.semi_major_axis;
    double mu = G * earth->mass;
    double period_seconds = 2.0 * M_PI * sqrt(pow(a, 3.0) / mu);
    double mean_motion = sqrt(mu / pow(a, 3.0));

    double initial_true_anomaly = craft->orbit.true_anomaly;
    INFO("Initial true anomaly: " << initial_true_anomaly << " rad");

    double propagation_time = period_seconds * 100.0;
    INFO("Propagation time: " << propagation_time << " s (" << propagation_time / period_seconds << " periods)");

    OrbitalElements propagated = propagate_orbital_elements(craft->orbit, propagation_time, earth->mass);

    double final_true_anomaly = propagated.true_anomaly;
    INFO("Final true anomaly: " << final_true_anomaly << " rad");

    double expected_delta_anomaly = mean_motion * propagation_time;
    double expected_final_anomaly = fmod(initial_true_anomaly + expected_delta_anomaly, 2.0 * M_PI);

    INFO("Expected final anomaly: " << expected_final_anomaly << " rad");

    double raw_error = fabs(final_true_anomaly - expected_final_anomaly);
    double anomaly_error = fmin(raw_error, 2.0 * M_PI - raw_error);

    INFO("True anomaly error: " << anomaly_error << " rad (" << anomaly_error * 180.0 / M_PI << "°)");

    REQUIRE(anomaly_error < 1.0e-3);

    destroy_simulation(sim);
}