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

using Catch::Matchers::WithinAbs;

SCENARIO("Periapsis-triggered prograde burn behavior", "[maneuver][periapsis]") {
    const double TIME_STEP = 60.0;
    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_periapsis_burn.toml"));

    Spacecraft* craft = &sim->spacecraft[0];
    Spacecraft* craft_cross = &sim->spacecraft[1];
    CelestialBody* parent = &sim->bodies[craft->parent_index];

    // Shared fixture values (from precalc_periapsis_burn.py)
    const double initial_periapsis = 7259700.0;
    const double burn1_preburn_v = 8448.412303782408344;
    const double burn1_expected_sma = 13404876.681005753576756;

    // BurnResult captures exact pre-burn state vectors, enabling tight
    // tolerances (R_TOL, ANG_TOL) for periapsis assertions.
    // Propagation-level tolerances (A_TOL*10, V_TOL*100, M_TOL*10) remain
    // for post-burn+60s-propagation state comparisons.

    SECTION("spacecraft loads correctly") {
        REQUIRE(sim->craft_count == 2);
        REQUIRE(std::string(sim->spacecraft[0].name) == "TestSatellite");
        REQUIRE(std::string(sim->spacecraft[1].name) == "TestSatelliteCrossing");
        REQUIRE(sim->spacecraft[0].parent_index == 1);
        REQUIRE(sim->spacecraft[1].parent_index == 1);
    }

    SECTION("prograde burn at periapsis fires immediately and raises orbit") {
        double a_before = craft->orbit.semi_major_axis;
        double e_before = craft->orbit.eccentricity;
        double peri_before = a_before * (1.0 - e_before);

        // Execute one step — burn fires immediately (nu=0, trigger=0)
        update_simulation(sim);

        // Verify burn fired at exact periapsis via burn_result
        const BurnResult& br = sim->maneuvers[0].burn_result;
        REQUIRE(br.valid);
        REQUIRE_THAT(br.true_anomaly, WithinAbs(0.0, ANG_TOL));
        REQUIRE_THAT(vec3_magnitude(br.position), WithinAbs(initial_periapsis, R_TOL));

        // Maneuver executed
        REQUIRE(sim->maneuvers[0].executed);

        // Periapsis preserved after burn
        double final_sma = craft->orbit.semi_major_axis;
        double final_ecc = craft->orbit.eccentricity;
        double final_periapsis = final_sma * (1.0 - final_ecc);
        REQUIRE_THAT(final_periapsis, WithinAbs(initial_periapsis, R_TOL));

        // Pre-burn velocity captured at exact burn time (tight tolerance)
        REQUIRE_THAT(vec3_magnitude(br.velocity), WithinAbs(burn1_preburn_v, V_TOL));

        // Semi-major axis after burn
        REQUIRE_THAT(final_sma, WithinAbs(burn1_expected_sma, A_TOL));

        INFO("Initial SMA: " << a_before << " m");
        INFO("Final SMA: " << final_sma << " m");
        INFO("Initial periapsis: " << peri_before << " m");
        INFO("Final periapsis: " << final_periapsis << " m");
        INFO("Burn time position: " << br.position.x << ", " << br.position.y << ", " << br.position.z);
        INFO("Burn time velocity: " << br.velocity.x << ", " << br.velocity.y << ", " << br.velocity.z);
    }

    SECTION("two sequential periapsis burns execute at same location") {
        // Find maneuver indices for craft 0
        int burn1_idx = -1, burn2_idx = -1;
        for (int i = 0; i < sim->maneuver_count; i++) {
            if (sim->maneuvers[i].craft_index == 0 && !sim->maneuvers[i].executed) {
                if (burn1_idx < 0) burn1_idx = i;
                else burn2_idx = i;
            }
        }
        REQUIRE(burn1_idx >= 0);
        REQUIRE(burn2_idx >= 0);

        double initial_periapsis_val = craft->orbit.semi_major_axis * (1.0 - craft->orbit.eccentricity);
        double initial_apoapsis_val = craft->orbit.semi_major_axis * (1.0 + craft->orbit.eccentricity);
        INFO("Initial periapsis: " << initial_periapsis_val << " m");
        INFO("Initial apoapsis: " << initial_apoapsis_val << " m");

        const int max_steps = 300;
        for (int i = 0; i < max_steps; i++) {
            update_simulation(sim);
        }

        REQUIRE(sim->maneuvers[burn1_idx].executed);
        REQUIRE(sim->maneuvers[burn2_idx].executed);

        // Read exact burn-time state from burn_result
        const BurnResult& br1 = sim->maneuvers[burn1_idx].burn_result;
        const BurnResult& br2 = sim->maneuvers[burn2_idx].burn_result;
        REQUIRE(br1.valid);
        REQUIRE(br2.valid);

        double burn1_radius = vec3_magnitude(br1.position);
        double burn2_radius = vec3_magnitude(br2.position);
        double burn1_time = sim->maneuvers[burn1_idx].executed_time;
        double burn2_time = sim->maneuvers[burn2_idx].executed_time;

        // Both burns at exact periapsis radius
        REQUIRE_THAT(burn1_radius, WithinAbs(initial_periapsis, R_TOL));
        REQUIRE_THAT(burn2_radius, WithinAbs(initial_periapsis, R_TOL));

        // Both at exact true anomaly = 0 (burn_result captures pre-burn state)
        REQUIRE_THAT(br1.true_anomaly, WithinAbs(0.0, ANG_TOL));
        REQUIRE_THAT(br2.true_anomaly, WithinAbs(0.0, ANG_TOL));

        // Both burns at same radius (same periapsis location)
        REQUIRE_THAT(burn1_radius, WithinAbs(burn2_radius, R_TOL));

        // Time between burns ≈ orbital period
        double time_between = burn2_time - burn1_time;
        double burn1_period = 2.0 * M_PI * sqrt(pow(burn1_expected_sma, 3.0) / (G * parent->mass));
        REQUIRE_THAT(time_between, WithinAbs(burn1_period, M_TOL * 10));

        // Debug info (after assertions so Catch2 captures it)
        INFO("Burn 1: t=" << burn1_time << "s, r=" << burn1_radius << "m, nu=" << br1.true_anomaly << " rad");
        INFO("  pos=" << br1.position.x << ", " << br1.position.y << ", " << br1.position.z);
        INFO("  vel=" << br1.velocity.x << ", " << br1.velocity.y << ", " << br1.velocity.z);
        INFO("Burn 2: t=" << burn2_time << "s, r=" << burn2_radius << "m, nu=" << br2.true_anomaly << " rad");
        INFO("  pos=" << br2.position.x << ", " << br2.position.y << ", " << br2.position.z);
        INFO("  vel=" << br2.velocity.x << ", " << br2.velocity.y << ", " << br2.velocity.z);
        INFO("Time between burns: " << time_between << " s");
        INFO("Expected period: " << burn1_period << " s");
        REQUIRE(true);  // dummy to capture INFO
    }

    SECTION("periapsis burn fires when crossing from 90 degrees") {
        int cross_maneuver = -1;
        for (int i = 0; i < sim->maneuver_count; i++) {
            if (sim->maneuvers[i].craft_index == 1) {
                cross_maneuver = i;
                break;
            }
        }
        REQUIRE(cross_maneuver >= 0);

        double cross_initial_periapsis = craft_cross->orbit.semi_major_axis * (1.0 - craft_cross->orbit.eccentricity);
        double cross_initial_apoapsis = craft_cross->orbit.semi_major_axis * (1.0 + craft_cross->orbit.eccentricity);
        INFO("Initial true anomaly: " << craft_cross->orbit.true_anomaly << " rad");
        INFO("Initial periapsis: " << cross_initial_periapsis << " m");
        INFO("Initial apoapsis: " << cross_initial_apoapsis << " m");

        const int max_steps = 1000;
        for (int i = 0; i < max_steps && !sim->maneuvers[cross_maneuver].executed; i++) {
            update_simulation(sim);
        }

        REQUIRE(sim->maneuvers[cross_maneuver].executed);

        // Read exact burn-time state from burn_result
        const BurnResult& br = sim->maneuvers[cross_maneuver].burn_result;
        REQUIRE(br.valid);

        double burn_radius = vec3_magnitude(br.position);

        // Burn at exact periapsis radius
        REQUIRE_THAT(burn_radius, WithinAbs(cross_initial_periapsis, R_TOL));

        // True anomaly = 0 at burn (burn_result captures pre-burn state)
        REQUIRE_THAT(br.true_anomaly, WithinAbs(0.0, ANG_TOL));

        INFO("Burn at step " << max_steps << ", t=" << sim->maneuvers[cross_maneuver].executed_time << "s");
        INFO("  radius=" << burn_radius << ", nu=" << br.true_anomaly << " rad");
        INFO("  pos=" << br.position.x << ", " << br.position.y << ", " << br.position.z);
        INFO("  vel=" << br.velocity.x << ", " << br.velocity.y << ", " << br.velocity.z);
    }

    SECTION("burn location equals new periapsis after prograde burn") {
        double a_before = craft->orbit.semi_major_axis;
        double e_before = craft->orbit.eccentricity;
        double peri_before = a_before * (1.0 - e_before);
        double r_before = vec3_magnitude(craft->local_position);

        update_simulation(sim);

        REQUIRE(sim->maneuvers[0].executed);

        // Verify burn happened at periapsis via burn_result
        const BurnResult& br = sim->maneuvers[0].burn_result;
        REQUIRE(br.valid);
        REQUIRE_THAT(vec3_magnitude(br.position), WithinAbs(peri_before, R_TOL));
        REQUIRE_THAT(br.true_anomaly, WithinAbs(0.0, ANG_TOL));

        double final_periapsis = craft->orbit.semi_major_axis * (1.0 - craft->orbit.eccentricity);

        // Final periapsis equals initial periapsis (burn at periapsis preserves it)
        REQUIRE_THAT(final_periapsis, WithinAbs(peri_before, R_TOL));

        INFO("Initial radius: " << r_before << " m");
        INFO("Initial periapsis: " << peri_before << " m");
        INFO("Final periapsis: " << final_periapsis << " m");
        INFO("Burn_result radius: " << vec3_magnitude(br.position) << " m");
    }

    destroy_simulation(sim);
}