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

using Catch::Matchers::WithinAbs;

SCENARIO("Cartesian to Elements - Advanced conversion tests",
         "[orbital_mechanics][cartesian][elements]") {
    const double M_sun = 1.989e30;

    // NOTE: Semi-major axis tolerance for |a|=1e11 m cases. The vis-viva equation
    // a = -mu/(2*epsilon) amplifies floating-point error in specific energy
    // (~1e-15 rel) to absolute errors of ~1e-4 m at this scale. Header A_TOL=1e-6
    // would fail; 2e-4 provides comfortable margin over observed ~1.4e-4 m error.
    const double A_TOL_LARGE = 2e-4;

    auto convert_and_recover = [&](const OrbitalElements& elements) {
        Vec3 pos, vel;
        orbital_elements_to_cartesian(elements, M_sun, &pos, &vel);
        return cartesian_to_orbital_elements(pos, vel, M_sun);
    };

    auto make_elements = [&](double a, double e, double nu, double inc,
                             double lon_anode, double arg_peri) {
        OrbitalElements el = {};
        el.semi_major_axis = a;
        el.eccentricity = e;
        el.true_anomaly = nu;
        el.inclination = inc;
        el.longitude_of_ascending_node = lon_anode;
        el.argument_of_periapsis = arg_peri;
        return el;
    };

    SECTION("eccentricity spectrum: circular to highly hyperbolic") {
        const double r = 1.496e11;
        const double v_circular = sqrt(G * M_sun / r);
        const Vec3 pos_circ = {r, 0.0, 0.0};
        const Vec3 vel_circ = {0.0, v_circular, 0.0};

        const OrbitalElements circular = make_elements(r, 0.0, 0.0, 0.0, 0.0, 0.0);

        Vec3 converted_pos, converted_vel;
        orbital_elements_to_cartesian(circular, M_sun, &converted_pos, &converted_vel);

        const OrbitalElements recovered_circ =
            cartesian_to_orbital_elements(converted_pos, converted_vel, M_sun);
        REQUIRE_THAT(recovered_circ.eccentricity, WithinAbs(0.0, E_TOL));
        REQUIRE_THAT(recovered_circ.semi_major_axis, WithinAbs(r, A_TOL_LARGE));
        REQUIRE(compare_vec3(pos_circ, converted_pos, A_TOL_LARGE));
        REQUIRE(compare_vec3(vel_circ, converted_vel, V_TOL));

        // Near-circular (e=0.001)
        const OrbitalElements near_circ = make_elements(1.496e11, 0.001, 0.5, 0.0, 0.0, 0.0);
        const OrbitalElements rec_near_circ = convert_and_recover(near_circ);
        REQUIRE_THAT(rec_near_circ.eccentricity, WithinAbs(0.001, E_TOL));
        REQUIRE_THAT(rec_near_circ.semi_major_axis, WithinAbs(1.496e11, A_TOL_LARGE));

        // Elliptical (e=0.5)
        const OrbitalElements elliptical = make_elements(1.0e11, 0.5, 0.8, 0.0, 0.0, 0.0);
        const OrbitalElements rec_elliptical = convert_and_recover(elliptical);
        REQUIRE_THAT(rec_elliptical.eccentricity, WithinAbs(0.5, E_TOL));
        REQUIRE_THAT(rec_elliptical.semi_major_axis, WithinAbs(1.0e11, A_TOL_LARGE));

        // Highly elliptical (e=0.95)
        const OrbitalElements high_ell = make_elements(1.0e11, 0.95, 0.1, 0.0, 0.0, 0.0);
        const OrbitalElements rec_high_ell = convert_and_recover(high_ell);
        REQUIRE_THAT(rec_high_ell.eccentricity, WithinAbs(0.95, E_TOL));
        REQUIRE_THAT(rec_high_ell.semi_major_axis, WithinAbs(1.0e11, A_TOL_LARGE));

        // Near-parabolic (e=0.999)
        const OrbitalElements near_par = make_elements(1.0e11, 0.999, 0.05, 0.0, 0.0, 0.0);
        const OrbitalElements rec_near_par = convert_and_recover(near_par);
        REQUIRE_THAT(rec_near_par.eccentricity, WithinAbs(0.999, E_TOL));

        // Parabolic (e=1.0)
        OrbitalElements parabolic = {};
        parabolic.semi_latus_rectum = 1.0e11;
        parabolic.eccentricity = 1.0;
        parabolic.true_anomaly = 0.5;
        parabolic.inclination = 0.0;
        parabolic.longitude_of_ascending_node = 0.0;
        parabolic.argument_of_periapsis = 0.0;
        const OrbitalElements rec_parabolic = convert_and_recover(parabolic);
        REQUIRE_THAT(rec_parabolic.eccentricity, WithinAbs(1.0, E_TOL));
        REQUIRE_THAT(rec_parabolic.semi_latus_rectum, WithinAbs(1.0e11, A_TOL));

        // Hyperbolic (e=2.0)
        const OrbitalElements hyper = make_elements(-1.0e11, 2.0, 0.5, 0.0, 0.0, 0.0);
        const OrbitalElements rec_hyper = convert_and_recover(hyper);
        REQUIRE_THAT(rec_hyper.eccentricity, WithinAbs(2.0, E_TOL));
        REQUIRE_THAT(rec_hyper.semi_major_axis, WithinAbs(-1.0e11, A_TOL_LARGE));

        // Highly hyperbolic (e=10.0)
        const OrbitalElements high_hyper = make_elements(-1.0e10, 10.0, 0.8, 0.0, 0.0, 0.0);
        const OrbitalElements rec_high_hyper = convert_and_recover(high_hyper);
        REQUIRE_THAT(rec_high_hyper.eccentricity, WithinAbs(10.0, E_TOL));
        REQUIRE_THAT(rec_high_hyper.semi_major_axis, WithinAbs(-1.0e10, A_TOL));
    }

    SECTION("inclination: zero, polar, and retrograde") {
        // Zero inclination (equatorial)
        const OrbitalElements eq = make_elements(1.0e11, 0.3, 0.5, 0.0, 0.0, 0.0);
        const OrbitalElements rec_eq = convert_and_recover(eq);
        REQUIRE_THAT(rec_eq.inclination, WithinAbs(0.0, ANG_TOL));
        REQUIRE_THAT(rec_eq.eccentricity, WithinAbs(0.3, E_TOL));

        // 90-degree inclination (polar)
        const OrbitalElements polar = make_elements(1.0e11, 0.2, 0.6, M_PI / 2.0, 0.5, 0.3);
        const OrbitalElements rec_polar = convert_and_recover(polar);
        REQUIRE_THAT(rec_polar.inclination, WithinAbs(M_PI / 2.0, ANG_TOL_COARSE));
        REQUIRE_THAT(rec_polar.longitude_of_ascending_node, WithinAbs(0.5, ANG_TOL_COARSE));
        REQUIRE_THAT(rec_polar.argument_of_periapsis, WithinAbs(0.3, ANG_TOL_COARSE));

        // 180-degree inclination (retrograde)
        const OrbitalElements retro = make_elements(1.0e11, 0.2, 0.6, M_PI, 0.5, 0.3);
        const OrbitalElements rec_retro = convert_and_recover(retro);
        REQUIRE_THAT(rec_retro.inclination, WithinAbs(M_PI, ANG_TOL_COARSE));
    }

    SECTION("true anomaly at key orbital positions") {
        struct nu_test {
            double nu;
            double expected_nu;
            const char* label;
        };
        std::vector<nu_test> tests = {
            {0.0, 0.0, "periapsis"},
            {M_PI, M_PI, "apoapsis"},
            {M_PI / 2.0, M_PI / 2.0, "quadrature +90"},
            {-M_PI / 2.0, 3.0 * M_PI / 2.0, "quadrature -90"},
            {3.0 * M_PI / 2.0, 3.0 * M_PI / 2.0, "quadrature +270"},
            {-3.0 * M_PI / 2.0, M_PI / 2.0, "quadrature -270"},
        };

        for (const auto& t : tests) {
            const OrbitalElements elements = make_elements(1.0e11, 0.5, t.nu, 0.0, 0.0, 0.0);
            const OrbitalElements recovered = convert_and_recover(elements);
            INFO("Test: " << t.label << " (input nu=" << t.nu << ")");
            REQUIRE_THAT(recovered.true_anomaly, WithinAbs(t.expected_nu, ANG_TOL));
            REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, E_TOL));
        }
    }

    SECTION("quadrature at various eccentricities") {
        struct e_test {
            double e;
        };
        std::vector<e_test> tests = {
            {0.9},
            {0.1},
        };

        for (const auto& t : tests) {
            const OrbitalElements elements = make_elements(1.0e11, t.e, M_PI / 2.0, 0.0, 0.0, 0.0);
            const OrbitalElements recovered = convert_and_recover(elements);
            REQUIRE_THAT(recovered.eccentricity, WithinAbs(t.e, E_TOL));
            REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, A_TOL_LARGE));
            REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, ANG_TOL));
        }
    }

    SECTION("large true anomaly values") {
        struct large_nu_test {
            double nu;
            double expected_nu;
            const char* label;
        };
        std::vector<large_nu_test> tests = {
            {5.0, 5.0, "nu=5.0"},
            {-5.0, 1.28318530717958623, "nu=-5.0"},
            {10.0, 10.0 - 2.0 * M_PI, "nu=10.0"},
        };

        for (const auto& t : tests) {
            const OrbitalElements elements = make_elements(1.0e11, 0.5, t.nu, 0.0, 0.0, 0.0);
            const OrbitalElements recovered = convert_and_recover(elements);
            INFO("Test: " << t.label);
            REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, E_TOL));
            REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, A_TOL_LARGE));
            REQUIRE_THAT(recovered.true_anomaly, WithinAbs(t.expected_nu, ANG_TOL));
        }
    }

    SECTION("3D orientation with quadrature point") {
        const OrbitalElements elements = make_elements(1.0e11, 0.5, M_PI / 2.0,
                                                       M_PI / 3.0, M_PI / 4.0, M_PI / 6.0);
        const OrbitalElements recovered = convert_and_recover(elements);
        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, E_TOL));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, A_TOL_LARGE));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, ANG_TOL));
        REQUIRE_THAT(recovered.inclination, WithinAbs(M_PI / 3.0, ANG_TOL_COARSE));
        REQUIRE_THAT(recovered.longitude_of_ascending_node, WithinAbs(M_PI / 4.0, ANG_TOL_COARSE));
        REQUIRE_THAT(recovered.argument_of_periapsis, WithinAbs(M_PI / 6.0, ANG_TOL_COARSE));
    }

    SECTION("multiple true anomaly points in sequence") {
        std::array<double, 5> true_anomalies = {0.0, M_PI / 4.0, M_PI / 2.0,
                                                 3.0 * M_PI / 4.0, M_PI};

        for (double nu : true_anomalies) {
            const OrbitalElements elements = make_elements(1.0e11, 0.5, nu, 0.0, 0.0, 0.0);
            const OrbitalElements recovered = convert_and_recover(elements);
            REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, E_TOL));
            REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, A_TOL_LARGE));
            REQUIRE_THAT(recovered.true_anomaly, WithinAbs(nu, ANG_TOL));
        }
    }

    SECTION("hyperbolic orbit at quadrature point") {
        const OrbitalElements elements = make_elements(-1.0e11, 2.0, M_PI / 2.0, 0.0, 0.0, 0.0);
        const OrbitalElements recovered = convert_and_recover(elements);
        REQUIRE_THAT(recovered.eccentricity, WithinAbs(2.0, E_TOL));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(-1.0e11, A_TOL_LARGE));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, ANG_TOL));
    }
}