#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 = 10.0;
const double POSITION_TOLERANCE = 1.0e4;

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

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

    REQUIRE(load_system_config(sim, "tests/test_analytical_propagation_timesteps.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("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, 1, 0, TIME_STEP);

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

    Spacecraft* craft = &sim->spacecraft[0];
    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 expected_pos_change = vel_change * small_dt;
    double pos_error = fabs(pos_change - expected_pos_change);

    INFO("Expected position change: " << expected_pos_change << " m");
    INFO("Position error: " << pos_error << " m");

    REQUIRE(pos_change < VELOCITY_TOLERANCE * small_dt * 10.0);
    REQUIRE(vel_change < VELOCITY_TOLERANCE);

    destroy_simulation(sim);
}

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

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

    REQUIRE(load_system_config(sim, "tests/test_analytical_propagation_timesteps.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);

    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) {
            double relative_error = pos_error / expected_pos_error;

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

            REQUIRE(relative_error < 0.5);
        }
    }

    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, 1, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_analytical_propagation_timesteps.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);
    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 anomaly_error = fabs(final_true_anomaly - expected_final_anomaly);

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

    REQUIRE(anomaly_error < 1.0e-3);

    destroy_simulation(sim);
}