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

// Test prograde burn at periapsis (true anomaly = 0)
// Verifies that the maneuver executes correctly when starting at periapsis
TEST_CASE("Prograde burn at periapsis preserves periapsis distance", "[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];

    // Record initial state (at periapsis)
    double initial_radius = vec3_magnitude(craft->local_position);
    double initial_sma = craft->orbit.semi_major_axis;
    double initial_ecc = craft->orbit.eccentricity;
    double initial_periapsis = initial_sma * (1.0 - initial_ecc);
    double initial_velocity = vec3_magnitude(craft->local_velocity);

    INFO("Initial state:");
    INFO("  Radius: " << initial_radius << " (should equal periapsis: " << initial_periapsis << ")");
    INFO("  True anomaly: " << craft->orbit.true_anomaly);
    INFO("  Velocity: " << initial_velocity);
    INFO("  Maneuver trigger: true_anomaly = " << sim->maneuvers[0].trigger_value);

    // Execute one step
    update_simulation(sim);

    // Check if maneuver executed
    INFO("After 1 step:");
    INFO("  Maneuver executed: " << sim->maneuvers[0].executed);
    INFO("  Final radius: " << vec3_magnitude(craft->local_position));
    INFO("  Final velocity: " << vec3_magnitude(craft->local_velocity));

    // The maneuver should have executed since we started at periapsis
    // This assertion will fail due to the bug - the trigger check happens
    // after physics moves the spacecraft past periapsis
    REQUIRE(sim->maneuvers[0].executed);

    // If the maneuver executed, verify the physics:
    double final_sma = craft->orbit.semi_major_axis;
    double final_ecc = craft->orbit.eccentricity;
    double final_periapsis = final_sma * (1.0 - final_ecc);
    double final_velocity = vec3_magnitude(craft->local_velocity);

    // Periapsis distance should be preserved
    REQUIRE_THAT(final_periapsis, Catch::Matchers::WithinAbs(initial_periapsis, 1.0));

    // Semi-major axis and velocity should increase
    REQUIRE(final_sma > initial_sma);
    REQUIRE(final_velocity > initial_velocity);

    destroy_simulation(sim);
}

TEST_CASE("Prograde burn at periapsis raises apoapsis not periapsis", "[maneuver][periapsis][apoapsis]") {
    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];

    double initial_periapsis = craft->orbit.semi_major_axis * (1.0 - craft->orbit.eccentricity);
    double initial_apoapsis = craft->orbit.semi_major_axis * (1.0 + craft->orbit.eccentricity);

    update_simulation(sim);

    double final_sma = craft->orbit.semi_major_axis;
    double final_ecc = craft->orbit.eccentricity;
    double final_periapsis = final_sma * (1.0 - final_ecc);
    double final_apoapsis = final_sma * (1.0 + final_ecc);

    INFO("Initial: peri=" << initial_periapsis << " apo=" << initial_apoapsis);
    INFO("Final:   peri=" << final_periapsis << " apo=" << final_apoapsis);

    REQUIRE(final_apoapsis > initial_apoapsis);
    REQUIRE_THAT(final_periapsis, Catch::Matchers::WithinAbs(initial_periapsis, 1.0));

    destroy_simulation(sim);
}

TEST_CASE("Burn location equals new periapsis after prograde burn", "[maneuver][periapsis][location]") {
    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];

    double burn_radius = vec3_magnitude(craft->local_position);

    update_simulation(sim);

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

    INFO("Burn radius: " << burn_radius);
    INFO("Final periapsis: " << final_periapsis);

    REQUIRE_THAT(burn_radius, Catch::Matchers::WithinAbs(final_periapsis, 1.0));

    destroy_simulation(sim);
}