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

using Catch::Matchers::WithinAbs;

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

    SECTION("Quadrature point ν=π/2 (90°) 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 ν=-π/2 (-90°) 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 ν=3π/2 (270°) 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 ν=-3π/2 (-270°) 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 ν=5.0 rad (≈286°) 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("Large negative true anomaly ν=-5.0 rad (≈-286°) 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 ν=10.0 rad (≈573°) 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, 1e6));
        REQUIRE_THAT(recovered.true_anomaly, WithinAbs(-2.56637061435917246, 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 ν=π/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));
    }
}