#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/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("Two periapsis burns execute at same location", "[maneuver][periapsis][sequential]") {
    const double TIME_STEP = 60.0;
    const int ORBIT_STEPS = 300;
    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);

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

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

    int maneuver_indices[2];
    int maneuver_count_for_craft = 0;
    for (int i = 0; i < sim->maneuver_count; i++) {
        if (sim->maneuvers[i].craft_index == 0) {
            maneuver_indices[maneuver_count_for_craft++] = i;
        }
    }
    REQUIRE(maneuver_count_for_craft == 2);

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

    INFO("Initial orbit:");
    INFO("  Periapsis: " << initial_periapsis);
    INFO("  Apoapsis: " << initial_apoapsis);
    INFO("  Eccentricity: " << craft->orbit.eccentricity);

    double burn1_time = -1.0;
    double burn1_radius = -1.0;
    double burn1_true_anomaly = -10.0;
    double burn1_period = -1.0;
    double burn2_time = -1.0;
    double burn2_radius = -1.0;
    double burn2_true_anomaly = -10.0;

    for (int i = 0; i < ORBIT_STEPS; i++) {
        update_simulation(sim);

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

            Vec3 r = craft->local_position;
            Vec3 v = craft->local_velocity;
            Vec3 h = vec3_cross(r, v);
            Vec3 e_vec = calculate_eccentricity_vector(r, v, h, G * parent->mass);
            double e_mag = vec3_magnitude(e_vec);
            burn1_true_anomaly = calculate_true_anomaly(r, v, e_vec, e_mag, burn1_radius);
            burn1_true_anomaly = normalize_angle(burn1_true_anomaly);

            INFO("First burn executed at step " << i);
            INFO("  Time: " << burn1_time);
            INFO("  Radius: " << burn1_radius);
            INFO("  True anomaly: " << burn1_true_anomaly << " rad (" << burn1_true_anomaly * 180.0 / M_PI << "°)");
            INFO("  Periapsis: " << initial_periapsis);
            INFO("  Apoapsis: " << initial_apoapsis);
            INFO("  New period: " << burn1_period << " seconds");
        }

        if (sim->maneuvers[maneuver_indices[1]].executed && burn2_time < 0) {
            burn2_time = sim->time;
            burn2_radius = vec3_magnitude(craft->local_position);

            Vec3 r = craft->local_position;
            Vec3 v = craft->local_velocity;
            Vec3 h = vec3_cross(r, v);
            Vec3 e_vec = calculate_eccentricity_vector(r, v, h, G * parent->mass);
            double e_mag = vec3_magnitude(e_vec);
            burn2_true_anomaly = calculate_true_anomaly(r, v, e_vec, e_mag, burn2_radius);
            burn2_true_anomaly = normalize_angle(burn2_true_anomaly);

            INFO("Second burn executed at step " << i);
            INFO("  Time: " << burn2_time);
            INFO("  Radius: " << burn2_radius);
            INFO("  True anomaly: " << burn2_true_anomaly << " rad (" << burn2_true_anomaly * 180.0 / M_PI << "°)");
        }
    }

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

    INFO("Burn comparison:");
    INFO("  Burn 1: time=" << burn1_time << ", radius=" << burn1_radius << ", true_anomaly=" << burn1_true_anomaly);
    INFO("  Burn 2: time=" << burn2_time << ", radius=" << burn2_radius << ", true_anomaly=" << burn2_true_anomaly);

    REQUIRE_THAT(burn1_radius, Catch::Matchers::WithinAbs(initial_periapsis, 10000.0));
    REQUIRE_THAT(burn2_radius, Catch::Matchers::WithinAbs(initial_periapsis, 10000.0));

    REQUIRE(fabs(burn1_true_anomaly) < 0.5);
    REQUIRE(fabs(burn2_true_anomaly) < 0.5);

    INFO("Expected orbital period (after burn 1): " << burn1_period << " seconds");
    INFO("Actual time between burns: " << (burn2_time - burn1_time) << " seconds");

    REQUIRE_THAT(burn2_time - burn1_time, Catch::Matchers::WithinAbs(burn1_period, TIME_STEP * 2.0));

    destroy_simulation(sim);
}

TEST_CASE("Periapsis burn fires when crossing periapsis", "[maneuver][periapsis][crossing]") {
    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[1];  // TestSatelliteCrossing
    CelestialBody* parent = &sim->bodies[craft->parent_index];

    int maneuver_index = -1;
    for (int i = 0; i < sim->maneuver_count; i++) {
        if (sim->maneuvers[i].craft_index == 1) {
            maneuver_index = i;
            break;
        }
    }
    REQUIRE(maneuver_index >= 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);

    INFO("Initial orbit:");
    INFO("  Periapsis: " << initial_periapsis);
    INFO("  Apoapsis: " << initial_apoapsis);
    INFO("  Initial true anomaly: " << craft->orbit.true_anomaly << " rad");

    double burn_time = -1.0;
    double burn_radius = -1.0;
    double burn_true_anomaly = -10.0;

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

        if (sim->maneuvers[maneuver_index].executed) {
            burn_time = sim->time;
            burn_radius = vec3_magnitude(craft->local_position);

            Vec3 r = craft->local_position;
            Vec3 v = craft->local_velocity;
            Vec3 h = vec3_cross(r, v);
            Vec3 e_vec = calculate_eccentricity_vector(r, v, h, G * parent->mass);
            double e_mag = vec3_magnitude(e_vec);
            burn_true_anomaly = calculate_true_anomaly(r, v, e_vec, e_mag, burn_radius);
            burn_true_anomaly = normalize_angle(burn_true_anomaly);

            INFO("Burn executed at step " << i);
            INFO("  Time: " << burn_time);
            INFO("  Radius: " << burn_radius);
            INFO("  True anomaly: " << burn_true_anomaly << " rad (" << burn_true_anomaly * 180.0 / M_PI << "°)");
            INFO("  Periapsis: " << initial_periapsis);
            INFO("  Apoapsis: " << initial_apoapsis);
        }
    }

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

    REQUIRE_THAT(burn_radius, Catch::Matchers::WithinAbs(initial_periapsis, 1000.0));

    REQUIRE(fabs(burn_true_anomaly) < 0.5);

    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 initial_periapsis = craft->orbit.semi_major_axis * (1.0 - craft->orbit.eccentricity);
    double initial_radius = 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);

    INFO("Initial radius: " << initial_radius);
    INFO("Initial periapsis: " << initial_periapsis);
    INFO("Final periapsis: " << final_periapsis);

    REQUIRE_THAT(initial_radius, Catch::Matchers::WithinAbs(initial_periapsis, 100.0));

    REQUIRE_THAT(final_periapsis, Catch::Matchers::WithinAbs(initial_periapsis, 100.0));

    destroy_simulation(sim);
}