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

SCENARIO("True anomaly round-trip conversion preserves values",
         "[orbital_elements][true_anomaly]") {
    const double parent_mass = 5.972e24;
    const double a = 7000e3;
    const double e = 0.3;

    SECTION("at periapsis (nu = 0)") {
        const double expected_nu = 0.0;

        OrbitalElements elements = {0};
        elements.semi_major_axis = a;
        elements.eccentricity = e;
        elements.true_anomaly = expected_nu;
        elements.inclination = 0.0;
        elements.longitude_of_ascending_node = 0.0;
        elements.argument_of_periapsis = 0.0;

        Vec3 pos, vel;
        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,
                     Catch::Matchers::WithinAbs(expected_nu, 1e-12));
    }

    SECTION("at apoapsis (nu = pi)") {
        const double expected_nu = M_PI;

        OrbitalElements elements = {0};
        elements.semi_major_axis = a;
        elements.eccentricity = e;
        elements.true_anomaly = expected_nu;
        elements.inclination = 0.0;
        elements.longitude_of_ascending_node = 0.0;
        elements.argument_of_periapsis = 0.0;

        Vec3 pos, vel;
        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,
                     Catch::Matchers::WithinAbs(expected_nu, 1e-12));
    }

    SECTION("at 90 degrees (nu = pi/2)") {
        const double expected_nu = M_PI / 2.0;

        OrbitalElements elements = {0};
        elements.semi_major_axis = a;
        elements.eccentricity = e;
        elements.true_anomaly = expected_nu;
        elements.inclination = 0.0;
        elements.longitude_of_ascending_node = 0.0;
        elements.argument_of_periapsis = 0.0;

        Vec3 pos, vel;
        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,
                     Catch::Matchers::WithinAbs(expected_nu, 1e-12));
    }

    SECTION("at 270 degrees (nu = 3*pi/2)") {
        const double expected_nu = 3.0 * M_PI / 2.0;

        OrbitalElements elements = {0};
        elements.semi_major_axis = a;
        elements.eccentricity = e;
        elements.true_anomaly = expected_nu;
        elements.inclination = 0.0;
        elements.longitude_of_ascending_node = 0.0;
        elements.argument_of_periapsis = 0.0;

        Vec3 pos, vel;
        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,
                     Catch::Matchers::WithinAbs(expected_nu, 1e-12));
    }
}

SCENARIO("Radius at periapsis and apoapsis matches analytical formula",
         "[orbital_elements][sanity]") {
    const double parent_mass = 5.972e24;
    const double a = 7000e3;
    const double e = 0.3;
    const double expected_r_peri = a * (1.0 - e);  // 4900000.0
    const double expected_r_apo = a * (1.0 + e);   // 9100000.0

    SECTION("periapsis radius = a*(1-e)") {
        OrbitalElements peri = {0};
        peri.semi_major_axis = a;
        peri.eccentricity = e;
        peri.true_anomaly = 0.0;

        Vec3 pos, vel;
        orbital_elements_to_cartesian(peri, 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,
                     Catch::Matchers::WithinAbs(expected_r_peri, 1e-6));
    }

    SECTION("apoapsis radius = a*(1+e)") {
        OrbitalElements apo = {0};
        apo.semi_major_axis = a;
        apo.eccentricity = e;
        apo.true_anomaly = M_PI;

        Vec3 pos, vel;
        orbital_elements_to_cartesian(apo, 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,
                     Catch::Matchers::WithinAbs(expected_r_apo, 1e-6));
    }
}