#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>
#include "../src/physics.h"
#include "../src/orbital_mechanics.h"
#include <cmath>

using Catch::Matchers::WithinAbs;

SCENARIO("True anomaly round-trip conversion and radius sanity checks",
         "[orbital_elements][true_anomaly][sanity]") {
    const double parent_mass = 5.972e24;
    const double a = 7000e3;
    const double e = 0.3;
    OrbitalElements elements = {0};
    elements.semi_major_axis = a;
    elements.eccentricity = e;
    Vec3 pos, vel;

    // Precomputed analytical values
    const double expected_r_peri = a * (1.0 - e);  // 4900000.0
    const double expected_r_apo = a * (1.0 + e);   // 9100000.0

    auto check_roundtrip = [&](double expected_nu) {
        elements.true_anomaly = expected_nu;

        orbital_elements_to_cartesian(elements, parent_mass, &pos, &vel);
        OrbitalElements reconstructed = cartesian_to_orbital_elements(pos, vel, parent_mass);

        INFO("Expected nu: " << expected_nu);
        INFO("Reconstructed: " << reconstructed.true_anomaly);
        REQUIRE_THAT(reconstructed.true_anomaly, WithinAbs(expected_nu, 1e-12));
    };

    SECTION("at periapsis (nu = 0)") { check_roundtrip(0.0); }
    SECTION("at apoapsis (nu = pi)") { check_roundtrip(M_PI); }
    SECTION("at 90 degrees (nu = pi/2)") { check_roundtrip(M_PI / 2.0); }
    SECTION("at 270 degrees (nu = 3*pi/2)") { check_roundtrip(3.0 * M_PI / 2.0); }

    SECTION("periapsis radius = a*(1-e)") {
        orbital_elements_to_cartesian(elements, parent_mass, &pos, &vel);
        const double r_peri = vec3_magnitude(pos);

        INFO("Expected r: " << expected_r_peri);
        INFO("Calculated r: " << r_peri);

        REQUIRE_THAT(r_peri, WithinAbs(expected_r_peri, 1e-6));
    }

    SECTION("apoapsis radius = a*(1+e)") {
        elements.true_anomaly = M_PI;

        orbital_elements_to_cartesian(elements, parent_mass, &pos, &vel);
        const double r_apo = vec3_magnitude(pos);

        INFO("Expected r: " << expected_r_apo);
        INFO("Calculated r: " << r_apo);
        REQUIRE_THAT(r_apo, WithinAbs(expected_r_apo, 1e-6));
    }
}