#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>
#include "../src/physics.h"
#include "../src/orbital_mechanics.h"
#include "../src/simulation.h"
#include "../src/spacecraft.h"
#include "../src/maneuver.h"
#include "../src/config_loader.h"
#include "../src/test_utilities.h"
#include <cmath>
#include <cstring>

const double POSITION_TOLERANCE = 1e-3;
const double VELOCITY_TOLERANCE = 1e-3;
const double ELEMENT_TOLERANCE = 1e-6;
const double ENERGY_TOLERANCE = 1e-6;

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

void execute_maneuver_by_name(SimulationState* sim, const char* maneuver_name, Spacecraft* craft) {
    int maneuver_index = find_maneuver_by_name(sim, maneuver_name);
    REQUIRE(maneuver_index >= 0);

    Maneuver* maneuver = &sim->maneuvers[maneuver_index];
    REQUIRE(!maneuver->executed);

    // Set simulation time to trigger (for time-based triggers)
    if (maneuver->trigger_type == TRIGGER_TIME) {
        sim->time = maneuver->trigger_value;
    }

    // Execute maneuver
    execute_maneuver(maneuver, craft, sim->time);

    // Verify execution
    REQUIRE(maneuver->executed);
    REQUIRE(maneuver->executed_time == sim->time);
}

TEST_CASE("Config loading for hybrid impulse burns", "[hybrid][impulse][config]") {
    const double TIME_STEP = 60.0;

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

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

    REQUIRE(sim->body_count == 2);
    REQUIRE(std::string(sim->bodies[0].name) == "Sun");
    REQUIRE(std::string(sim->bodies[1].name) == "Earth");

    REQUIRE(sim->craft_count == 6);

    REQUIRE(std::string(sim->spacecraft[0].name) == "Hohmann_Transfer");
    REQUIRE(sim->spacecraft[0].parent_index == 1);

    REQUIRE(std::string(sim->spacecraft[1].name) == "Plane_Change");
    REQUIRE(sim->spacecraft[1].parent_index == 1);

    REQUIRE(std::string(sim->spacecraft[2].name) == "Periapsis_Burn");
    REQUIRE(sim->spacecraft[2].parent_index == 1);

    REQUIRE(std::string(sim->spacecraft[3].name) == "Apoapsis_Burn");
    REQUIRE(sim->spacecraft[3].parent_index == 1);

    REQUIRE(std::string(sim->spacecraft[4].name) == "Small_Delta_v");
    REQUIRE(sim->spacecraft[4].parent_index == 1);

    REQUIRE(std::string(sim->spacecraft[5].name) == "Large_Delta_v");
    REQUIRE(sim->spacecraft[5].parent_index == 1);

    REQUIRE(sim->maneuver_count == 7);

    destroy_simulation(sim);
}

SCENARIO("Hohmann transfer with two burns", "[hybrid][impulse][hohmann]") {
    const double TIME_STEP = 60.0;

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

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

    Spacecraft* craft = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[1];

    Vec3 initial_pos;
    Vec3 initial_vel;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &initial_pos, &initial_vel);
    craft->local_position = initial_pos;
    craft->local_velocity = initial_vel;

    OrbitalElements initial_elements = craft->orbit;

    SECTION("First burn at perigee raises apogee") {
        double initial_velocity_mag = vec3_magnitude(initial_vel);

        // Execute first maneuver via maneuver system
        execute_maneuver_by_name(sim, "hohmann_burn_1", craft);

        double new_velocity_mag = vec3_magnitude(craft->local_velocity);

        REQUIRE(new_velocity_mag > initial_velocity_mag);

        Vec3 new_pos = craft->local_position;
        Vec3 new_vel = craft->local_velocity;

        OrbitalElements new_elements = cartesian_to_orbital_elements(new_pos, new_vel, earth->mass);

        INFO("Initial a: " << initial_elements.semi_major_axis);
        INFO("New a: " << new_elements.semi_major_axis);
        INFO("Initial e: " << initial_elements.eccentricity);
        INFO("New e: " << new_elements.eccentricity);

        REQUIRE(new_elements.semi_major_axis > initial_elements.semi_major_axis);
        REQUIRE(new_elements.eccentricity > initial_elements.eccentricity);
    }

    SECTION("Second burn at apogee circularizes orbit") {
        // Execute first burn
        execute_maneuver_by_name(sim, "hohmann_burn_1", craft);

        OrbitalElements after_first_burn = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        // Set up position at apogee (true_anomaly = PI)
        OrbitalElements apogee_elements = after_first_burn;
        apogee_elements.true_anomaly = M_PI;

        Vec3 apogee_pos;
        Vec3 apogee_vel;
        orbital_elements_to_cartesian(apogee_elements, earth->mass, &apogee_pos, &apogee_vel);
        craft->local_position = apogee_pos;
        craft->local_velocity = apogee_vel;

        // Execute second maneuver via maneuver system
        execute_maneuver_by_name(sim, "hohmann_burn_2", craft);

        Vec3 final_pos = craft->local_position;
        Vec3 final_vel = craft->local_velocity;

        OrbitalElements final_elements = cartesian_to_orbital_elements(final_pos, final_vel, earth->mass);

        INFO("After first burn a: " << after_first_burn.semi_major_axis);
        INFO("After first burn e: " << after_first_burn.eccentricity);
        INFO("Final a: " << final_elements.semi_major_axis);
        INFO("Final e: " << final_elements.eccentricity);

        REQUIRE(final_elements.semi_major_axis > after_first_burn.semi_major_axis);
        REQUIRE(final_elements.eccentricity < after_first_burn.eccentricity);
        REQUIRE(final_elements.eccentricity < 0.1);
    }

    destroy_simulation(sim);
}

SCENARIO("Large burns (Δv > orbital velocity)", "[hybrid][impulse][large_delta_v]") {
    const double TIME_STEP = 60.0;

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

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

    Spacecraft* craft = &sim->spacecraft[5];
    CelestialBody* earth = &sim->bodies[1];

    Vec3 initial_pos;
    Vec3 initial_vel;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &initial_pos, &initial_vel);
    craft->local_position = initial_pos;
    craft->local_velocity = initial_vel;

    OrbitalElements initial_elements = cartesian_to_orbital_elements(initial_pos, initial_vel, earth->mass);

    double initial_velocity_mag = vec3_magnitude(initial_vel);
    double escape_velocity = sqrt(2.0 * G * earth->mass / vec3_magnitude(initial_pos));

    SECTION("Large prograde burn produces hyperbolic orbit") {
        INFO("Initial velocity: " << initial_velocity_mag << " m/s");
        INFO("Escape velocity: " << escape_velocity << " m/s");

        // Execute large burn via maneuver system
        execute_maneuver_by_name(sim, "large_burn", craft);

        double final_velocity_mag = vec3_magnitude(craft->local_velocity);
        INFO("Final velocity: " << final_velocity_mag << " m/s");

        REQUIRE(final_velocity_mag > escape_velocity);

        OrbitalElements new_elements = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        INFO("Initial e: " << initial_elements.eccentricity);
        INFO("New e: " << new_elements.eccentricity);

        REQUIRE(new_elements.eccentricity > 1.0);
        REQUIRE(new_elements.semi_major_axis < 0);
    }

    SECTION("Large burn produces correct hyperbolic trajectory") {
        // Execute large burn via maneuver system
        execute_maneuver_by_name(sim, "large_burn", craft);

        OrbitalElements new_elements = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        double final_velocity_mag = vec3_magnitude(craft->local_velocity);
        double r = vec3_magnitude(craft->local_position);
        double vis_viva_expected = final_velocity_mag * final_velocity_mag;
        double vis_viva_calculated = G * earth->mass * (2.0 / r - 1.0 / new_elements.semi_major_axis);

        INFO("Vis-viva expected: " << vis_viva_expected);
        INFO("Vis-viva calculated: " << vis_viva_calculated);

        double vis_viva_error = fabs(vis_viva_expected - vis_viva_calculated) / vis_viva_expected;
        REQUIRE(vis_viva_error < 1e-6);
    }

    destroy_simulation(sim);
}

SCENARIO("Energy conservation during burns", "[hybrid][impulse][energy]") {
    const double TIME_STEP = 60.0;

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

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

    Spacecraft* craft = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[1];

    Vec3 initial_pos;
    Vec3 initial_vel;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &initial_pos, &initial_vel);
    craft->local_position = initial_pos;
    craft->local_velocity = initial_vel;

    double initial_ke = 0.5 * craft->mass * vec3_dot(craft->local_velocity, craft->local_velocity);
    double initial_pe = -G * craft->mass * earth->mass / vec3_magnitude(craft->local_position);
    double initial_total_energy = initial_ke + initial_pe;

    SECTION("Prograde burn increases total energy") {
        double delta_v = 1000.0;
        Vec3 v_initial = craft->local_velocity;

        // Get maneuver delta_v from config
        int maneuver_index = find_maneuver_by_name(sim, "hohmann_burn_1");
        REQUIRE(maneuver_index >= 0);
        Maneuver* maneuver = &sim->maneuvers[maneuver_index];
        delta_v = maneuver->delta_v;

        // Execute burn via maneuver system
        execute_maneuver_by_name(sim, "hohmann_burn_1", craft);

        Vec3 v_final = craft->local_velocity;
        Vec3 dv = vec3_sub(v_final, v_initial);

        double expected_energy_change = vec3_dot(v_initial, dv) * craft->mass + 0.5 * craft->mass * vec3_dot(dv, dv);

        double final_ke = 0.5 * craft->mass * vec3_dot(craft->local_velocity, craft->local_velocity);
        double final_pe = -G * craft->mass * earth->mass / vec3_magnitude(craft->local_position);
        double final_total_energy = final_ke + final_pe;

        double actual_energy_change = final_total_energy - initial_total_energy;

        INFO("Initial energy: " << initial_total_energy);
        INFO("Final energy: " << final_total_energy);
        INFO("Expected ΔE: " << expected_energy_change);
        INFO("Actual ΔE: " << actual_energy_change);

        REQUIRE(final_total_energy > initial_total_energy);

        double energy_error = fabs(actual_energy_change - expected_energy_change) / fabs(expected_energy_change);
        REQUIRE(energy_error < 1e-6);
    }

    SECTION("Retrograde burn decreases total energy") {
        double delta_v = 1000.0;
        Vec3 v_initial = craft->local_velocity;

        // Reset spacecraft for second test
        craft->local_position = initial_pos;
        craft->local_velocity = initial_vel;
        sim->time = 0.0;
        sim->maneuvers[find_maneuver_by_name(sim, "hohmann_burn_1")].executed = false;

        // Create a retrograde maneuver for this test
        Vec3 retrograde_dir = calculate_retrograde_dir(v_initial);
        Vec3 dv_vec = vec3_scale(retrograde_dir, delta_v);
        apply_custom_burn(craft, dv_vec);

        Vec3 v_final = craft->local_velocity;
        Vec3 dv = vec3_sub(v_final, v_initial);

        double expected_energy_change = vec3_dot(v_initial, dv) * craft->mass + 0.5 * craft->mass * vec3_dot(dv, dv);

        double final_ke = 0.5 * craft->mass * vec3_dot(craft->local_velocity, craft->local_velocity);
        double final_pe = -G * craft->mass * earth->mass / vec3_magnitude(craft->local_position);
        double final_total_energy = final_ke + final_pe;

        double actual_energy_change = final_total_energy - initial_total_energy;

        INFO("Initial energy: " << initial_total_energy);
        INFO("Final energy: " << final_total_energy);
        INFO("Expected ΔE: " << expected_energy_change);
        INFO("Actual ΔE: " << actual_energy_change);

        REQUIRE(final_total_energy < initial_total_energy);

        double energy_error = fabs(actual_energy_change - expected_energy_change) / fabs(expected_energy_change);
        REQUIRE(energy_error < 1e-6);
    }

    destroy_simulation(sim);
}

SCENARIO("Round-trip conversion with burns", "[hybrid][impulse][roundtrip]") {
    const double TIME_STEP = 60.0;

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

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

    Spacecraft* craft = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[1];

    SECTION("Orbital elements → Cartesian → burn → orbital elements") {
        OrbitalElements original_elements = craft->orbit;

        Vec3 position_from_elements;
        Vec3 velocity_from_elements;
        orbital_elements_to_cartesian(original_elements, earth->mass, &position_from_elements, &velocity_from_elements);
        craft->local_position = position_from_elements;
        craft->local_velocity = velocity_from_elements;

        INFO("Original semi_major_axis: " << original_elements.semi_major_axis);
        INFO("Original eccentricity: " << original_elements.eccentricity);

        OrbitalElements recovered_elements = cartesian_to_orbital_elements(position_from_elements, velocity_from_elements, earth->mass);

        INFO("Recovered semi_major_axis: " << recovered_elements.semi_major_axis);
        INFO("Recovered eccentricity: " << recovered_elements.eccentricity);

        REQUIRE_THAT(recovered_elements.semi_major_axis, Catch::Matchers::WithinAbs(original_elements.semi_major_axis, ELEMENT_TOLERANCE));
        REQUIRE_THAT(recovered_elements.eccentricity, Catch::Matchers::WithinAbs(original_elements.eccentricity, ELEMENT_TOLERANCE));

        // Execute maneuver via system
        execute_maneuver_by_name(sim, "hohmann_burn_1", craft);

        OrbitalElements post_burn_elements = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        INFO("Post-burn semi_major_axis: " << post_burn_elements.semi_major_axis);
        INFO("Post-burn eccentricity: " << post_burn_elements.eccentricity);

        REQUIRE(post_burn_elements.semi_major_axis != recovered_elements.semi_major_axis);
        REQUIRE(post_burn_elements.eccentricity != recovered_elements.eccentricity);
    }

    SECTION("Multiple round-trip conversions with burns") {
        OrbitalElements original_elements = craft->orbit;

        Vec3 position;
        Vec3 velocity;
        orbital_elements_to_cartesian(original_elements, earth->mass, &position, &velocity);
        craft->local_position = position;
        craft->local_velocity = velocity;

        for (int i = 0; i < 5; i++) {
            OrbitalElements elements = cartesian_to_orbital_elements(position, velocity, earth->mass);
            orbital_elements_to_cartesian(elements, earth->mass, &position, &velocity);

            INFO("Iteration " << i << " complete");
        }

        OrbitalElements final_elements = cartesian_to_orbital_elements(position, velocity, earth->mass);

        INFO("Original semi_major_axis: " << original_elements.semi_major_axis);
        INFO("Final semi_major_axis: " << final_elements.semi_major_axis);
        INFO("Original eccentricity: " << original_elements.eccentricity);
        INFO("Final eccentricity: " << final_elements.eccentricity);

        double a_error = fabs(final_elements.semi_major_axis - original_elements.semi_major_axis) / original_elements.semi_major_axis;
        double e_error = fabs(final_elements.eccentricity - original_elements.eccentricity);

        REQUIRE(a_error < 1e-9);
        REQUIRE(e_error < 1e-9);
    }

    destroy_simulation(sim);
}

SCENARIO("Multiple burn sequences", "[hybrid][impulse][sequence]") {
    const double TIME_STEP = 60.0;

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

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

    Spacecraft* craft = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[1];

    Vec3 initial_pos;
    Vec3 initial_vel;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &initial_pos, &initial_vel);
    craft->local_position = initial_pos;
    craft->local_velocity = initial_vel;

    SECTION("Two-burn sequence raises orbit") {
        OrbitalElements initial_elements = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        INFO("Initial a: " << initial_elements.semi_major_axis);
        INFO("Initial e: " << initial_elements.eccentricity);

        // Execute first burn via maneuver system
        execute_maneuver_by_name(sim, "hohmann_burn_1", craft);

        OrbitalElements after_first_burn = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        INFO("After first burn a: " << after_first_burn.semi_major_axis);
        INFO("After first burn e: " << after_first_burn.eccentricity);

        REQUIRE(after_first_burn.semi_major_axis > initial_elements.semi_major_axis);

        // Propagate to apogee for second burn
        OrbitalElements apogee_elements = after_first_burn;
        apogee_elements.true_anomaly = M_PI;

        Vec3 apogee_pos;
        Vec3 apogee_vel;
        orbital_elements_to_cartesian(apogee_elements, earth->mass, &apogee_pos, &apogee_vel);
        craft->local_position = apogee_pos;
        craft->local_velocity = apogee_vel;

        // Execute second burn via maneuver system
        execute_maneuver_by_name(sim, "hohmann_burn_2", craft);

        OrbitalElements after_second_burn = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        INFO("After second burn a: " << after_second_burn.semi_major_axis);
        INFO("After second burn e: " << after_second_burn.eccentricity);

        REQUIRE(after_second_burn.semi_major_axis > after_first_burn.semi_major_axis);
        REQUIRE(after_second_burn.eccentricity < after_first_burn.eccentricity);
    }

    SECTION("Three-burn sequence with plane change") {
        OrbitalElements initial_elements = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        // Reset spacecraft
        craft->local_position = initial_pos;
        craft->local_velocity = initial_vel;
        sim->time = 0.0;
        for (int i = 0; i < sim->maneuver_count; i++) {
            sim->maneuvers[i].executed = false;
        }

        // Execute prograde burn manually (no config maneuver for this sequence)
        Vec3 prograde_dir = calculate_prograde_dir(craft->local_velocity);
        Vec3 dv1 = vec3_scale(prograde_dir, 500.0);
        apply_custom_burn(craft, dv1);

        OrbitalElements after_burn1 = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        // Execute normal burn
        Vec3 normal_dir = calculate_normal_dir(craft->local_position, craft->local_velocity);
        Vec3 dv2 = vec3_scale(normal_dir, 300.0);
        apply_custom_burn(craft, dv2);

        OrbitalElements after_burn2 = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        // Execute second prograde burn
        prograde_dir = calculate_prograde_dir(craft->local_velocity);
        Vec3 dv3 = vec3_scale(prograde_dir, 200.0);
        apply_custom_burn(craft, dv3);

        OrbitalElements after_burn3 = cartesian_to_orbital_elements(craft->local_position, craft->local_velocity, earth->mass);

        INFO("Initial a: " << initial_elements.semi_major_axis);
        INFO("After burn 3 a: " << after_burn3.semi_major_axis);
        INFO("Initial inclination: " << initial_elements.inclination);
        INFO("After burn 3 inclination: " << after_burn3.inclination);

        REQUIRE(after_burn3.semi_major_axis > initial_elements.semi_major_axis);
        REQUIRE(after_burn3.inclination > initial_elements.inclination);
    }

    destroy_simulation(sim);
}

TEST_CASE("Burn direction vector calculation", "[hybrid][impulse][direction]") {
    const double TIME_STEP = 60.0;

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

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

    Spacecraft* craft = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[1];

    Vec3 position;
    Vec3 velocity;
    orbital_elements_to_cartesian(craft->orbit, earth->mass, &position, &velocity);

    SECTION("Prograde and retrograde are opposite") {
        Vec3 prograde = calculate_prograde_dir(velocity);
        Vec3 retrograde = calculate_retrograde_dir(velocity);

        double dot_product = vec3_dot(prograde, retrograde);
        INFO("Prograde · Retrograde: " << dot_product);

        REQUIRE_THAT(dot_product, Catch::Matchers::WithinAbs(-1.0, 1e-6));
    }

    SECTION("Normal and antinormal are opposite") {
        Vec3 normal = calculate_normal_dir(position, velocity);
        Vec3 antinormal = calculate_antinormal_dir(position, velocity);

        double dot_product = vec3_dot(normal, antinormal);
        INFO("Normal · Antinormal: " << dot_product);

        REQUIRE_THAT(dot_product, Catch::Matchers::WithinAbs(-1.0, 1e-6));
    }

    SECTION("Radial in and radial out are opposite") {
        Vec3 radial_in = calculate_radial_in_dir(position);
        Vec3 radial_out = calculate_radial_out_dir(position);

        double dot_product = vec3_dot(radial_in, radial_out);
        INFO("Radial_in · Radial_out: " << dot_product);

        REQUIRE_THAT(dot_product, Catch::Matchers::WithinAbs(-1.0, 1e-6));
    }

    destroy_simulation(sim);
}