#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/rendezvous.h"
#include "../src/test_utilities.h"
#include <cmath>
#include <cstring>

using Catch::Matchers::WithinAbs;

TEST_CASE("Maneuver loading from config", "[maneuver][config]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);

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

    REQUIRE(sim->maneuver_count == 2);
    REQUIRE(std::string(sim->maneuvers[0].name) == "orbit_raise_1");
    REQUIRE(std::string(sim->maneuvers[1].name) == "orbit_raise_2");
    REQUIRE(sim->maneuvers[0].trigger_type == TRIGGER_TIME);
    REQUIRE(sim->maneuvers[1].trigger_type == TRIGGER_TRUE_ANOMALY);
    REQUIRE(sim->maneuvers[0].delta_v == 500.0);

    destroy_simulation(sim);
}

TEST_CASE("Time-based trigger executes at correct time", "[maneuver][trigger][time]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);

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

    REQUIRE(sim->maneuver_count == 2);
    REQUIRE(!sim->maneuvers[0].executed);
    REQUIRE(!sim->maneuvers[1].executed);

    double initial_velocity = vec3_magnitude(sim->spacecraft[0].local_velocity);

    const double BURN_TIME = 3600.0;
    while (sim->time < BURN_TIME + sim->dt) {
        update_simulation(sim);
    }

    REQUIRE(sim->maneuvers[0].executed);
    REQUIRE(fabs(sim->maneuvers[0].executed_time - BURN_TIME) < TIME_STEP);

    double after_velocity = vec3_magnitude(sim->spacecraft[0].local_velocity);
    REQUIRE(after_velocity > initial_velocity);

    destroy_simulation(sim);
}

TEST_CASE("True anomaly trigger executes at correct angle", "[maneuver][trigger][true_anomaly]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);

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

    REQUIRE(sim->maneuver_count == 2);
    REQUIRE(!sim->maneuvers[1].executed);

    double first_burn_velocity = 0.0;

    const double FIRST_BURN_TIME = 3600.0;
    while (sim->time < FIRST_BURN_TIME) {
        update_simulation(sim);
    }

    first_burn_velocity = vec3_magnitude(sim->spacecraft[0].local_velocity);

    const double TARGET_ANOMALY = 3.14159;
    (void)TARGET_ANOMALY;
    double max_sim_time = FIRST_BURN_TIME + 72000.0;

    while (sim->time < max_sim_time) {
        update_simulation(sim);
        if (sim->maneuvers[1].executed) {
            break;
        }
    }

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

    double second_burn_velocity = vec3_magnitude(sim->spacecraft[0].local_velocity);
    double second_burn_kinetic_energy = 0.5 * sim->spacecraft[0].mass *
                                          second_burn_velocity * second_burn_velocity;
    double first_burn_kinetic_energy = 0.5 * sim->spacecraft[0].mass *
                                         first_burn_velocity * first_burn_velocity;

    REQUIRE(second_burn_kinetic_energy > first_burn_kinetic_energy);

    destroy_simulation(sim);
}

TEST_CASE("Maneuvers only execute once", "[maneuver][execution]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);

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

    const double MAX_TIME = 20000.0;
    while (sim->time < MAX_TIME) {
        update_simulation(sim);
    }

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

    double execution_count = 0.0;
    for (int i = 0; i < sim->maneuver_count; i++) {
        if (sim->maneuvers[i].executed) {
            execution_count += 1.0;
        }
    }

    REQUIRE(execution_count == 2.0);

    destroy_simulation(sim);
}

TEST_CASE("Duplicate maneuver names fail config load", "[maneuver][config][error]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);

    bool result = load_system_config(sim, "tests/test_maneuver_planning.toml");
    REQUIRE(result);

    REQUIRE(sim->maneuver_count == 2);

    Maneuver duplicate_maneuver = sim->maneuvers[0];
    sim->maneuvers[sim->maneuver_count] = duplicate_maneuver;

    (void)sim->maneuver_count;
    sim->maneuver_count++;

    bool is_duplicate = (std::string(sim->maneuvers[0].name) ==
                       std::string(sim->maneuvers[2].name));

    REQUIRE(is_duplicate);

    destroy_simulation(sim);
}

TEST_CASE("Time-triggered burn executes at step boundary", "[maneuver][timing][quantization]") {
    const double DT = 10.0;

    SimulationState* sim = create_simulation(10, 10, 100, DT);
    REQUIRE(load_system_config(sim, "tests/test_maneuver_planning.toml"));
    initialize_orbital_objects(sim);

    // Trigger at t=305.0, steps are at 0, 10, 20, ..., 300, 310
    // Should fire at t=310 (5s late)
    const double BURN_TIME = 305.0;

    Maneuver burn = create_maneuver(
        "TestBurst",
        0,
        BURN_PROGRADE,
        10.0,
        TRIGGER_TIME,
        BURN_TIME
    );

    int idx = add_maneuver_to_simulation(sim, &burn);
    REQUIRE(idx >= 0);

    int steps = 0;
    const int MAX_STEPS = 1000;
    while (!sim->maneuvers[idx].executed && steps < MAX_STEPS) {
        update_simulation(sim);
        steps++;
    }

    REQUIRE(sim->maneuvers[idx].executed);
    INFO("Trigger time: " << BURN_TIME);
    INFO("Executed at: " << sim->maneuvers[idx].executed_time);
    INFO("Steps taken: " << steps);
    INFO("Quantization error: " << (sim->maneuvers[idx].executed_time - BURN_TIME) << " s");

    double expected_step = std::ceil(BURN_TIME / DT) * DT;
    INFO("Expected step: " << expected_step);
    REQUIRE(sim->maneuvers[idx].executed_time == expected_step);
    REQUIRE_THAT(sim->maneuvers[idx].executed_time - BURN_TIME, WithinAbs(5.0, 0.01));

    destroy_simulation(sim);
}

TEST_CASE("Time-triggered burn on exact step boundary", "[maneuver][timing][quantization]") {
    const double DT = 10.0;

    SimulationState* sim = create_simulation(10, 10, 100, DT);
    REQUIRE(load_system_config(sim, "tests/test_maneuver_planning.toml"));
    initialize_orbital_objects(sim);

    // Trigger at exact step boundary
    const double BURN_TIME = 300.0;

    Maneuver burn = create_maneuver(
        "TestBurst",
        0,
        BURN_PROGRADE,
        10.0,
        TRIGGER_TIME,
        BURN_TIME
    );

    int idx = add_maneuver_to_simulation(sim, &burn);
    REQUIRE(idx >= 0);

    int steps = 0;
    const int MAX_STEPS = 1000;
    while (!sim->maneuvers[idx].executed && steps < MAX_STEPS) {
        update_simulation(sim);
        steps++;
    }

    REQUIRE(sim->maneuvers[idx].executed);
    INFO("Trigger time: " << BURN_TIME);
    INFO("Executed at: " << sim->maneuvers[idx].executed_time);
    INFO("Steps taken: " << steps);
    INFO("Quantization error: " << (sim->maneuvers[idx].executed_time - BURN_TIME) << " s");

    REQUIRE(sim->maneuvers[idx].executed_time == BURN_TIME);

    destroy_simulation(sim);
}

TEST_CASE("Time-triggered burn quantization error accumulates over long sim", "[maneuver][timing][quantization]") {
    const double DT = 10.0;

    SimulationState* sim = create_simulation(10, 10, 100, DT);
    REQUIRE(load_system_config(sim, "tests/test_maneuver_planning.toml"));
    initialize_orbital_objects(sim);

    // Trigger at a time that's 7s after a step boundary
    const double BURN_TIME = 62807.0;
    double expected_step = std::ceil(BURN_TIME / DT) * DT; // 62810
    double quantization_error = expected_step - BURN_TIME; // 3s

    Maneuver burn = create_maneuver(
        "TestBurst",
        0,
        BURN_PROGRADE,
        10.0,
        TRIGGER_TIME,
        BURN_TIME
    );

    int idx = add_maneuver_to_simulation(sim, &burn);
    REQUIRE(idx >= 0);

    int steps = 0;
    const int MAX_STEPS = 10000;
    while (!sim->maneuvers[idx].executed && steps < MAX_STEPS) {
        update_simulation(sim);
        steps++;
    }

    REQUIRE(sim->maneuvers[idx].executed);
    INFO("Trigger time: " << BURN_TIME);
    INFO("Executed at: " << sim->maneuvers[idx].executed_time);
    INFO("Steps taken: " << steps);
    INFO("Quantization error: " << (sim->maneuvers[idx].executed_time - BURN_TIME) << " s");
    INFO("Expected quantization error: " << quantization_error << " s");

    REQUIRE(sim->maneuvers[idx].executed_time == expected_step);
    REQUIRE_THAT(sim->maneuvers[idx].executed_time - BURN_TIME, WithinAbs(quantization_error, 0.01));

    destroy_simulation(sim);
}

// ============================================================================
// DT Sweep Tests - Measure quantization error at different time steps
// ============================================================================

static int find_spacecraft_by_name(SimulationState* sim, const char* name) {
    for (int i = 0; i < sim->craft_count; i++) {
        if (strcmp(sim->spacecraft[i].name, name) == 0) {
            return i;
        }
    }
    return -1;
}

static double compute_separation(SimulationState* sim, int chaser_idx, int target_idx) {
    Vec3 sep = vec3_sub(sim->spacecraft[chaser_idx].local_position,
                        sim->spacecraft[target_idx].local_position);
    return vec3_magnitude(sep);
}

TEST_CASE("DT sweep: Hohmann transfer separation vs time step", "[rendezvous_hohmann][integration][dt_sweep]") {
    // Run the same Hohmann transfer scenario at multiple DT values
    // and measure the final separation between chaser and target.
    // This reproduces the quantization error documented in
    // docs/planning/hohmann-rendezvous-quantization-fix.md

    const double DT_VALUES[] = {0.1, 0.5, 1.0, 2.0, 5.0, 10.0};
    const int NUM_DT = sizeof(DT_VALUES) / sizeof(DT_VALUES[0]);

    // Expected separations from planning doc (measured at each DT)
    // The actual values will vary based on the specific trigger offset
    // within the step boundary.
    const double EXPECTED_SEP[] = {8.75, 40.0, 55.0, 150.0, 500.0, 1324.0};

    for (int d = 0; d < NUM_DT; d++) {
        const double DT = DT_VALUES[d];

        CAPTURE(DT);

        SimulationState* sim = create_simulation(3, 5, 10, DT);
        REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml"));

        int target_idx = find_spacecraft_by_name(sim, "Target_Satellite");
        int chaser_idx = find_spacecraft_by_name(sim, "Chaser_Lower");
        REQUIRE(target_idx >= 0);
        REQUIRE(chaser_idx >= 0);

        Spacecraft* target = &sim->spacecraft[target_idx];
        Spacecraft* chaser = &sim->spacecraft[chaser_idx];
        CelestialBody* earth = &sim->bodies[0];

        initialize_orbital_objects(sim);

        double r1 = vec3_magnitude(chaser->local_position);
        double r2 = vec3_magnitude(target->local_position);

        // Calculate Hohmann transfer parameters
        HohmannTransfer hohmann = calculate_hohmann_transfer(r1, r2, earth->mass);
        double angular_separation = chaser->orbit.true_anomaly - target->orbit.true_anomaly;
        while (angular_separation > M_PI) angular_separation -= 2.0 * M_PI;
        while (angular_separation < -M_PI) angular_separation += 2.0 * M_PI;
        double wait_time = calculate_next_hohmann_wait_time(r1, r2, angular_separation, earth->mass, DT);
        double arrival_time = wait_time + hohmann.transfer_time;

        // Create departure maneuver
        Maneuver departure = create_maneuver(
            "Departure_Burn",
            chaser_idx,
            BURN_PROGRADE,
            hohmann.dv1,
            TRIGGER_TIME,
            wait_time
        );
        int dep_idx = add_maneuver_to_simulation(sim, &departure);
        REQUIRE(dep_idx >= 0);

        // Create arrival maneuver
        Maneuver arrival = create_maneuver(
            "Circularization_Burn",
            chaser_idx,
            BURN_PROGRADE,
            hohmann.dv2,
            TRIGGER_TIME,
            arrival_time
        );
        int arr_idx = add_maneuver_to_simulation(sim, &arrival);
        REQUIRE(arr_idx >= 0);

        // Run simulation until arrival burn executes
        const int MAX_STEPS = 700000;
        bool transfer_complete = false;
        for (int i = 0; i < MAX_STEPS; i++) {
            update_simulation(sim);
            if (sim->maneuvers[arr_idx].executed && !transfer_complete) {
                transfer_complete = true;
                break;
            }
        }

        REQUIRE(sim->maneuvers[dep_idx].executed);
        REQUIRE(sim->maneuvers[arr_idx].executed);
        REQUIRE(transfer_complete);

        // Measure final separation
        double separation = compute_separation(sim, chaser_idx, target_idx);

        INFO("=== DT Sweep: DT=" << DT << "s ===");
        INFO("  Wait time: " << wait_time << " s");
        INFO("  Arrival time: " << arrival_time << " s");
        INFO("  Departure executed at: " << sim->maneuvers[dep_idx].executed_time << " s");
        INFO("  Arrival executed at: " << sim->maneuvers[arr_idx].executed_time << " s");
        INFO("  Departure quantization error: " << (sim->maneuvers[dep_idx].executed_time - wait_time) << " s");
        INFO("  Arrival quantization error: " << (sim->maneuvers[arr_idx].executed_time - arrival_time) << " s");
        INFO("  Final separation: " << separation << " m");
        INFO("  Chaser eccentricity: " << chaser->orbit.eccentricity);
        INFO("  Expected separation (from doc): " << EXPECTED_SEP[d] << " m");

        // Verify separation is within expected range
        // Use generous margins since quantization error varies with trigger offset
        REQUIRE_THAT(separation, WithinAbs(EXPECTED_SEP[d], EXPECTED_SEP[d] * 0.5 + 50.0));

        // At small DT, eccentricity should be nearly zero
        if (DT <= 1.0) {
            REQUIRE_THAT(chaser->orbit.eccentricity, WithinAbs(0.0, 0.01));
        }

        destroy_simulation(sim);
    }
}

TEST_CASE("DT sweep: quantization error is bounded by DT", "[maneuver][timing][quantization][dt_sweep]") {
    // For TRIGGER_TIME, the quantization error is always in [0, DT).
    // This test verifies that behavior across multiple DT values.

    const double DT_VALUES[] = {1.0, 5.0, 10.0, 30.0};
    const int NUM_DT = sizeof(DT_VALUES) / sizeof(DT_VALUES[0]);

    for (int d = 0; d < NUM_DT; d++) {
        const double DT = DT_VALUES[d];

        CAPTURE(DT);

        // Test multiple trigger offsets within one step.
        // Each offset gets its own simulation to avoid maneuver count limits.
        for (int offset = 1; offset < 10; offset++) {
            const double trigger_time = 100.0 + offset;
            double expected_delay = std::ceil(trigger_time / DT) * DT - trigger_time;

            SimulationState* sim = create_simulation(10, 10, 100, DT);
            REQUIRE(load_system_config(sim, "tests/test_maneuver_planning.toml"));
            initialize_orbital_objects(sim);

            Maneuver burn = create_maneuver(
                "TestBurst",
                0,
                BURN_PROGRADE,
                10.0,
                TRIGGER_TIME,
                trigger_time
            );

            int idx = add_maneuver_to_simulation(sim, &burn);
            REQUIRE(idx >= 0);

            int steps = 0;
            const int MAX_STEPS = 10000;
            while (!sim->maneuvers[idx].executed && steps < MAX_STEPS) {
                update_simulation(sim);
                steps++;
            }

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

            double actual_delay = sim->maneuvers[idx].executed_time - trigger_time;

            // Quantization error must be in [0, DT)
            REQUIRE(actual_delay >= 0.0);
            REQUIRE(actual_delay < DT + 0.01);

            // Should match ceil to step boundary
            double expected_step = std::ceil(trigger_time / DT) * DT;
            REQUIRE(sim->maneuvers[idx].executed_time == expected_step);

            if (offset >= 9) {
                INFO("DT=" << DT << " offset=" << offset
                     << " trigger=" << trigger_time
                     << " executed=" << sim->maneuvers[idx].executed_time
                     << " delay=" << actual_delay
                     << " expected_delay=" << expected_delay);
            }

            destroy_simulation(sim);
        }
    }
}

TEST_CASE("DT sweep: Hohmann arrival burn timing error", "[rendezvous_hohmann][integration][dt_sweep]") {
    // Measure the exact timing error of the arrival burn at different DTs.
    // The arrival burn should fire at the calculated arrival_time, but
    // quantization causes it to fire at the next step boundary.
    // This timing error directly maps to position error at orbital speeds.

    const double DT_VALUES[] = {0.1, 1.0, 5.0, 10.0};
    const int NUM_DT = sizeof(DT_VALUES) / sizeof(DT_VALUES[0]);

    for (int d = 0; d < NUM_DT; d++) {
        const double DT = DT_VALUES[d];

        CAPTURE(DT);

        SimulationState* sim = create_simulation(3, 5, 10, DT);
        REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml"));

        int target_idx = find_spacecraft_by_name(sim, "Target_Satellite");
        int chaser_idx = find_spacecraft_by_name(sim, "Chaser_Lower");
        REQUIRE(target_idx >= 0);
        REQUIRE(chaser_idx >= 0);

        Spacecraft* chaser = &sim->spacecraft[chaser_idx];
        CelestialBody* earth = &sim->bodies[0];

        initialize_orbital_objects(sim);

        double r1 = vec3_magnitude(chaser->local_position);
        double r2 = vec3_magnitude(sim->spacecraft[target_idx].local_position);

        HohmannTransfer hohmann = calculate_hohmann_transfer(r1, r2, earth->mass);
        double angular_separation = chaser->orbit.true_anomaly - sim->spacecraft[target_idx].orbit.true_anomaly;
        while (angular_separation > M_PI) angular_separation -= 2.0 * M_PI;
        while (angular_separation < -M_PI) angular_separation += 2.0 * M_PI;
        double wait_time = calculate_next_hohmann_wait_time(r1, r2, angular_separation, earth->mass, DT);
        double arrival_time = wait_time + hohmann.transfer_time;

        // Create arrival maneuver
        Maneuver arrival = create_maneuver(
            "Circularization_Burn",
            chaser_idx,
            BURN_PROGRADE,
            hohmann.dv2,
            TRIGGER_TIME,
            arrival_time
        );
        int arr_idx = add_maneuver_to_simulation(sim, &arrival);
        REQUIRE(arr_idx >= 0);

        // Run until arrival burn executes
        const int MAX_STEPS = 700000;
        for (int i = 0; i < MAX_STEPS; i++) {
            update_simulation(sim);
            if (sim->maneuvers[arr_idx].executed) {
                break;
            }
        }

        double timing_error = sim->maneuvers[arr_idx].executed_time - arrival_time;
        double chaser_speed = vec3_magnitude(chaser->local_velocity);
        double position_error_estimate = timing_error * chaser_speed;

        INFO("=== Arrival Timing: DT=" << DT << "s ===");
        INFO("  Arrival time (calculated): " << arrival_time << " s");
        INFO("  Arrival time (actual): " << sim->maneuvers[arr_idx].executed_time << " s");
        INFO("  Timing error: " << timing_error << " s");
        INFO("  Chaser speed: " << chaser_speed << " m/s");
        INFO("  Position error estimate: " << position_error_estimate << " m");
        INFO("  Expected timing error < DT: " << (timing_error < DT + 0.01 ? "PASS" : "FAIL"));

        REQUIRE(timing_error >= 0.0);
        REQUIRE(timing_error < DT + 0.01);

        destroy_simulation(sim);
    }
}