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

using Catch::Matchers::WithinAbs;

// Simulate continuous/low-thrust burn with sub-steps (replaces numerical integrator)
static 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 = burn_duration / num_steps;
    double dv_per = total_dv / num_steps;

    for (int i = 0; i < num_steps; i++) {
        Vec3 pos, 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);
        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, parent_mass);
    }
    return current_orbit;
}

SCENARIO("Hybrid burns: impulse + continuous burn behavior", "[hybrid][burns]") {
    const double TIME_STEP = 60.0;
    const double MU_EARTH = G * 5.972e24;

    SimulationState* sim = create_simulation(10, 10, 100, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_hybrid_burns.toml"));

    // Helper: initialize a spacecraft from its orbital elements
    auto init_craft = [&](Spacecraft* craft, CelestialBody* parent) {
        Vec3 pos, vel;
        orbital_elements_to_cartesian(craft->orbit, parent->mass, &pos, &vel);
        craft->local_position = pos;
        craft->local_velocity = vel;
    };

    // Helper: find maneuver by name
    auto find_maneuver = [&](const char* name) -> int {
        for (int i = 0; i < sim->maneuver_count; i++) {
            if (strcmp(sim->maneuvers[i].name, name) == 0) return i;
        }
        return -1;
    };

    // Helper: execute maneuver by name (sets time, calls execute_maneuver)
    auto exec_by_name = [&](const char* name, Spacecraft* craft) {
        int idx = find_maneuver(name);
        REQUIRE(idx >= 0);
        Maneuver* m = &sim->maneuvers[idx];
        REQUIRE(!m->executed);
        if (m->trigger_type == TRIGGER_TIME) {
            sim->time = m->trigger_value;
        }
        execute_maneuver(m, craft, sim, sim->time);
        REQUIRE(m->executed);
        REQUIRE_THAT(m->executed_time, WithinAbs(sim->time, M_TOL));
    };

    // Shared fixtures
    CelestialBody* sun = &sim->bodies[0];
    CelestialBody* earth = &sim->bodies[1];
    Spacecraft* hohmann = &sim->spacecraft[0];
    Spacecraft* large_dv = &sim->spacecraft[5];
    Spacecraft* low_thrust = &sim->spacecraft[6];
    Spacecraft* multi_burn = &sim->spacecraft[7];
    Spacecraft* mode_trans = &sim->spacecraft[8];
    Spacecraft* energy_cons = &sim->spacecraft[9];

    SECTION("config loads correctly: 2 bodies, 10 spacecraft, 7 maneuvers") {
        REQUIRE(sim->body_count == 2);
        REQUIRE(std::string(sun->name) == "Sun");
        REQUIRE(std::string(earth->name) == "Earth");
        REQUIRE(sim->craft_count == 10);
        REQUIRE(sim->maneuver_count == 7);

        REQUIRE(std::string(hohmann->name) == "Hohmann_Transfer");
        REQUIRE(hohmann->parent_index == 1);
        REQUIRE(std::string(large_dv->name) == "Large_Delta_v");
    }

    SECTION("first burn at perigee raises apogee") {
        init_craft(hohmann, earth);

        const double v_before = vec3_magnitude(hohmann->local_velocity);
        exec_by_name("hohmann_burn_1", hohmann);
        const double v_after = vec3_magnitude(hohmann->local_velocity);

        REQUIRE_THAT(v_after, WithinAbs(10112.490413, V_TOL));

        const auto post_els = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        INFO("v_before: " << v_before << " m/s");
        INFO("v_after: " << v_after << " m/s");
        INFO("a_after: " << post_els.semi_major_axis << " m");
        INFO("e_after: " << post_els.eccentricity);

        REQUIRE_THAT(post_els.semi_major_axis, WithinAbs(25762376.160113, A_TOL));
        REQUIRE_THAT(post_els.eccentricity, WithinAbs(0.737174864697325, E_TOL));
    }

    SECTION("second burn at apogee circularizes orbit") {
        init_craft(hohmann, earth);

        exec_by_name("hohmann_burn_1", hohmann);
        const auto after_1 = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        // Propagate to apogee
        auto apogee_els = after_1;
        apogee_els.true_anomaly = M_PI;
        Vec3 apogee_pos, apogee_vel;
        orbital_elements_to_cartesian(apogee_els, earth->mass, &apogee_pos, &apogee_vel);
        hohmann->local_position = apogee_pos;
        hohmann->local_velocity = apogee_vel;

        exec_by_name("hohmann_burn_2", hohmann);
        const auto final_els = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        INFO("a_after_first: " << after_1.semi_major_axis);
        INFO("a_final: " << final_els.semi_major_axis);
        INFO("e_final: " << final_els.eccentricity);

        REQUIRE_THAT(final_els.semi_major_axis, WithinAbs(46176507.362571, A_TOL));
        REQUIRE_THAT(final_els.eccentricity, WithinAbs(0.030811231156453, E_TOL));
    }

    SECTION("large prograde burn produces hyperbolic orbit") {
        init_craft(large_dv, earth);

        const double v_before = vec3_magnitude(large_dv->local_velocity);
        const double r = vec3_magnitude(large_dv->local_position);
        const double v_escape = sqrt(2.0 * G * earth->mass / r);

        exec_by_name("large_burn", large_dv);
        const double v_after = vec3_magnitude(large_dv->local_velocity);

        INFO("v_before: " << v_before << " m/s");
        INFO("v_escape: " << v_escape << " m/s");
        INFO("v_after: " << v_after << " m/s");

        REQUIRE_THAT(v_after, WithinAbs(19545.946840, V_TOL));

        const auto hyper_els = cartesian_to_orbital_elements(
            large_dv->local_position, large_dv->local_velocity, earth->mass);

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

        REQUIRE_THAT(hyper_els.eccentricity, WithinAbs(5.709434906871548, E_TOL));
        REQUIRE_THAT(hyper_els.semi_major_axis, WithinAbs(-1486377.906994, A_TOL));
    }

    SECTION("large burn satisfies vis-viva equation") {
        init_craft(large_dv, earth);

        exec_by_name("large_burn", large_dv);
        const auto hyper_els = cartesian_to_orbital_elements(
            large_dv->local_position, large_dv->local_velocity, earth->mass);

        const double v_sq = vec3_magnitude(large_dv->local_velocity)
                          * vec3_magnitude(large_dv->local_velocity);
        const double r = vec3_magnitude(large_dv->local_position);
        const double vis_viva_calc = G * earth->mass * (2.0 / r - 1.0 / hyper_els.semi_major_axis);

        INFO("vis_viva_expected: " << v_sq);
        INFO("vis_viva_calculated: " << vis_viva_calc);

        const double err = fabs(v_sq - vis_viva_calc) / v_sq;
        REQUIRE_THAT(err, WithinAbs(0.0, D_TOL));
    }

    SECTION("prograde burn increases total energy") {
        init_craft(hohmann, earth);

        const double m = hohmann->mass;
        const Vec3 v_init = hohmann->local_velocity;
        const double ke_init = 0.5 * m * vec3_dot(v_init, v_init);
        const double r_init = vec3_magnitude(hohmann->local_position);
        const double pe_init = -G * m * earth->mass / r_init;
        const double E_init = ke_init + pe_init;

        exec_by_name("hohmann_burn_1", hohmann);

        const Vec3 v_final = hohmann->local_velocity;
        const Vec3 dv = vec3_sub(v_final, v_init);
        const double ke_final = 0.5 * m * vec3_dot(v_final, v_final);
        const double pe_final = -G * m * earth->mass / vec3_magnitude(hohmann->local_position);
        const double E_final = ke_final + pe_final;

        const double dE_actual = E_final - E_init;
        const double dE_expected = vec3_dot(v_init, dv) * m + 0.5 * m * vec3_dot(dv, dv);

        INFO("E_init: " << E_init);
        INFO("E_final: " << E_final);
        INFO("dE_actual: " << dE_actual);
        INFO("dE_expected: " << dE_expected);

        REQUIRE_THAT(E_final, WithinAbs(-7735877962.552383, A_TOL));

        const double dE_err = fabs(dE_actual - dE_expected) / fabs(dE_expected);
        REQUIRE_THAT(dE_err, WithinAbs(0.0, D_TOL));
    }

    SECTION("retrograde burn decreases total energy") {
        init_craft(hohmann, earth);

        const double m = hohmann->mass;
        const Vec3 v_init = hohmann->local_velocity;
        const double ke_init = 0.5 * m * vec3_dot(v_init, v_init);
        const double pe_init = -G * m * earth->mass / vec3_magnitude(hohmann->local_position);
        const double E_init = ke_init + pe_init;

        const Vec3 retro_dir = calculate_retrograde_dir(v_init);
        apply_custom_burn(hohmann, vec3_scale(retro_dir, 1000.0));

        const Vec3 v_final = hohmann->local_velocity;
        const Vec3 dv = vec3_sub(v_final, v_init);
        const double ke_final = 0.5 * m * vec3_dot(v_final, v_final);
        const double pe_final = -G * m * earth->mass / vec3_magnitude(hohmann->local_position);
        const double E_final = ke_final + pe_final;

        const double dE_actual = E_final - E_init;
        const double dE_expected = vec3_dot(v_init, dv) * m + 0.5 * m * vec3_dot(dv, dv);

        INFO("E_init: " << E_init);
        INFO("E_final: " << E_final);
        INFO("dE_actual: " << dE_actual);
        INFO("dE_expected: " << dE_expected);

        REQUIRE_THAT(E_final, WithinAbs(-36606044984.248001, A_TOL));

        const double dE_err = fabs(dE_actual - dE_expected) / fabs(dE_expected);
        REQUIRE_THAT(dE_err, WithinAbs(0.0, D_TOL));
    }

    SECTION("orbital elements -> Cartesian -> burn -> orbital elements") {
        init_craft(hohmann, earth);

        const auto orig_els = hohmann->orbit;
        const auto recovered = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        INFO("orig_a: " << orig_els.semi_major_axis);
        INFO("recovered_a: " << recovered.semi_major_axis);
        INFO("orig_e: " << orig_els.eccentricity);
        INFO("recovered_e: " << recovered.eccentricity);

        REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(orig_els.semi_major_axis, A_TOL));
        REQUIRE_THAT(recovered.eccentricity, WithinAbs(orig_els.eccentricity, E_TOL));

        exec_by_name("hohmann_burn_1", hohmann);
        const auto post_burn = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        INFO("post_burn_a: " << post_burn.semi_major_axis);
        INFO("post_burn_e: " << post_burn.eccentricity);

        REQUIRE(post_burn.semi_major_axis != recovered.semi_major_axis);
        REQUIRE(post_burn.eccentricity != recovered.eccentricity);
    }

    SECTION("multiple round-trip conversions maintain stability") {
        init_craft(hohmann, earth);

        const auto orig_els = hohmann->orbit;
        Vec3 pos = hohmann->local_position;
        Vec3 vel = hohmann->local_velocity;

        for (int i = 0; i < 5; i++) {
            const auto els = cartesian_to_orbital_elements(pos, vel, earth->mass);
            orbital_elements_to_cartesian(els, earth->mass, &pos, &vel);
        }

        const auto final_els = cartesian_to_orbital_elements(pos, vel, earth->mass);
        const double a_err = fabs(final_els.semi_major_axis - orig_els.semi_major_axis)
                           / orig_els.semi_major_axis;
        const double e_err = fabs(final_els.eccentricity - orig_els.eccentricity);

        INFO("orig_a: " << orig_els.semi_major_axis);
        INFO("final_a: " << final_els.semi_major_axis);
        INFO("orig_e: " << orig_els.eccentricity);
        INFO("final_e: " << final_els.eccentricity);

        REQUIRE_THAT(a_err, WithinAbs(0.0, REL_TOL));
        REQUIRE_THAT(e_err, WithinAbs(0.0, E_TOL));
    }

    SECTION("two-burn sequence raises orbit") {
        init_craft(hohmann, earth);

        exec_by_name("hohmann_burn_1", hohmann);
        const auto after_1 = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        REQUIRE_THAT(after_1.semi_major_axis, WithinAbs(25762376.160113, A_TOL));

        // Propagate to apogee
        auto apogee_els = after_1;
        apogee_els.true_anomaly = M_PI;
        Vec3 apogee_pos, apogee_vel;
        orbital_elements_to_cartesian(apogee_els, earth->mass, &apogee_pos, &apogee_vel);
        hohmann->local_position = apogee_pos;
        hohmann->local_velocity = apogee_vel;

        exec_by_name("hohmann_burn_2", hohmann);
        const auto after_2 = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        INFO("a_after_2: " << after_2.semi_major_axis);
        INFO("e_after_2: " << after_2.eccentricity);

        REQUIRE_THAT(after_2.semi_major_axis, WithinAbs(46176507.362571, A_TOL));
        REQUIRE_THAT(after_2.eccentricity, WithinAbs(0.030811231156453, E_TOL));
    }

    SECTION("three-burn sequence with plane change") {
        init_craft(hohmann, earth);

        const auto init_els = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        // Burn 1: prograde 500 m/s
        apply_custom_burn(hohmann, vec3_scale(calculate_prograde_dir(hohmann->local_velocity), 500.0));

        // Burn 2: normal 300 m/s
        apply_custom_burn(hohmann, vec3_scale(calculate_normal_dir(hohmann->local_position,
                                                                    hohmann->local_velocity), 300.0));

        // Burn 3: prograde 200 m/s
        apply_custom_burn(hohmann, vec3_scale(calculate_prograde_dir(hohmann->local_velocity), 200.0));
        const auto after_3 = cartesian_to_orbital_elements(
            hohmann->local_position, hohmann->local_velocity, earth->mass);

        INFO("init_a: " << init_els.semi_major_axis);
        INFO("final_a: " << after_3.semi_major_axis);
        INFO("init_inc: " << init_els.inclination);
        INFO("final_inc: " << after_3.inclination);

        REQUIRE_THAT(after_3.semi_major_axis, WithinAbs(8383687.781504, A_TOL));
        REQUIRE_THAT(after_3.inclination, WithinAbs(0.036692041490386, ANG_TOL));
    }

    SECTION("prograde and retrograde are opposite") {
        init_craft(hohmann, earth);

        const Vec3 pro = calculate_prograde_dir(hohmann->local_velocity);
        const Vec3 retro = calculate_retrograde_dir(hohmann->local_velocity);
        const double dot = vec3_dot(pro, retro);

        INFO("prograde . retrograde: " << dot);

        REQUIRE_THAT(dot, WithinAbs(-1.0, V_TOL));
    }

    SECTION("normal and antinormal are opposite") {
        init_craft(hohmann, earth);

        const Vec3 norm = calculate_normal_dir(hohmann->local_position, hohmann->local_velocity);
        const Vec3 anti = calculate_antinormal_dir(hohmann->local_position, hohmann->local_velocity);
        const double dot = vec3_dot(norm, anti);

        INFO("normal . antinormal: " << dot);

        REQUIRE_THAT(dot, WithinAbs(-1.0, V_TOL));
    }

    SECTION("radial in and radial out are opposite") {
        init_craft(hohmann, earth);

        const Vec3 rad_in = calculate_radial_in_dir(hohmann->local_position);
        const Vec3 rad_out = calculate_radial_out_dir(hohmann->local_position);
        const double dot = vec3_dot(rad_in, rad_out);

        INFO("radial_in . radial_out: " << dot);

        REQUIRE_THAT(dot, WithinAbs(-1.0, V_TOL));
    }

    SECTION("continuous prograde burn raises semi-major axis") {
        const auto final_els = simulate_continuous_burn(
            low_thrust->orbit, earth->mass, 100.0, 5000.0, 100, BURN_PROGRADE);

        INFO("initial_a: " << low_thrust->orbit.semi_major_axis);
        INFO("final_a: " << final_els.semi_major_axis);
        INFO("final_e: " << final_els.eccentricity);

        REQUIRE_THAT(final_els.semi_major_axis, WithinAbs(6951054.544051, A_TOL));

        const double v_circ_init = sqrt(MU_EARTH / low_thrust->orbit.semi_major_axis);
        const double eps_init = -MU_EARTH / (2.0 * low_thrust->orbit.semi_major_axis);
        const double eps_final = -MU_EARTH / (2.0 * final_els.semi_major_axis);
        const double expected_dv = (eps_final - eps_init) / v_circ_init;
        const double rel_err = fabs(expected_dv - 100.0) / 100.0;

        INFO("v_circ_init: " << v_circ_init);
        INFO("expected_dv: " << expected_dv);
        INFO("relative_error: " << rel_err);

        REQUIRE_THAT(rel_err, WithinAbs(0.0, 0.01));
        REQUIRE_THAT(final_els.eccentricity, WithinAbs(0.003347573985440, E_TOL));
    }

    SECTION("continuous multi-burn sequence raises orbit") {
        const auto after_1 = simulate_continuous_burn(
            multi_burn->orbit, earth->mass, 50.0, 2000.0, 20, BURN_PROGRADE);

        REQUIRE_THAT(after_1.semi_major_axis, WithinAbs(7094118.510013, A_TOL));

        const auto final_els = simulate_continuous_burn(
            after_1, earth->mass, 75.0, 3000.0, 30, BURN_PROGRADE);

        INFO("final_a: " << final_els.semi_major_axis);

        REQUIRE_THAT(final_els.semi_major_axis, WithinAbs(7237952.003198, A_TOL));

        const double v_circ_init = sqrt(MU_EARTH / multi_burn->orbit.semi_major_axis);
        const double eps_init = -MU_EARTH / (2.0 * multi_burn->orbit.semi_major_axis);
        const double eps_final = -MU_EARTH / (2.0 * final_els.semi_major_axis);
        const double expected_dv = (eps_final - eps_init) / v_circ_init;
        const double rel_err = fabs(expected_dv - 125.0) / 125.0;

        INFO("expected_dv: " << expected_dv);
        INFO("relative_error: " << rel_err);

        REQUIRE_THAT(rel_err, WithinAbs(0.0, 0.01));
    }

    SECTION("continuous burn on elliptical orbit raises semi-major axis") {
        const auto final_els = simulate_continuous_burn(
            mode_trans->orbit, earth->mass, 200.0, 4000.0, 80, BURN_PROGRADE);

        INFO("initial_a: " << mode_trans->orbit.semi_major_axis);
        INFO("final_a: " << final_els.semi_major_axis);
        INFO("initial_e: " << mode_trans->orbit.eccentricity);
        INFO("final_e: " << final_els.eccentricity);

        REQUIRE_THAT(final_els.semi_major_axis, WithinAbs(13012778.714495, A_TOL));

        const double energy_before = -MU_EARTH / (2.0 * mode_trans->orbit.semi_major_axis);
        const double energy_after = -MU_EARTH / (2.0 * final_els.semi_major_axis);
        const double energy_change = energy_after - energy_before;

        INFO("energy_change: " << energy_change);

        REQUIRE_THAT(energy_change, WithinAbs(1292584.077011, A_TOL));
    }

    SECTION("continuous burn energy increases monotonically") {
        const auto final_els = simulate_continuous_burn(
            energy_cons->orbit, earth->mass, 150.0, 6000.0, 120, BURN_PROGRADE);

        const double m = energy_cons->mass;
        const double ke_init = 0.5 * m * vec3_dot(energy_cons->local_velocity,
                                                    energy_cons->local_velocity);
        const double pe_init = -G * m * earth->mass / vec3_magnitude(energy_cons->local_position);
        const double E_init = ke_init + pe_init;

        Vec3 pos, vel;
        orbital_elements_to_cartesian(final_els, earth->mass, &pos, &vel);
        const double ke_final = 0.5 * m * vec3_dot(vel, vel);
        const double pe_final = -G * m * earth->mass / vec3_magnitude(pos);
        const double E_final = ke_final + pe_final;

        const double total_dE = E_final - E_init;
        const double v_circ = sqrt(MU_EARTH / energy_cons->orbit.semi_major_axis);
        const double expected_approx = m * v_circ * 150.0;
        const double rel_err = fabs(total_dE - expected_approx) / expected_approx;

        INFO("E_init: " << E_init);
        INFO("E_final: " << E_final);
        INFO("total_dE: " << total_dE);
        INFO("expected_approx: " << expected_approx);
        INFO("relative_error: " << rel_err);

        REQUIRE_THAT(total_dE, WithinAbs(1048578803.759296, A_TOL));
        REQUIRE_THAT(rel_err, WithinAbs(0.0, 0.01));
    }

    SECTION("continuous vs impulsive burn agree within 1%") {
        const auto orbit_cont = simulate_continuous_burn(
            low_thrust->orbit, earth->mass, 100.0, 5000.0, 100, BURN_PROGRADE);
        const auto orbit_imp = simulate_continuous_burn(
            low_thrust->orbit, earth->mass, 100.0, 5000.0, 1, BURN_PROGRADE);

        const double diff_a = fabs(orbit_cont.semi_major_axis - orbit_imp.semi_major_axis);
        const double rel_diff = diff_a / orbit_cont.semi_major_axis * 100.0;

        const double v_cont = sqrt(MU_EARTH / orbit_cont.semi_major_axis);
        const double v_imp = sqrt(MU_EARTH / orbit_imp.semi_major_axis);
        const double v_diff = fabs(v_cont - v_imp);

        INFO("continuous_a: " << orbit_cont.semi_major_axis);
        INFO("impulsive_a: " << orbit_imp.semi_major_axis);
        INFO("rel_diff_a: " << rel_diff << "%");
        INFO("v_difference: " << v_diff);

        REQUIRE_THAT(rel_diff, WithinAbs(0.0, 1.0));
        REQUIRE_THAT(v_diff, WithinAbs(1.297686, 0.5));
    }

    SECTION("propagation during burn: path length > straight line") {
        OrbitalElements current = low_thrust->orbit;
        const double dt_burn = 5000.0 / 100;
        const double dv_per = 100.0 / 100;

        Vec3 pos_start, pos_end;
        double total_path = 0.0;
        Vec3 prev_pos;
        bool first = true;

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

            if (i == 0) pos_start = pos;
            if (i == 100) pos_end = pos;

            if (!first) {
                total_path += vec3_distance(prev_pos, pos);
            }
            first = false;
            prev_pos = pos;

            if (i < 100) {
                Vec3 dir = get_burn_direction_vector(BURN_PROGRADE, pos, vel);
                vel = vec3_add(vel, vec3_scale(dir, dv_per));
                current = cartesian_to_orbital_elements(pos, vel, earth->mass);
                current = propagate_orbital_elements(current, dt_burn, earth->mass);
            }
        }

        const double straight = vec3_distance(pos_start, pos_end);
        const double r_start = vec3_magnitude(pos_start);
        const double r_end = vec3_magnitude(pos_end);

        INFO("total_path: " << total_path);
        INFO("straight_line: " << straight);
        INFO("r_start: " << r_start);
        INFO("r_end: " << r_end);

        REQUIRE_THAT(total_path, WithinAbs(38113183.100583, A_TOL));
        REQUIRE_THAT(r_end, WithinAbs(6972172.241655, R_TOL));

        // Check final radius within expected bounds
        const double v_init = sqrt(MU_EARTH / low_thrust->orbit.semi_major_axis);
        const double eps_init = -MU_EARTH / (2.0 * low_thrust->orbit.semi_major_axis);
        const double eps_final = eps_init + v_init * 100.0;
        const double a_expected = -MU_EARTH / (2.0 * eps_final);
        const double e_init = low_thrust->orbit.eccentricity;
        const double r_peri = a_expected * (1.0 - e_init);
        const double r_apo = a_expected * (1.0 + e_init);

        INFO("a_expected: " << a_expected);
        INFO("r_peri: " << r_peri);
        INFO("r_apo: " << r_apo);
    }

    SECTION("continuous burn: semi-major axis increases monotonically") {
        OrbitalElements current = low_thrust->orbit;
        const double dt_burn = 5000.0 / 100;
        const double dv_per = 100.0 / 100;

        bool monotonic = true;
        double max_e = 0.0;
        double min_e = 1.0;
        double initial_a = current.semi_major_axis;
        double a_prev = current.semi_major_axis;

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

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

            current = cartesian_to_orbital_elements(pos, vel, earth->mass);
            current = propagate_orbital_elements(current, dt_burn, earth->mass);

            if (current.semi_major_axis < a_prev) monotonic = false;
            a_prev = current.semi_major_axis;

            if (current.eccentricity > max_e) max_e = current.eccentricity;
            if (current.eccentricity < min_e) min_e = current.eccentricity;
        }

        const double final_a = current.semi_major_axis;
        const double total_change = final_a - initial_a;

        // Check deviation from linear trend
        double max_dev = 0.0;
        OrbitalElements cur = low_thrust->orbit;
        a_prev = initial_a;
        for (int i = 0; i < 100; i++) {
            Vec3 pos, vel;
            orbital_elements_to_cartesian(cur, earth->mass, &pos, &vel);
            Vec3 dir = get_burn_direction_vector(BURN_PROGRADE, pos, vel);
            vel = vec3_add(vel, vec3_scale(dir, dv_per));
            cur = cartesian_to_orbital_elements(pos, vel, earth->mass);
            cur = propagate_orbital_elements(cur, dt_burn, earth->mass);

            const double expected = initial_a + (i + 1) * (total_change / 100.0);
            const double dev = fabs(cur.semi_major_axis - expected);
            if (dev > max_dev) max_dev = dev;
        }

        INFO("monotonic: " << (monotonic ? "yes" : "no"));
        INFO("max_ecc: " << max_e);
        INFO("total_a_change: " << total_change);
        INFO("max_deviation: " << max_dev);
        INFO("max_deviation_pct: " << (max_dev / total_change * 100.0) << "%");

        REQUIRE(monotonic);
        REQUIRE_THAT(max_e, WithinAbs(0.009304764034330, E_TOL));
        REQUIRE_THAT(max_dev, WithinAbs(626.066928, A_TOL));
    }

    destroy_simulation(sim);
}