#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>
#include <cmath>
#include "../src/orbital_mechanics.h"
#include "../src/orbital_objects.h"
#include "../src/test_utilities.h"
#include "../src/config_loader.h"
#include "../src/simulation.h"

using Catch::Matchers::WithinAbs;

TEST_CASE("Cartesian to Elements - Advanced Tests", "[orbital_mechanics]") {
    const double G = 6.67430e-11;
    const double M_sun = 1.989e30;
    const double mu = G * M_sun;

    SECTION("Circular orbit conversion preserves exact circular parameters") {
        double r = 1.496e11;
        double v_circular = sqrt(mu / r);
        Vec3 position = {r, 0.0, 0.0};
        Vec3 velocity = {0.0, v_circular, 0.0};

        OrbitalElements elements = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(elements.eccentricity, WithinAbs(0.0, 1e-10));
        REQUIRE_THAT(elements.semi_major_axis, WithinAbs(r, 1e3));

        Vec3 converted_position, converted_velocity;
        orbital_elements_to_cartesian(elements, M_sun, &converted_position, &converted_velocity);

        REQUIRE(compare_vec3(position, converted_position, 1e3));
        REQUIRE(compare_vec3(velocity, converted_velocity, 1e-3));
    }

    SECTION("Near-circular orbit (e=0.001) recovers small eccentricity") {
        OrbitalElements elements = {
            .semi_major_axis = 1.496e11,
            .eccentricity = 0.001,
            .true_anomaly = 0.5,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.001, 1e-6));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.496e11, 1e3));
    }

    SECTION("Elliptical orbit (e=0.5) preserves orbital shape") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = 0.8,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
    }

    SECTION("Highly elliptical orbit (e=0.95) preserves extreme eccentricity") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.95,
            .true_anomaly = 0.1,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.95, 1e-3));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
    }

    SECTION("Near-parabolic orbit (e=0.999) recovers near-escape trajectory") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.999,
            .true_anomaly = 0.05,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.999, 1e-3));
    }

    SECTION("Parabolic orbit (e=1.0) recovers escape trajectory") {
        OrbitalElements elements = {
            .semi_latus_rectum = 1.0e11,
            .eccentricity = 1.0,
            .true_anomaly = 0.5,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(1.0, 1e-2));
        REQUIRE_THAT(recovered.semi_latus_rectum, WithinAbs(1.0e11, 1e3));
    }

    SECTION("Hyperbolic orbit (e=2.0) preserves unbound trajectory") {
        OrbitalElements elements = {
            .semi_major_axis = -1.0e11,
            .eccentricity = 2.0,
            .true_anomaly = 0.5,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(2.0, 1e-3));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(-1.0e11, 1e6));
    }

    SECTION("Highly hyperbolic orbit (e=10.0) preserves extreme unbound trajectory") {
        OrbitalElements elements = {
            .semi_major_axis = -1.0e10,
            .eccentricity = 10.0,
            .true_anomaly = 0.8,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(10.0, 1e-3));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(-1.0e10, 1e8));
    }

    SECTION("Zero inclination (i=0) preserves equatorial orbit") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.3,
            .true_anomaly = 0.5,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.inclination, WithinAbs(0.0, 1e-6));
        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.3, 1e-4));
    }

    SECTION("90-degree inclination (i=pi/2) preserves polar orbit") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.2,
            .true_anomaly = 0.6,
            .inclination = M_PI / 2.0,
            .longitude_of_ascending_node = 0.5,
            .argument_of_periapsis = 0.3
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.inclination, WithinAbs(M_PI / 2.0, 1e-4));
        REQUIRE_THAT(recovered.longitude_of_ascending_node, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.argument_of_periapsis, WithinAbs(0.3, 1e-4));
    }

    SECTION("180-degree inclination (i=pi) preserves retrograde orbit") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.2,
            .true_anomaly = 0.6,
            .inclination = M_PI,
            .longitude_of_ascending_node = 0.5,
            .argument_of_periapsis = 0.3
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.inclination, WithinAbs(M_PI, 1e-4));
    }

    SECTION("Periapsis (nu=0) recovers true anomaly correctly") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = 0.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(0.0, 1e-6));
        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
    }

    SECTION("Apoapsis (nu=pi) recovers true anomaly correctly") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = M_PI,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI, 1e-6));
        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
    }

    SECTION("Quadrature point nu=pi/2 (90 deg) preserves orbital elements") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = M_PI / 2.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, 1e-6));
    }

    SECTION("Quadrature point nu=-pi/2 (-90 deg) preserves orbital elements") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = -M_PI / 2.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(3.0 * M_PI / 2.0, 1e-6));
    }

    SECTION("Quadrature point nu=3pi/2 (270 deg) preserves orbital elements") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = 3.0 * M_PI / 2.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(3.0 * M_PI / 2.0, 1e-6));
    }

    SECTION("Quadrature point nu=-3pi/2 (-270 deg) preserves orbital elements") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = -3.0 * M_PI / 2.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, 1e-6));
    }

    SECTION("Quadrature point with high eccentricity (e=0.9) preserves accuracy") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.9,
            .true_anomaly = M_PI / 2.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.9, 1e-3));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e7));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, 1e-5));
    }

    SECTION("Quadrature point with low eccentricity (e=0.1) preserves accuracy") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.1,
            .true_anomaly = M_PI / 2.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.1, 1e-5));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e4));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, 1e-6));
    }

    SECTION("Large true anomaly nu=5.0 rad (approx 286 deg) preserves accuracy") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = 5.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(5.0, 1e-6));
    }

    SECTION("Large negative true anomaly nu=-5.0 rad (approx -286 deg) preserves accuracy") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = -5.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(1.28318530717958623, 1e-6));
    }

    SECTION("Very large true anomaly nu=10.0 rad (approx 573 deg) preserves accuracy") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = 10.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e5));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(10.0 - 2.0 * M_PI, 1e-5));
    }

    SECTION("Quadrature point with 3D orientation preserves all elements") {
        OrbitalElements elements = {
            .semi_major_axis = 1.0e11,
            .eccentricity = 0.5,
            .true_anomaly = M_PI / 2.0,
            .inclination = M_PI / 3.0,
            .longitude_of_ascending_node = M_PI / 4.0,
            .argument_of_periapsis = M_PI / 6.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, 1e-5));
        REQUIRE_THAT(recovered.inclination, WithinAbs(M_PI / 3.0, 1e-4));
        REQUIRE_THAT(recovered.longitude_of_ascending_node, WithinAbs(M_PI / 4.0, 1e-4));
        REQUIRE_THAT(recovered.argument_of_periapsis, WithinAbs(M_PI / 6.0, 1e-4));
    }

    SECTION("Multiple quadrature points in sequence maintain accuracy") {
        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++) {
            OrbitalElements elements = {
                .semi_major_axis = 1.0e11,
                .eccentricity = 0.5,
                .true_anomaly = true_anomalies[i],
                .inclination = 0.0,
                .longitude_of_ascending_node = 0.0,
                .argument_of_periapsis = 0.0
            };

            Vec3 position, velocity;
            orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

            OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

            REQUIRE_THAT(recovered.eccentricity, WithinAbs(0.5, 1e-4));
            REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(1.0e11, 1e6));
            REQUIRE_THAT(recovered.true_anomaly, WithinAbs(true_anomalies[i], 1e-6));
        }
    }

    SECTION("Hyperbolic orbit at quadrature point nu=pi/2") {
        OrbitalElements elements = {
            .semi_major_axis = -1.0e11,
            .eccentricity = 2.0,
            .true_anomaly = M_PI / 2.0,
            .inclination = 0.0,
            .longitude_of_ascending_node = 0.0,
            .argument_of_periapsis = 0.0
        };

        Vec3 position, velocity;
        orbital_elements_to_cartesian(elements, M_sun, &position, &velocity);

        OrbitalElements recovered = cartesian_to_orbital_elements(position, velocity, M_sun);

        REQUIRE_THAT(recovered.eccentricity, WithinAbs(2.0, 1e-3));
        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(-1.0e11, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(M_PI / 2.0, 1e-5));
    }
}