#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_expected_sma = 13404876.6810;
    const double burn1_expected_v = 8943.1448;
    const double burn1_expected_radius = 7265936.0570;
    const double burn2_expected_radius = 7262462.4116;
    const double cross_expected_radius = 7259786.1864;

    // Propagation-level tolerance constants (coarser than conversion tolerances).
    //
    // NOTE: These tolerances exist because the test measures spacecraft state
    // AFTER update_simulation() returns — i.e. after the full 60s post-burn
    // propagation in the new orbit. The burn itself fires at exact nu=0 (the
    // trigger detects angular_distance(0,0) < 0.01 and sets scheduled_dt=0).
    // The burn happens, the orbit changes, then the craft flies 60s in the
    // new orbit before the test reads craft->local_position. So nu=0.074 rad
    // is the true anomaly 60s after the burn, not the nu at burn time.
    //
    // Plan: add a BurnResult struct (Vec3 position, Vec3 velocity) to Maneuver.
    // populate it in execute_maneuver() before the remaining_dt propagation.
    // The test will then read sim->maneuvers[i].burn_result to get exact
    // burn-time state vectors, eliminating these propagation-level tolerances
    // and allowing assertions like "burn position == periapsis" directly.
    //
    // C++ vs Python state vector agreement at the same simulation step is
    // ~50 microns (floating-point noise), confirming sim_engine.py matches.
    const double PERIAPSIS_TOL = 1.0;        // periapsis preserved by burn
    const double PROP_RADIUS_TOL = 0.001;    // sub-step offset at burn (~50 μm)
    const double PROP_ANGLE_TOL = 0.075;     // nu 60s after burn1 (~0.074 rad)
    const double PROP_TIME_TOL = 28.0;       // period vs time_between (~25.8 s)
    const double CROSS_ANGLE_TOL = 0.009;    // nu 60s after cross burn (~0.009 rad)
    const double PROP_SMA_TOL = 1.0;         // SMA after single burn
    const double PROP_VEL_TOL = 0.1;         // velocity after single burn

    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 v_before = vec3_magnitude(craft->local_velocity);
        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);

        // 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, PERIAPSIS_TOL));

        // Semi-major axis and velocity increase (from precalculated expected values)
        REQUIRE_THAT(final_sma, WithinAbs(burn1_expected_sma, PROP_SMA_TOL));
        REQUIRE_THAT(vec3_magnitude(craft->local_velocity), WithinAbs(burn1_expected_v, PROP_VEL_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("Velocity change: " << (v_before - vec3_magnitude(craft->local_velocity)) << " m/s");
    }

    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");

        double burn1_time = -1.0, burn1_radius = -1.0, burn1_nu = -10.0;
        double burn2_time = -1.0, burn2_radius = -1.0, burn2_nu = -10.0;
        double burn1_period = -1.0;
        Vec3 burn1_pos = {}, burn2_pos = {};
        Vec3 burn1_vel = {}, burn2_vel = {};

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

            if (sim->maneuvers[burn1_idx].executed && burn1_time < 0) {
                burn1_time = sim->time;
                burn1_radius = vec3_magnitude(craft->local_position);
                burn1_nu = craft->orbit.true_anomaly;
                burn1_period = 2.0 * M_PI * sqrt(pow(craft->orbit.semi_major_axis, 3.0) / (G * parent->mass));
                burn1_pos = craft->local_position;
                burn1_vel = craft->local_velocity;
            }

            if (sim->maneuvers[burn2_idx].executed && burn2_time < 0) {
                burn2_time = sim->time;
                burn2_radius = vec3_magnitude(craft->local_position);
                burn2_nu = craft->orbit.true_anomaly;
                burn2_pos = craft->local_position;
                burn2_vel = craft->local_velocity;
            }
        }

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

        // Both burns at expected periapsis-adjacent radius
        REQUIRE_THAT(burn1_radius, WithinAbs(burn1_expected_radius, PROP_RADIUS_TOL));
        REQUIRE_THAT(burn2_radius, WithinAbs(burn2_expected_radius, PROP_RADIUS_TOL));

        // Both at true anomaly ≈ 0 (within 0.1 rad after post-burn propagation)
        REQUIRE_THAT(burn1_nu, WithinAbs(0.0, PROP_ANGLE_TOL));
        REQUIRE_THAT(burn2_nu, WithinAbs(0.0, PROP_ANGLE_TOL));

        // Time between burns ≈ orbital period (within 1 timestep)
        double time_between = burn2_time - burn1_time;
        REQUIRE_THAT(time_between, WithinAbs(burn1_period, PROP_TIME_TOL));

        // Debug info (after assertions so Catch2 captures it)
        INFO("Burn 1: t=" << burn1_time << "s, r=" << burn1_radius << "m, nu=" << burn1_nu << " rad");
        INFO("  pos=" << burn1_pos.x << ", " << burn1_pos.y << ", " << burn1_pos.z);
        INFO("  vel=" << burn1_vel.x << ", " << burn1_vel.y << ", " << burn1_vel.z);
        INFO("Burn 2: t=" << burn2_time << "s, r=" << burn2_radius << "m, nu=" << burn2_nu << " rad");
        INFO("  pos=" << burn2_pos.x << ", " << burn2_pos.y << ", " << burn2_pos.z);
        INFO("  vel=" << burn2_vel.x << ", " << burn2_vel.y << ", " << burn2_vel.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");

        double burn_time = -1.0, burn_radius = -1.0, burn_nu = -10.0;
        const int max_steps = 1000;
        for (int i = 0; i < max_steps && !sim->maneuvers[cross_maneuver].executed; i++) {
            update_simulation(sim);
            if (sim->maneuvers[cross_maneuver].executed) {
                burn_time = sim->time;
                burn_radius = vec3_magnitude(craft_cross->local_position);
                burn_nu = craft_cross->orbit.true_anomaly;
                INFO("Burn at step " << i << ", t=" << burn_time << "s");
                INFO("  radius=" << burn_radius << ", nu=" << burn_nu << " rad");
            }
        }

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

        // Burn radius close to expected periapsis-adjacent radius
        REQUIRE_THAT(burn_radius, WithinAbs(cross_expected_radius, PROP_RADIUS_TOL));

        // True anomaly ≈ 0 at burn (within 0.01 rad after post-burn propagation)
        REQUIRE_THAT(burn_nu, WithinAbs(0.0, CROSS_ANGLE_TOL));
    }

    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);

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

        // Initial radius equals periapsis
        REQUIRE_THAT(r_before, WithinAbs(peri_before, PERIAPSIS_TOL));

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

        INFO("Initial radius: " << r_before << " m");
        INFO("Initial periapsis: " << peri_before << " m");
        INFO("Final periapsis: " << final_periapsis << " m");
    }

    destroy_simulation(sim);
}