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

TEST_CASE("Barker's equation - zero mean anomaly", "[barker][analytical]") {
    double M = 0.0;
    double nu = solve_barker_equation(M);
    double nu_expected = 0.0;
    REQUIRE_THAT(nu, Catch::Matchers::WithinAbs(nu_expected, 1e-15));
}

TEST_CASE("Barker's equation - small positive mean anomaly", "[barker][analytical]") {
    double M = 0.1;
    double nu = solve_barker_equation(M);
    REQUIRE(nu > 0.0);
    REQUIRE(nu < M_PI);
    double D = tan(nu / 2.0);
    double M_recovered = D + (D * D * D) / 3.0;
    REQUIRE_THAT(M_recovered, Catch::Matchers::WithinAbs(M, 1e-14));
}

TEST_CASE("Barker's equation - moderate positive mean anomaly", "[barker][analytical]") {
    double M = 1.0;
    double nu = solve_barker_equation(M);
    REQUIRE(nu > 0.0);
    REQUIRE(nu < M_PI);
    double D = tan(nu / 2.0);
    double M_recovered = D + (D * D * D) / 3.0;
    REQUIRE_THAT(M_recovered, Catch::Matchers::WithinAbs(M, 1e-14));
}

TEST_CASE("Barker's equation - large positive mean anomaly", "[barker][analytical]") {
    double M = 5.0;
    double nu = solve_barker_equation(M);
    REQUIRE(nu > 0.0);
    REQUIRE(nu < M_PI);
    double D = tan(nu / 2.0);
    double M_recovered = D + (D * D * D) / 3.0;
    REQUIRE_THAT(M_recovered, Catch::Matchers::WithinAbs(M, 1e-14));
}

TEST_CASE("Barker's equation - very large mean anomaly", "[barker][analytical]") {
    double M = 20.0;
    double nu = solve_barker_equation(M);
    REQUIRE(nu > 0.0);
    REQUIRE(nu < M_PI);
    double D = tan(nu / 2.0);
    double M_recovered = D + (D * D * D) / 3.0;
    REQUIRE_THAT(M_recovered, Catch::Matchers::WithinAbs(M, 1e-13));
}

TEST_CASE("Barker's equation - small negative mean anomaly", "[barker][analytical]") {
    double M = -0.1;
    double nu = solve_barker_equation(M);
    REQUIRE(nu < 0.0);
    REQUIRE(nu > -M_PI);
    double D = tan(nu / 2.0);
    double M_recovered = D + (D * D * D) / 3.0;
    REQUIRE_THAT(M_recovered, Catch::Matchers::WithinAbs(M, 1e-14));
}

TEST_CASE("Barker's equation - moderate negative mean anomaly", "[barker][analytical]") {
    double M = -1.0;
    double nu = solve_barker_equation(M);
    REQUIRE(nu < 0.0);
    REQUIRE(nu > -M_PI);
    double D = tan(nu / 2.0);
    double M_recovered = D + (D * D * D) / 3.0;
    REQUIRE_THAT(M_recovered, Catch::Matchers::WithinAbs(M, 1e-14));
}

TEST_CASE("Barker's equation - large negative mean anomaly", "[barker][analytical]") {
    double M = -5.0;
    double nu = solve_barker_equation(M);
    REQUIRE(nu < 0.0);
    REQUIRE(nu > -M_PI);
    double D = tan(nu / 2.0);
    double M_recovered = D + (D * D * D) / 3.0;
    REQUIRE_THAT(M_recovered, Catch::Matchers::WithinAbs(M, 1e-14));
}

TEST_CASE("Barker's equation - round-trip conversion", "[barker][analytical]") {
    std::vector<double> test_values = {-10.0, -5.0, -1.0, -0.5, -0.1, 0.0, 0.1, 0.5, 1.0, 5.0, 10.0};

    for (double M_original : test_values) {
        double nu = solve_barker_equation(M_original);
        double D = tan(nu / 2.0);
        double M_recovered = D + (D * D * D) / 3.0;
        REQUIRE_THAT(M_recovered, Catch::Matchers::WithinAbs(M_original, 1e-13));
    }
}

TEST_CASE("Barker's equation - true anomaly range", "[barker][analytical]") {
    for (double M = -50.0; M <= 50.0; M += 1.0) {
        double nu = solve_barker_equation(M);
        REQUIRE(nu > -M_PI * 0.99);
        REQUIRE(nu < M_PI * 0.99);
    }
}

TEST_CASE("Parabolic orbit propagation using Barker's equation", "[barker][propagation]") {
    const double PARENT_MASS = 1.989e30;
    const double TIME_STEP = 3600.0;
    const int NUM_STEPS = 24;

    OrbitalElements initial;
    initial.semi_latus_rectum = 2.992e11;
    initial.eccentricity = 1.0;
    initial.true_anomaly = 0.0;
    initial.inclination = 0.0;
    initial.longitude_of_ascending_node = 0.0;
    initial.argument_of_periapsis = 0.0;

    Vec3 pos, vel;
    orbital_elements_to_cartesian(initial, PARENT_MASS, &pos, &vel);

    double initial_distance = vec3_magnitude(pos);
    double initial_velocity = vec3_magnitude(vel);
    double escape_velocity = sqrt(2.0 * G * PARENT_MASS / initial_distance);

    INFO("Initial distance: " << initial_distance / 1.496e11 << " AU");
    INFO("Initial velocity: " << initial_velocity / 1000.0 << " km/s");
    INFO("Escape velocity: " << escape_velocity / 1000.0 << " km/s");

    REQUIRE_THAT(initial_velocity, Catch::Matchers::WithinAbs(escape_velocity, 1.0));

    OrbitalElements current = initial;
    double total_time = 0.0;

    for (int step = 0; step < NUM_STEPS; step++) {
        OrbitalElements next = propagate_orbital_elements(current, TIME_STEP, PARENT_MASS);
        current = next;
        total_time += TIME_STEP;
    }

    Vec3 pos_final, vel_final;
    orbital_elements_to_cartesian(current, PARENT_MASS, &pos_final, &vel_final);

    double final_distance = vec3_magnitude(pos_final);
    double final_velocity = vec3_magnitude(vel_final);

    INFO("Final true anomaly: " << current.true_anomaly << " rad");
    INFO("Final distance: " << final_distance / 1.496e11 << " AU");
    INFO("Final velocity: " << final_velocity / 1000.0 << " km/s");

    REQUIRE(final_distance > initial_distance);

    REQUIRE(final_velocity < initial_velocity);

    double final_escape_velocity = sqrt(2.0 * G * PARENT_MASS / final_distance);
    INFO("Final escape velocity: " << final_escape_velocity / 1000.0 << " km/s");

    REQUIRE_THAT(final_velocity, Catch::Matchers::WithinAbs(final_escape_velocity, 1.0));
}