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

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

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

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

    REQUIRE(load_system_config(sim, "tests/test_analytical_propagation_apsides.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 (ν=π):");
    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);
    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, 1, 0, TIME_STEP);

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

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

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