#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/orbital_objects.h"
#include "../src/maneuver.h"
#include "../src/config_loader.h"
#include "../src/test_utilities.h"
#include <cmath>
#include <cstring>
#include <vector>

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

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

    if (maneuver->trigger_type == TRIGGER_TIME) {
        sim->time = maneuver->trigger_value;
    }

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

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

double calculate_spacecraft_kinetic_energy(Spacecraft* craft) {
    double v_squared = craft->local_velocity.x * craft->local_velocity.x +
                       craft->local_velocity.y * craft->local_velocity.y +
                       craft->local_velocity.z * craft->local_velocity.z;
    return 0.5 * craft->mass * v_squared;
}

double calculate_spacecraft_potential_energy(Spacecraft* craft, CelestialBody* parent) {
    double distance = vec3_magnitude(craft->local_position);
    if (distance < 1.0) distance = 1.0;
    return -G * craft->mass * parent->mass / distance;
}

double calculate_spacecraft_total_energy(Spacecraft* craft, CelestialBody* parent) {
    return calculate_spacecraft_kinetic_energy(craft) +
           calculate_spacecraft_potential_energy(craft, parent);
}

OrbitalElements simulate_continuous_burn(OrbitalElements initial_orbit, double parent_mass,
                                         double total_dv, double burn_duration,
                                         int num_steps, BurnDirection direction) {
    OrbitalElements current_orbit = initial_orbit;
    double dt_burn_step = burn_duration / num_steps;
    double dv_per_step = total_dv / num_steps;

    for (int i = 0; i < num_steps; i++) {
        Vec3 pos;
        Vec3 vel;
        orbital_elements_to_cartesian(current_orbit, parent_mass, &pos, &vel);

        Vec3 dir = get_burn_direction_vector(direction, pos, vel);
        Vec3 dv_vec = vec3_scale(dir, dv_per_step);
        vel = vec3_add(vel, dv_vec);

        current_orbit = cartesian_to_orbital_elements(pos, vel, parent_mass);
        current_orbit = propagate_orbital_elements(current_orbit, dt_burn_step, parent_mass);
    }

    return current_orbit;
}

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

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

    REQUIRE(load_system_config(sim, "tests/test_hybrid_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 == 10);

    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(std::string(sim->spacecraft[6].name) == "Low_Thrust_Ion");
    REQUIRE(sim->spacecraft[6].parent_index == 1);

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

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

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

    REQUIRE(sim->maneuver_count == 7);

    destroy_simulation(sim);
}

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

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

    REQUIRE(load_system_config(sim, "tests/test_hybrid_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_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_maneuver_by_name(sim, "hohmann_burn_1", craft);

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

        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_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("Impulse large burns (Δv > orbital velocity)", "[hybrid][burns][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/test_hybrid_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_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_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("Impulse energy conservation during burns", "[hybrid][burns][impulse][energy]") {
    const double TIME_STEP = 60.0;

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

    REQUIRE(load_system_config(sim, "tests/test_hybrid_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;

        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_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;

        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;

        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("Impulse round-trip conversion with burns", "[hybrid][burns][impulse][roundtrip]") {
    const double TIME_STEP = 60.0;

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

    REQUIRE(load_system_config(sim, "tests/test_hybrid_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_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("Impulse multiple burn sequences", "[hybrid][burns][impulse][sequence]") {
    const double TIME_STEP = 60.0;

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

    REQUIRE(load_system_config(sim, "tests/test_hybrid_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_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);

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

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

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

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

        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("Impulse burn direction vector calculation", "[hybrid][burns][impulse][direction]") {
    const double TIME_STEP = 60.0;

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

    REQUIRE(load_system_config(sim, "tests/test_hybrid_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);
}

TEST_CASE("Continuous low-thrust burns (ion engines)", "[hybrid][burns][continuous][low_thrust]") {
    const double TIME_STEP = 60.0;

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

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

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

    double initial_semi_major = craft->orbit.semi_major_axis;
    double initial_eccentricity = craft->orbit.eccentricity;

    INFO("Initial semi-major axis: " << initial_semi_major << " m");
    INFO("Initial eccentricity: " << initial_eccentricity);

    double burn_duration = 5000.0;
    double total_dv = 100.0;
    int num_steps = 100;

    OrbitalElements final_orbit = simulate_continuous_burn(craft->orbit, earth->mass,
                                                           total_dv, burn_duration,
                                                           num_steps, BURN_PROGRADE);

    INFO("Final semi-major axis: " << final_orbit.semi_major_axis << " m");
    INFO("Final eccentricity: " << final_orbit.eccentricity);

    REQUIRE(final_orbit.semi_major_axis > initial_semi_major);

    double a_before = initial_semi_major;
    double a_after = final_orbit.semi_major_axis;
    double mu = G * earth->mass;
    double v_circular_initial = sqrt(mu / a_before);
    double v_circular_final = sqrt(mu / a_after);

    double epsilon_initial = -mu / (2.0 * a_before);
    double epsilon_final = -mu / (2.0 * a_after);
    double delta_epsilon = epsilon_final - epsilon_initial;

    INFO("Initial circular velocity: " << v_circular_initial << " m/s");
    INFO("Final circular velocity: " << v_circular_final << " m/s");
    INFO("Initial specific energy: " << epsilon_initial << " J/kg");
    INFO("Final specific energy: " << epsilon_final << " J/kg");
    INFO("Energy change: " << delta_epsilon << " J/kg");
    INFO("Applied delta-v: " << total_dv << " m/s");

    double expected_dv_from_energy = delta_epsilon / v_circular_initial;

    INFO("Expected delta-v from energy: " << expected_dv_from_energy << " m/s");

    double relative_error = fabs(expected_dv_from_energy - total_dv) / total_dv;
    INFO("Relative error: " << relative_error * 100 << "%");

    REQUIRE(relative_error < 0.01);

    REQUIRE(final_orbit.eccentricity < 0.01);

    destroy_simulation(sim);
}

TEST_CASE("Continuous multi-burn sequences", "[hybrid][burns][continuous][multi_burn]") {
    const double TIME_STEP = 60.0;

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

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

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

    double initial_semi_major = craft->orbit.semi_major_axis;

    INFO("Initial semi-major axis: " << initial_semi_major << " m");

    double burn_duration_1 = 2000.0;
    double total_dv_1 = 50.0;
    int num_steps_1 = 20;

    OrbitalElements orbit_after_burn1 = simulate_continuous_burn(craft->orbit, earth->mass,
                                                                  total_dv_1, burn_duration_1,
                                                                  num_steps_1, BURN_PROGRADE);

    INFO("Semi-major axis after burn 1: " << orbit_after_burn1.semi_major_axis << " m");

    REQUIRE(orbit_after_burn1.semi_major_axis > initial_semi_major);

    double burn_duration_2 = 3000.0;
    double total_dv_2 = 75.0;
    int num_steps_2 = 30;

    OrbitalElements final_orbit = simulate_continuous_burn(orbit_after_burn1, earth->mass,
                                                            total_dv_2, burn_duration_2,
                                                            num_steps_2, BURN_PROGRADE);

    INFO("Final semi-major axis: " << final_orbit.semi_major_axis << " m");

    REQUIRE(final_orbit.semi_major_axis > orbit_after_burn1.semi_major_axis);

    double a_before = initial_semi_major;
    double a_after = final_orbit.semi_major_axis;
    double mu = G * earth->mass;
    double v_circular_initial = sqrt(mu / a_before);
    double v_circular_final = sqrt(mu / a_after);

    double epsilon_initial = -mu / (2.0 * a_before);
    double epsilon_final = -mu / (2.0 * a_after);
    double delta_epsilon = epsilon_final - epsilon_initial;
    double total_dv_applied = total_dv_1 + total_dv_2;

    INFO("Total applied delta-v: " << total_dv_applied << " m/s");
    INFO("Initial specific energy: " << epsilon_initial << " J/kg");
    INFO("Final specific energy: " << epsilon_final << " J/kg");
    INFO("Energy change: " << delta_epsilon << " J/kg");

    double expected_dv_from_energy = delta_epsilon / v_circular_initial;

    INFO("Expected delta-v from energy: " << expected_dv_from_energy << " m/s");

    double relative_error = fabs(expected_dv_from_energy - total_dv_applied) / total_dv_applied;
    INFO("Relative error: " << relative_error * 100 << "%");

    REQUIRE(relative_error < 0.01);

    destroy_simulation(sim);
}

TEST_CASE("Continuous mode transitions during burns", "[hybrid][burns][continuous][mode_transition]") {
    const double TIME_STEP = 60.0;

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

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

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

    double initial_semi_major = craft->orbit.semi_major_axis;
    double initial_eccentricity = craft->orbit.eccentricity;

    INFO("Initial semi-major axis: " << initial_semi_major << " m");
    INFO("Initial eccentricity: " << initial_eccentricity);

    double burn_duration = 4000.0;
    double total_dv = 200.0;
    int num_steps = 80;

    OrbitalElements current_orbit = craft->orbit;
    double dt_burn_step = burn_duration / num_steps;
    double dv_per_step = total_dv / num_steps;

    for (int i = 0; i < num_steps; i++) {
        Vec3 pos;
        Vec3 vel;
        orbital_elements_to_cartesian(current_orbit, earth->mass, &pos, &vel);

        Vec3 dir = get_burn_direction_vector(BURN_PROGRADE, pos, vel);
        Vec3 dv_vec = vec3_scale(dir, dv_per_step);
        vel = vec3_add(vel, dv_vec);

        OrbitalElements orbit_from_cart = cartesian_to_orbital_elements(pos, vel, earth->mass);

        current_orbit = propagate_orbital_elements(orbit_from_cart, dt_burn_step, earth->mass);
    }

    INFO("Final semi-major axis: " << current_orbit.semi_major_axis << " m");
    INFO("Final eccentricity: " << current_orbit.eccentricity);

    REQUIRE(current_orbit.semi_major_axis > initial_semi_major);

    double mu = G * earth->mass;
    double energy_before = -mu / (2.0 * initial_semi_major);
    double energy_after = -mu / (2.0 * current_orbit.semi_major_axis);
    double energy_change = energy_after - energy_before;

    double expected_energy_change = 0.5 * (sqrt(mu / initial_semi_major) + sqrt(mu / current_orbit.semi_major_axis)) * total_dv;

    INFO("Energy change: " << energy_change << " J/kg");
    INFO("Expected energy change: " << expected_energy_change << " J/kg");

    REQUIRE(fabs(energy_change) > 0);
    // FIXME: not comparing expected_energy_change

    destroy_simulation(sim);
}

TEST_CASE("Continuous energy conservation during burns", "[hybrid][burns][continuous][energy]") {
    const double TIME_STEP = 60.0;

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

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

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

    double initial_energy = calculate_spacecraft_total_energy(craft, earth);

    INFO("Initial total energy: " << initial_energy << " J");

    double burn_duration = 6000.0;
    double total_dv = 150.0;
    int num_steps = 120;

    OrbitalElements current_orbit = craft->orbit;
    double dt_burn_step = burn_duration / num_steps;
    double dv_per_step = total_dv / num_steps;

    std::vector<double> energy_history;
    double max_energy_jump = 0.0;

    for (int i = 0; i < num_steps; i++) {
        Vec3 pos;
        Vec3 vel;
        orbital_elements_to_cartesian(current_orbit, earth->mass, &pos, &vel);

        Vec3 dir = get_burn_direction_vector(BURN_PROGRADE, pos, vel);
        Vec3 dv_vec = vec3_scale(dir, dv_per_step);
        vel = vec3_add(vel, dv_vec);

        current_orbit = cartesian_to_orbital_elements(pos, vel, earth->mass);
        current_orbit = propagate_orbital_elements(current_orbit, dt_burn_step, earth->mass);

        orbital_elements_to_cartesian(current_orbit, earth->mass, &pos, &vel);
        Spacecraft temp_craft = *craft;
        temp_craft.local_position = pos;
        temp_craft.local_velocity = vel;

        double current_energy = calculate_spacecraft_total_energy(&temp_craft, earth);
        energy_history.push_back(current_energy);

        if (i > 0) {
            double energy_jump = fabs(current_energy - energy_history[i - 1]);
            max_energy_jump = fmax(max_energy_jump, energy_jump);
        }
    }

    double final_energy = energy_history[num_steps - 1];
    double total_energy_change = final_energy - initial_energy;

    INFO("Final total energy: " << final_energy << " J");
    INFO("Total energy change: " << total_energy_change << " J");
    INFO("Max energy jump between steps: " << max_energy_jump << " J");

    REQUIRE(total_energy_change > 0);

    double expected_energy_change_approx = craft->mass * sqrt(G * earth->mass / craft->orbit.semi_major_axis) * total_dv;
    double relative_error = fabs(total_energy_change - expected_energy_change_approx) / expected_energy_change_approx;

    INFO("Expected approximate energy change: " << expected_energy_change_approx << " J");
    INFO("Relative error: " << relative_error * 100 << "%");

    REQUIRE(relative_error < 0.1);

    double average_step_energy_change = fabs(total_energy_change) / num_steps;
    double max_jump_ratio = max_energy_jump / average_step_energy_change;

    INFO("Average energy change per step: " << average_step_energy_change << " J");
    INFO("Max jump / average: " << max_jump_ratio);

    REQUIRE(max_jump_ratio < 10.0);

    destroy_simulation(sim);
}

TEST_CASE("Continuous accuracy of continuous vs. impulsive burns", "[hybrid][burns][continuous][accuracy]") {
    const double TIME_STEP = 60.0;

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

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

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

    double initial_semi_major = craft->orbit.semi_major_axis;

    double burn_duration = 5000.0;
    double total_dv = 100.0;
    int num_steps_continuous = 100;

    OrbitalElements orbit_continuous = simulate_continuous_burn(craft->orbit, earth->mass,
                                                                total_dv, burn_duration,
                                                                num_steps_continuous, BURN_PROGRADE);

    OrbitalElements orbit_impulsive = simulate_continuous_burn(craft->orbit, earth->mass,
                                                              total_dv, burn_duration,
                                                              1, BURN_PROGRADE);

    INFO("Initial semi-major axis: " << initial_semi_major << " m");
    INFO("Continuous burn semi-major axis: " << orbit_continuous.semi_major_axis << " m");
    INFO("Impulsive burn semi-major axis: " << orbit_impulsive.semi_major_axis << " m");

    double difference_semi_major = fabs(orbit_continuous.semi_major_axis - orbit_impulsive.semi_major_axis);
    double relative_difference = difference_semi_major / orbit_continuous.semi_major_axis * 100.0;

    INFO("Semi-major axis difference: " << difference_semi_major << " m");
    INFO("Relative difference: " << relative_difference << "%");

    REQUIRE(relative_difference < 1.0);

    double mu = G * earth->mass;
    double v_continuous = sqrt(mu / orbit_continuous.semi_major_axis);
    double v_impulsive = sqrt(mu / orbit_impulsive.semi_major_axis);
    double v_difference = fabs(v_continuous - v_impulsive);

    INFO("Continuous burn velocity: " << v_continuous << " m/s");
    INFO("Impulsive burn velocity: " << v_impulsive << " m/s");
    INFO("Velocity difference: " << v_difference << " m/s");

    REQUIRE(v_difference < 2.0);

    destroy_simulation(sim);
}

TEST_CASE("Continuous propagation during burn", "[hybrid][burns][continuous][propagation]") {
    const double TIME_STEP = 60.0;

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

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

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

    double burn_duration = 5000.0;
    double total_dv = 100.0;
    int num_steps = 100;

    OrbitalElements current_orbit = craft->orbit;
    double dt_burn_step = burn_duration / num_steps;
    double dv_per_step = total_dv / num_steps;

    std::vector<Vec3> positions;
    std::vector<double> times;

    for (int i = 0; i <= num_steps; i++) {
        Vec3 pos;
        Vec3 vel;
        orbital_elements_to_cartesian(current_orbit, earth->mass, &pos, &vel);

        positions.push_back(pos);
        times.push_back(i * dt_burn_step);

        if (i < num_steps) {
            Vec3 dir = get_burn_direction_vector(BURN_PROGRADE, pos, vel);
            Vec3 dv_vec = vec3_scale(dir, dv_per_step);
            vel = vec3_add(vel, dv_vec);

            current_orbit = cartesian_to_orbital_elements(pos, vel, earth->mass);
            current_orbit = propagate_orbital_elements(current_orbit, dt_burn_step, earth->mass);
        }
    }

    double total_path_length = 0.0;
    for (size_t i = 1; i < positions.size(); i++) {
        total_path_length += vec3_distance(positions[i - 1], positions[i]);
    }

    INFO("Total path length during burn: " << total_path_length << " m");

    Vec3 pos_start = positions[0];
    Vec3 pos_end = positions[num_steps];
    double straight_line_distance = vec3_distance(pos_start, pos_end);

    INFO("Straight-line distance: " << straight_line_distance << " m");

    REQUIRE(total_path_length > straight_line_distance);

    double initial_radius = vec3_magnitude(pos_start);
    double final_radius = vec3_magnitude(pos_end);

    INFO("Initial radius: " << initial_radius << " m");
    INFO("Final radius: " << final_radius << " m");

    REQUIRE(final_radius > initial_radius);

    double mu = G * earth->mass;
    double v_initial = sqrt(mu / craft->orbit.semi_major_axis);
    double epsilon_initial = -mu / (2.0 * craft->orbit.semi_major_axis);
    double epsilon_final = epsilon_initial + v_initial * total_dv;
    double a_expected = -mu / (2.0 * epsilon_final);

    INFO("Expected final semi-major axis: " << a_expected << " m");

    double r_at_periapsis = a_expected * (1.0 - craft->orbit.eccentricity);
    double r_at_apoapsis = a_expected * (1.0 + craft->orbit.eccentricity);

    INFO("Expected radius at periapsis: " << r_at_periapsis << " m");
    INFO("Expected radius at apoapsis: " << r_at_apoapsis << " m");

    REQUIRE(final_radius >= r_at_periapsis - 1.0e5);
    REQUIRE(final_radius <= r_at_apoapsis + 1.0e5);

    destroy_simulation(sim);
}

TEST_CASE("Continuous numerical stability during many burn/conversion cycles", "[hybrid][burns][continuous][stability]") {
    const double TIME_STEP = 60.0;

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

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

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

    OrbitalElements initial_orbit = craft->orbit;
    double burn_duration = 5000.0;
    double total_dv = 100.0;
    int num_steps = 100;
    double dt_burn_step = burn_duration / num_steps;
    double dv_per_step = total_dv / num_steps;

    std::vector<double> semi_major_history;
    std::vector<double> eccentricity_history;

    OrbitalElements current_orbit = craft->orbit;

    for (int i = 0; i < num_steps; i++) {
        Vec3 pos;
        Vec3 vel;
        orbital_elements_to_cartesian(current_orbit, earth->mass, &pos, &vel);

        Vec3 dir = get_burn_direction_vector(BURN_PROGRADE, pos, vel);
        Vec3 dv_vec = vec3_scale(dir, dv_per_step);
        vel = vec3_add(vel, dv_vec);

        current_orbit = cartesian_to_orbital_elements(pos, vel, earth->mass);
        current_orbit = propagate_orbital_elements(current_orbit, dt_burn_step, earth->mass);

        semi_major_history.push_back(current_orbit.semi_major_axis);
        eccentricity_history.push_back(current_orbit.eccentricity);
    }

    bool monotonic_increase = true;
    for (size_t i = 1; i < semi_major_history.size(); i++) {
        if (semi_major_history[i] < semi_major_history[i - 1]) {
            monotonic_increase = false;
            break;
        }
    }

    INFO("Monotonic semi-major axis increase: " << (monotonic_increase ? "yes" : "no"));

    REQUIRE(monotonic_increase);

    double max_eccentricity = 0.0;
    double min_eccentricity = 1.0;

    for (size_t i = 0; i < eccentricity_history.size(); i++) {
        max_eccentricity = fmax(max_eccentricity, eccentricity_history[i]);
        min_eccentricity = fmin(min_eccentricity, eccentricity_history[i]);
    }

    INFO("Max eccentricity during burn: " << max_eccentricity);
    INFO("Min eccentricity during burn: " << min_eccentricity);

    REQUIRE(max_eccentricity < 0.1);

    double initial_semi_major = initial_orbit.semi_major_axis;
    double final_semi_major = semi_major_history[num_steps - 1];
    double total_change = final_semi_major - initial_semi_major;
    double average_change_per_step = total_change / num_steps;

    INFO("Total semi-major axis change: " << total_change << " m");
    INFO("Average change per step: " << average_change_per_step << " m");

    double max_deviation = 0.0;
    for (size_t i = 0; i < semi_major_history.size(); i++) {
        double expected = initial_semi_major + (i + 1) * average_change_per_step;
        double deviation = fabs(semi_major_history[i] - expected);
        max_deviation = fmax(max_deviation, deviation);
    }

    INFO("Max deviation from linear trend: " << max_deviation << " m");
    INFO("Relative deviation: " << (max_deviation / total_change * 100) << "%");

    REQUIRE(max_deviation < total_change * 0.5);

    destroy_simulation(sim);
}