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

using Catch::Matchers::WithinAbs;

SCENARIO("Barker's equation solves parabolic mean anomaly",
         "[barker][analytical][parabolic]") {
    const double PARENT_MASS = 1.989e30;
    const double TIME_STEP = 3600.0;
    const int NUM_STEPS = 24;

    auto check_roundtrip = [&](double M_original, double tol) {
        double nu = solve_barker_equation(M_original);
        double D = tan(nu / 2.0);
        double M_recovered = D + (D * D * D) / 3.0;
        REQUIRE_THAT(M_recovered, WithinAbs(M_original, tol));
    };

    SECTION("zero mean anomaly yields zero true anomaly") {
        double M = 0.0;
        double nu = solve_barker_equation(M);
        REQUIRE_THAT(nu, WithinAbs(0.0, 1e-15));
    }

    SECTION("positive mean anomaly values") {
        std::vector<std::pair<double, double>> tests = {
            std::make_pair(0.1, 1e-14), std::make_pair(1.0, 1e-14),
            std::make_pair(5.0, 1e-14), std::make_pair(20.0, 1e-13)
        };
        for (const auto& p : tests) {
            double nu = solve_barker_equation(p.first);
            REQUIRE(nu > 0.0);
            REQUIRE(nu < M_PI);
            check_roundtrip(p.first, p.second);
        }
    }

    SECTION("negative mean anomaly values") {
        std::vector<std::pair<double, double>> tests = {
            std::make_pair(-0.1, 1e-14), std::make_pair(-1.0, 1e-14),
            std::make_pair(-5.0, 1e-14)
        };
        for (const auto& p : tests) {
            double nu = solve_barker_equation(p.first);
            REQUIRE(nu < 0.0);
            REQUIRE(nu > -M_PI);
            check_roundtrip(p.first, p.second);
        }
    }

    SECTION("round-trip across full range") {
        std::vector<double> test_values = {-10.0, -5.0, -1.0, -0.5, -0.1,
                                            0.0, 0.1, 0.5, 1.0, 5.0, 10.0};
        for (double M_original : test_values) {
            check_roundtrip(M_original, 1e-13);
        }
    }

    SECTION("true anomaly stays within (-pi, pi) for M in [-50, 50]") {
        for (double M = -50.0; M <= 50.0; M += 1.0) {
            double nu = solve_barker_equation(M);
            REQUIRE(nu > -M_PI * 0.99);
            REQUIRE(nu < M_PI * 0.99);
        }
    }

    SECTION("parabolic orbit propagation preserves energy") {
        OrbitalElements initial = {};
        initial.semi_latus_rectum = 2.992e11;
        initial.eccentricity = 1.0;
        initial.true_anomaly = 0.0;
        initial.inclination = 0.0;
        initial.longitude_of_ascending_node = 0.0;
        initial.argument_of_periapsis = 0.0;

        Vec3 pos, vel;
        orbital_elements_to_cartesian(initial, PARENT_MASS, &pos, &vel);

        const double initial_distance = vec3_magnitude(pos);
        const double initial_velocity = vec3_magnitude(vel);
        const double escape_velocity = sqrt(2.0 * G * PARENT_MASS / initial_distance);

        INFO("Initial distance: " << initial_distance / 1.496e11 << " AU");
        INFO("Initial velocity: " << initial_velocity / 1000.0 << " km/s");
        INFO("Escape velocity: " << escape_velocity / 1000.0 << " km/s");
        REQUIRE_THAT(initial_velocity, WithinAbs(escape_velocity, 1.0));

        OrbitalElements current = initial;
        for (int step = 0; step < NUM_STEPS; step++) {
            current = propagate_orbital_elements(current, TIME_STEP, PARENT_MASS);
        }

        Vec3 pos_final, vel_final;
        orbital_elements_to_cartesian(current, PARENT_MASS, &pos_final, &vel_final);

        const double final_distance = vec3_magnitude(pos_final);
        const double final_velocity = vec3_magnitude(vel_final);
        const double final_escape_velocity = sqrt(2.0 * G * PARENT_MASS / final_distance);

        INFO("Final true anomaly: " << current.true_anomaly << " rad");
        INFO("Final distance: " << final_distance / 1.496e11 << " AU");
        INFO("Final velocity: " << final_velocity / 1000.0 << " km/s");
        INFO("Final escape velocity: " << final_escape_velocity / 1000.0 << " km/s");

        REQUIRE(final_distance > initial_distance);
        REQUIRE(final_velocity < initial_velocity);
        REQUIRE_THAT(final_velocity, WithinAbs(final_escape_velocity, 1.0));
    }
}