#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;

SCENARIO("Cartesian ↔ orbital elements round-trip conversion",
         "[cartesian][elements][roundtrip]") {
    const double TIME_STEP = 60.0;
    const double parent_mass = 5.972e24;

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_cartesian_to_elements_basic.toml"));
    Spacecraft* craft = &sim->spacecraft[0];

    const OrbitalElements& orig = craft->orbit;

    // Convert elements → state vectors
    Vec3 pos, vel;
    orbital_elements_to_cartesian(orig, parent_mass, &pos, &vel);

    const double expected_r = vec3_magnitude(pos);    // 7500000.0
    const double expected_v = vec3_magnitude(vel);    // 8928.484709...

    // Round-trip: state vectors → elements
    const OrbitalElements recovered = cartesian_to_orbital_elements(pos, vel, parent_mass);

    // Re-convert recovered elements → state vectors
    Vec3 pos2, vel2;
    orbital_elements_to_cartesian(recovered, parent_mass, &pos2, &vel2);
    const double recovered_r = vec3_magnitude(pos2);
    const double recovered_v = vec3_magnitude(vel2);

    SECTION("elements round-trip: semi-major axis") {
        const double da = fabs(recovered.semi_major_axis - orig.semi_major_axis);
        INFO("Original a: " << orig.semi_major_axis);
        INFO("Recovered a: " << recovered.semi_major_axis);
        INFO("Error: " << da << " m");
        REQUIRE_THAT(da, WithinAbs(0.0, A_TOL));
    }

    SECTION("elements round-trip: eccentricity") {
        const double de = fabs(recovered.eccentricity - orig.eccentricity);
        INFO("Original e: " << orig.eccentricity);
        INFO("Recovered e: " << recovered.eccentricity);
        INFO("Error: " << de);
        REQUIRE_THAT(de, WithinAbs(0.0, E_TOL));
    }

    SECTION("elements round-trip: true anomaly") {
        const double dnu = fabs(recovered.true_anomaly - orig.true_anomaly);
        INFO("Original nu: " << orig.true_anomaly);
        INFO("Recovered nu: " << recovered.true_anomaly);
        INFO("Error: " << dnu);
        REQUIRE_THAT(dnu, WithinAbs(0.0, ANG_TOL));
    }

    SECTION("elements round-trip: inclination") {
        const double dinc = fabs(recovered.inclination - orig.inclination);
        INFO("Original inc: " << orig.inclination);
        INFO("Recovered inc: " << recovered.inclination);
        INFO("Error: " << dinc);
        REQUIRE_THAT(dinc, WithinAbs(0.0, ANG_TOL));
    }

    SECTION("radius preservation") {
        INFO("Original r: " << expected_r);
        INFO("Recovered r: " << recovered_r);
        REQUIRE_THAT(recovered_r, WithinAbs(expected_r, R_TOL));
    }

    SECTION("velocity magnitude preservation") {
        INFO("Original v: " << expected_v);
        INFO("Recovered v: " << recovered_v);
        REQUIRE_THAT(recovered_v, WithinAbs(expected_v, V_TOL));
    }

    destroy_simulation(sim);
}