#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/test_utilities.h"
using Catch::Matchers::WithinAbs;

SCENARIO("Maneuver planning and execution", "[maneuver][planning][trigger][config]") {
    const double TIME_STEP = 60.0;
    const double BURN_TIME = 3600.0;
    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);

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

    auto run_until = [&](double target_time) {
        while (sim->time < target_time) {
            update_simulation(sim);
        }
    };

    SECTION("maneuvers load from config with correct properties") {
        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_THAT(sim->maneuvers[0].delta_v, WithinAbs(500.0, D_TOL));
    }

    SECTION("time-based trigger executes at 3600 s") {
        REQUIRE(!sim->maneuvers[0].executed);
        REQUIRE(!sim->maneuvers[1].executed);

        run_until(BURN_TIME + TIME_STEP);

        REQUIRE(sim->maneuvers[0].executed);
        REQUIRE_THAT(sim->maneuvers[0].executed_time, WithinAbs(BURN_TIME, TIME_STEP));

        const double after_velocity = vec3_magnitude(sim->spacecraft[0].local_velocity);
        REQUIRE_THAT(after_velocity, WithinAbs(8.170251503359999e3, V_TOL));
    }

    SECTION("true anomaly trigger executes after first burn") {
        REQUIRE(!sim->maneuvers[1].executed);

        run_until(BURN_TIME + TIME_STEP);
        REQUIRE(sim->maneuvers[0].executed);

        const double max_sim_time = BURN_TIME + 72000.0;
        while (sim->time < max_sim_time) {
            update_simulation(sim);
            if (sim->maneuvers[1].executed) {
                break;
            }
        }

        REQUIRE(sim->maneuvers[1].executed);
        REQUIRE_THAT(vec3_magnitude(sim->spacecraft[0].local_velocity),
                     WithinAbs(8.672299586435140e3, V_TOL));
    }

    SECTION("maneuvers execute only once") {
        run_until(20000.0);

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

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

    destroy_simulation(sim);
}