#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 <cmath>

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