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

// Test: Check omega after prograde burn that flips eccentricity vector direction
TEST_CASE("Omega calculation after prograde burn", "[omega][debug]") {
    double parent_mass = 5.972e24;  // Earth mass

    // Initial orbit: zero inclination, omega = 0
    // Start at apoapsis where eccentricity vector points opposite to position
    OrbitalElements elements = {0};
    elements.semi_major_axis = 1.0e7;
    elements.eccentricity = 0.3;
    elements.true_anomaly = M_PI;  // Start at apoapsis
    elements.inclination = 1e-12;  // Tiny inclination to trigger atan2 path
    elements.longitude_of_ascending_node = 0.0;
    elements.argument_of_periapsis = 0.0;

    // Get initial position and velocity
    Vec3 pos, vel;
    orbital_elements_to_cartesian(elements, parent_mass, &pos, &vel);

    // Get initial eccentricity vector
    Vec3 v = vel;
    double r = vec3_magnitude(pos);
    double mu = G * parent_mass;
    Vec3 e_vec_initial = vec3_scale(vec3_sub(vec3_scale(vel, r), vec3_scale(pos, vec3_magnitude(vel) / mu)), 1.0 / mu);
    double e_initial = vec3_magnitude(e_vec_initial);

    INFO("Initial state:");
    INFO("  e = " << e_initial);
    INFO("  e_vec = (" << e_vec_initial.x << ", " << e_vec_initial.y << ", " << e_vec_initial.z << ")");
    INFO("  pos = (" << pos.x << ", " << pos.y << ", " << pos.z << ")");
    INFO("  vel = (" << vel.x << ", " << vel.y << ", " << vel.z << ")");

    // Apply a prograde burn at periapsis
    Vec3 vel_dir = vec3_normalize(vel);
    Vec3 delta_v = vec3_scale(vel_dir, 1000.0);  // Large burn to flip e_vec
    vel = vec3_add(vel, delta_v);

    // Reconstruct orbital elements
    OrbitalElements new_elements = cartesian_to_orbital_elements(pos, vel, parent_mass);

    INFO("After prograde burn:");
    INFO("  new omega = " << new_elements.argument_of_periapsis << " rad (" << new_elements.argument_of_periapsis * 180.0 / M_PI << " deg)");
    INFO("  new e = " << new_elements.eccentricity);

    // Get new eccentricity vector
    Vec3 e_vec_new = vec3_scale(vec3_sub(vec3_scale(vel, r), vec3_scale(pos, vec3_magnitude(vel) / mu)), 1.0 / mu);
    INFO("  new e_vec = (" << e_vec_new.x << ", " << e_vec_new.y << ", " << e_vec_new.z << ")");

    // For zero-inclination orbit, omega should stay 0 (or 2π which is equivalent)
    // If omega becomes π, that's a bug
    bool omega_correct = (new_elements.argument_of_periapsis < M_PI / 2.0) ||
                        (new_elements.argument_of_periapsis > 1.5 * M_PI);

    REQUIRE(omega_correct);
}