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

TEST_CASE("True anomaly round-trip conversion at periapsis", "[orbital_elements][true_anomaly]") {
    double parent_mass = 5.972e24;

    OrbitalElements elements = {0};
    elements.semi_major_axis = 7000e3;
    elements.eccentricity = 0.3;
    elements.true_anomaly = 0.0;
    elements.inclination = 0.0;
    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);
    OrbitalElements reconstructed = cartesian_to_orbital_elements(pos, vel, parent_mass);

    INFO("Original true_anomaly: " << elements.true_anomaly);
    INFO("Reconstructed true_anomaly: " << reconstructed.true_anomaly);

    REQUIRE_THAT(reconstructed.true_anomaly,
                 Catch::Matchers::WithinAbs(elements.true_anomaly, 0.01));
}

TEST_CASE("True anomaly round-trip conversion at apoapsis", "[orbital_elements][true_anomaly]") {
    double parent_mass = 5.972e24;

    OrbitalElements elements = {0};
    elements.semi_major_axis = 7000e3;
    elements.eccentricity = 0.3;
    elements.true_anomaly = M_PI;
    elements.inclination = 0.0;
    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);
    OrbitalElements reconstructed = cartesian_to_orbital_elements(pos, vel, parent_mass);

    INFO("Original true_anomaly: " << elements.true_anomaly);
    INFO("Reconstructed true_anomaly: " << reconstructed.true_anomaly);

    REQUIRE_THAT(reconstructed.true_anomaly,
                 Catch::Matchers::WithinAbs(elements.true_anomaly, 0.01));
}

TEST_CASE("True anomaly round-trip conversion at 90 degrees", "[orbital_elements][true_anomaly]") {
    double parent_mass = 5.972e24;

    OrbitalElements elements = {0};
    elements.semi_major_axis = 7000e3;
    elements.eccentricity = 0.3;
    elements.true_anomaly = M_PI / 2.0;
    elements.inclination = 0.0;
    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);
    OrbitalElements reconstructed = cartesian_to_orbital_elements(pos, vel, parent_mass);

    INFO("Original true_anomaly: " << elements.true_anomaly);
    INFO("Reconstructed true_anomaly: " << reconstructed.true_anomaly);

    REQUIRE_THAT(reconstructed.true_anomaly,
                 Catch::Matchers::WithinAbs(elements.true_anomaly, 0.01));
}

TEST_CASE("True anomaly round-trip conversion at 270 degrees", "[orbital_elements][true_anomaly]") {
    double parent_mass = 5.972e24;

    OrbitalElements elements = {0};
    elements.semi_major_axis = 7000e3;
    elements.eccentricity = 0.3;
    elements.true_anomaly = 3.0 * M_PI / 2.0;
    elements.inclination = 0.0;
    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);
    OrbitalElements reconstructed = cartesian_to_orbital_elements(pos, vel, parent_mass);

    INFO("Original true_anomaly: " << elements.true_anomaly);
    INFO("Reconstructed true_anomaly: " << reconstructed.true_anomaly);

    REQUIRE_THAT(reconstructed.true_anomaly,
                 Catch::Matchers::WithinAbs(elements.true_anomaly, 0.01));
}

TEST_CASE("Radius at periapsis matches expected value", "[orbital_elements][sanity]") {
    double parent_mass = 5.972e24;

    OrbitalElements peri = {0};
    peri.semi_major_axis = 7000e3;
    peri.eccentricity = 0.3;
    peri.true_anomaly = 0.0;

    Vec3 pos, vel;
    orbital_elements_to_cartesian(peri, parent_mass, &pos, &vel);
    double r_peri = vec3_magnitude(pos);
    double expected_peri = peri.semi_major_axis * (1.0 - peri.eccentricity);

    INFO("At true_anomaly=0:");
    INFO("  Calculated radius: " << r_peri);
    INFO("  Expected: " << expected_peri);

    REQUIRE_THAT(r_peri, Catch::Matchers::WithinAbs(expected_peri, 1.0));
}

TEST_CASE("Radius at apoapsis matches expected value", "[orbital_elements][sanity]") {
    double parent_mass = 5.972e24;

    OrbitalElements apo = {0};
    apo.semi_major_axis = 7000e3;
    apo.eccentricity = 0.3;
    apo.true_anomaly = M_PI;

    Vec3 pos, vel;
    orbital_elements_to_cartesian(apo, parent_mass, &pos, &vel);
    double r_apo = vec3_magnitude(pos);
    double expected_apo = apo.semi_major_axis * (1.0 + apo.eccentricity);

    INFO("At true_anomaly=pi:");
    INFO("  Calculated radius: " << r_apo);
    INFO("  Expected: " << expected_apo);

    REQUIRE_THAT(r_apo, Catch::Matchers::WithinAbs(expected_apo, 1.0));
}