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

using Catch::Matchers::WithinAbs;

SCENARIO("Extreme eccentricity orbital conversions and vis-viva accuracy",
         "[extreme][eccentricity][high]") {
    const double TIME_STEP = 60.0;
    const double parent_mass = 5.972e24;
    const double mu = G * parent_mass;

    SimulationState* sim = create_simulation(10, 3, 0, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_extreme_eccentricity.toml"));

    Spacecraft* high_e = &sim->spacecraft[0];
    Spacecraft* near_parabolic = &sim->spacecraft[1];
    Spacecraft* hyperbolic = &sim->spacecraft[2];

    // Tolerances
    const double R_TOL = 1e-6;
    const double V_TOL = 1e-6;
    const double E_TOL = 1e-12;
    const double REL_TOL = 1e-8;

    // Precomputed analytical values for spacecraft 0 (a=6.5e8, e=0.99)
    const double a0 = high_e->orbit.semi_major_axis;
    const double e0 = high_e->orbit.eccentricity;
    const double expected_r_peri0 = a0 * (1.0 - e0);    // 6.5e6
    const double expected_r_apo0 = a0 * (1.0 + e0);     // 1.2935e9

    // Precomputed analytical values for spacecraft 1 (a=7.0e8, e=0.99)
    const double a1 = near_parabolic->orbit.semi_major_axis;
    const double e1 = near_parabolic->orbit.eccentricity;
    const double expected_r_peri1 = a1 * (1.0 - e1);    // 7.0e6
    const double expected_r_apo1 = a1 * (1.0 + e1);     // 1.393e9

    // Precomputed analytical values for spacecraft 2 (e=1.05)
    const double e2 = hyperbolic->orbit.eccentricity;
    const double max_nu_hyperbolic = acos(-1.0 / e2);   // ~2.8317 rad

    // Helper: convert elements to cartesian and check vis-viva consistency
    auto check_visviva = [&](const OrbitalElements& orbit, double r, double v) {
        double expected_v_sq = mu * (2.0 / r - 1.0 / orbit.semi_major_axis);
        REQUIRE(expected_v_sq > 0.0);
        const double expected_v = sqrt(expected_v_sq);
        const double rel_err = fabs(v - expected_v) / expected_v;
        INFO("v=" << v << " m/s, v_exp=" << expected_v << " m/s, rel_err=" << rel_err);
        REQUIRE_THAT(rel_err, WithinAbs(0.0, REL_TOL));
    };

    // Helper: convert elements to cartesian at given true anomaly
    auto convert_at_nu = [&](Spacecraft* craft, double nu) {
        craft->orbit.true_anomaly = nu;
        orbital_elements_to_cartesian(craft->orbit, parent_mass, &craft->local_position, &craft->local_velocity);
    };

    // Helper: round-trip check
    auto roundtrip = [&](double a, double e, double nu) {
        OrbitalElements elements = {};
        elements.semi_major_axis = a;
        elements.eccentricity = e;
        elements.true_anomaly = nu;
        Vec3 pos, vel;
        orbital_elements_to_cartesian(elements, parent_mass, &pos, &vel);
        OrbitalElements recovered = cartesian_to_orbital_elements(pos, vel, parent_mass);
        return recovered;
    };

    SECTION("highly elliptical: periapsis radius = a*(1-e)") {
        convert_at_nu(high_e, 0.0);
        const double r = vec3_magnitude(high_e->local_position);
        const double v = vec3_magnitude(high_e->local_velocity);

        INFO("r=" << r << " m, expected=" << expected_r_peri0 << " m");
        INFO("v=" << v << " m/s");

        REQUIRE_THAT(r, WithinAbs(expected_r_peri0, R_TOL));
        REQUIRE_THAT(v, WithinAbs(11046.701562, V_TOL));
        check_visviva(high_e->orbit, r, v);

        // Round-trip eccentricity accuracy
        const OrbitalElements recovered = roundtrip(a0, e0, 0.0);
        INFO("e_recovered=" << recovered.eccentricity << ", error=" << fabs(recovered.eccentricity - e0));
        REQUIRE_THAT(recovered.eccentricity, WithinAbs(e0, E_TOL));
    }

    SECTION("highly elliptical: apoapsis radius = a*(1+e)") {
        convert_at_nu(high_e, M_PI);
        const double r = vec3_magnitude(high_e->local_position);
        const double v = vec3_magnitude(high_e->local_velocity);

        INFO("r=" << r << " m, expected=" << expected_r_apo0 << " m");
        INFO("v=" << v << " m/s");

        REQUIRE_THAT(r, WithinAbs(expected_r_apo0, R_TOL));
        REQUIRE_THAT(v, WithinAbs(55.511063, V_TOL));
        check_visviva(high_e->orbit, r, v);
    }

    SECTION("near-parabolic: periapsis radius") {
        convert_at_nu(near_parabolic, 0.0);
        const double r = vec3_magnitude(near_parabolic->local_position);
        const double v = vec3_magnitude(near_parabolic->local_velocity);

        INFO("r=" << r << " m, expected=" << expected_r_peri1 << " m");
        INFO("v=" << v << " m/s");

        REQUIRE_THAT(r, WithinAbs(expected_r_peri1, R_TOL));
        REQUIRE_THAT(v, WithinAbs(10644.867979, V_TOL));
        check_visviva(near_parabolic->orbit, r, v);
    }

    SECTION("near-parabolic: apoapsis radius") {
        convert_at_nu(near_parabolic, M_PI);
        const double r = vec3_magnitude(near_parabolic->local_position);
        const double v = vec3_magnitude(near_parabolic->local_velocity);

        INFO("r=" << r << " m, expected=" << expected_r_apo1 << " m");
        INFO("v=" << v << " m/s");

        REQUIRE_THAT(r, WithinAbs(expected_r_apo1, R_TOL));
        REQUIRE_THAT(v, WithinAbs(53.491799, V_TOL));
        check_visviva(near_parabolic->orbit, r, v);
    }

    SECTION("near-parabolic: velocity at periapsis and apoapsis") {
        near_parabolic->orbit.true_anomaly = 0.0;
        Vec3 dummy, vel_peri;
        orbital_elements_to_cartesian(near_parabolic->orbit, parent_mass, &dummy, &vel_peri);
        const double v_peri = vec3_magnitude(vel_peri);

        near_parabolic->orbit.true_anomaly = M_PI;
        Vec3 vel_apo;
        orbital_elements_to_cartesian(near_parabolic->orbit, parent_mass, &dummy, &vel_apo);
        const double v_apo = vec3_magnitude(vel_apo);

        INFO("v_peri=" << v_peri << " m/s, v_apo=" << v_apo << " m/s");
        REQUIRE_THAT(v_peri, WithinAbs(10644.867979, V_TOL));
        REQUIRE_THAT(v_apo, WithinAbs(53.491799, V_TOL));
    }

    SECTION("hyperbolic: velocity matches vis-viva") {
        convert_at_nu(hyperbolic, 0.0);
        const double r = vec3_magnitude(hyperbolic->local_position);
        const double v = vec3_magnitude(hyperbolic->local_velocity);

        INFO("r=" << r << " m");
        INFO("v=" << v << " m/s");

        REQUIRE_THAT(v, WithinAbs(11211.998050, V_TOL));
    }

    SECTION("hyperbolic: true anomaly limits") {
        INFO("max_nu=" << max_nu_hyperbolic << " rad (±" << max_nu_hyperbolic * 180.0 / M_PI << "°)");

        // pi and 3pi/2 should be outside hyperbolic range
        const double pi = M_PI;
        const double three_pi_half = 3.0 * M_PI / 2.0;
        INFO("pi=" << pi << " rad, exceeds limit: " << (fabs(pi) >= max_nu_hyperbolic));
        INFO("3pi/2=" << three_pi_half << " rad, exceeds limit: " << (fabs(three_pi_half) >= max_nu_hyperbolic));
        REQUIRE(fabs(pi) >= max_nu_hyperbolic);
        REQUIRE(fabs(three_pi_half) >= max_nu_hyperbolic);
    }

    SECTION("vis-viva accuracy at multiple true anomalies") {
        const std::array<double, 4> true_anomalies = {0.0, M_PI / 2.0, M_PI, 3.0 * M_PI / 2.0};

        for (int i = 0; i < sim->craft_count; i++) {
            Spacecraft* craft = &sim->spacecraft[i];
            const double a = craft->orbit.semi_major_axis;
            const double e = craft->orbit.eccentricity;

            INFO("Spacecraft " << i << ": e=" << e << ", a=" << a);

            for (int j = 0; j < 4; j++) {
                double nu = true_anomalies[j];

                if (e > 1.0) {
                    if (fabs(nu) >= max_nu_hyperbolic) {
                        INFO("  nu=" << nu << " rad: SKIPPED (exceeds hyperbolic limit)");
                        continue;
                    }
                }

                craft->orbit.true_anomaly = nu;
                Vec3 pos, vel;
                orbital_elements_to_cartesian(craft->orbit, parent_mass, &pos, &vel);

                const double r = vec3_magnitude(pos);
                const double v = vec3_magnitude(vel);

                double expected_v_sq = mu * (2.0 / r - 1.0 / a);
                if (expected_v_sq > 0.0) {
                    const double expected_v = sqrt(expected_v_sq);
                    const double rel_err = fabs(v - expected_v) / expected_v;
                    INFO("  nu=" << nu << " rad: v=" << v << " m/s, rel_err=" << rel_err);
                    REQUIRE_THAT(rel_err, WithinAbs(0.0, REL_TOL));
                }
            }
        }
    }

    destroy_simulation(sim);
}