#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/test_utilities.h"
#include <cmath>

using Catch::Matchers::WithinAbs;

SCENARIO("Omega reconstruction after prograde burn at apoapsis", "[omega][debug]") {
    const double parent_mass = 5.972e24;
    const double mu = G * parent_mass;

    OrbitalElements elements = {};
    elements.semi_major_axis = 1.0e7;
    elements.eccentricity = 0.3;
    elements.true_anomaly = M_PI;
    elements.inclination = 1e-12;
    elements.longitude_of_ascending_node = 0.0;
    elements.argument_of_periapsis = 0.0;

    Vec3 pos = {}, vel = {};
    orbital_elements_to_cartesian(elements, parent_mass, &pos, &vel);

    const double r = vec3_magnitude(pos);
    const double v = vec3_magnitude(vel);

    const double r_dot_v = vec3_dot(pos, vel);
    const Vec3 e_vec = {
        ((v * v - mu / r) * pos.x - r_dot_v * vel.x) / mu,
        ((v * v - mu / r) * pos.y - r_dot_v * vel.y) / mu,
        ((v * v - mu / r) * pos.z - r_dot_v * vel.z) / mu,
    };
    const double e_initial = vec3_magnitude(e_vec);

    SECTION("initial apoapsis state is correct") {
        REQUIRE_THAT(r, WithinAbs(1.3e7, R_TOL));
        REQUIRE_THAT(v, WithinAbs(4632.763232589246, V_TOL));
        REQUIRE_THAT(e_initial, WithinAbs(0.3, E_TOL));
        REQUIRE_THAT(e_vec.x / e_initial, WithinAbs(1.0, E_TOL));
        REQUIRE_THAT(e_vec.y, WithinAbs(0.0, E_TOL));
        REQUIRE_THAT(e_vec.z, WithinAbs(0.0, E_TOL));
    }

    SECTION("prograde burn at apoapsis flips periapsis") {
        const Vec3 burn_dir = vec3_normalize(vel);
        const Vec3 delta_v = vec3_scale(burn_dir, 1000.0);
        const Vec3 vel_new = vec3_add(vel, delta_v);
        const double v_new = vec3_magnitude(vel_new);

        const OrbitalElements new_elements = cartesian_to_orbital_elements(pos, vel_new, parent_mass);

        REQUIRE_THAT(new_elements.semi_major_axis, WithinAbs(1.346885753127762e7, A_TOL));
        REQUIRE_THAT(new_elements.eccentricity, WithinAbs(3.481049006486453e-2, E_TOL));
        REQUIRE_THAT(new_elements.argument_of_periapsis, WithinAbs(M_PI, ANG_TOL));
        REQUIRE_THAT(new_elements.true_anomaly, WithinAbs(0.0, ANG_TOL));
        REQUIRE_THAT(new_elements.inclination, WithinAbs(0.0, ANG_TOL));
        REQUIRE_THAT(v_new, WithinAbs(5632.763232589246, V_TOL));
    }
}