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