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

const double POSITION_TOLERANCE_METERS = 10000.0;
const double PERIOD_TOLERANCE_SECONDS = 600.0;

TEST_CASE("Molniya orbit - position verification at multiple true anomalies", "[inclined][molniya]") {
    const double TIME_STEP = 60.0;
    const double SEMI_MAJOR_AXIS = 26540000.0;
    const double ECCENTRICITY = 0.74;

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_inclined_orbits.toml"));

    Spacecraft* molniya = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

    SECTION("Position at perigee (true_anomaly = 0)") {
        double expected_radius = SEMI_MAJOR_AXIS * (1.0 - ECCENTRICITY);
        double actual_radius = vec3_magnitude(vec3_sub(molniya->global_position, earth->global_position));
        double radius_error = fabs(actual_radius - expected_radius);

        INFO("Expected radius at perigee: " << expected_radius << " m");
        INFO("Actual radius: " << actual_radius << " m");
        INFO("Error: " << radius_error << " m");

        REQUIRE(radius_error < POSITION_TOLERANCE_METERS);

        CHECK(molniya->global_position.z != 0.0);
        INFO("Z-coordinate should be non-zero for inclined orbit (currently deferred)");
    }

    SECTION("Position at true_anomaly = π/2 (90°)") {
        molniya->orbit.true_anomaly = M_PI / 2.0;
        initialize_orbital_objects(sim);

        double expected_radius = SEMI_MAJOR_AXIS * (1.0 - ECCENTRICITY * ECCENTRICITY) / (1.0 + ECCENTRICITY * cos(M_PI / 2.0));
        double actual_radius = vec3_magnitude(vec3_sub(molniya->global_position, earth->global_position));
        double radius_error = fabs(actual_radius - expected_radius);

        INFO("Expected radius at ν=π/2: " << expected_radius << " m");
        INFO("Actual radius: " << actual_radius << " m");
        INFO("Error: " << radius_error << " m");

        REQUIRE(radius_error < POSITION_TOLERANCE_METERS);

        CHECK(molniya->global_position.z != 0.0);
    }

    SECTION("Position at apogee (true_anomaly = π)") {
        molniya->orbit.true_anomaly = M_PI;
        initialize_orbital_objects(sim);

        double expected_radius = SEMI_MAJOR_AXIS * (1.0 + ECCENTRICITY);
        double actual_radius = vec3_magnitude(vec3_sub(molniya->global_position, earth->global_position));
        double radius_error = fabs(actual_radius - expected_radius);

        INFO("Expected radius at apogee: " << expected_radius << " m");
        INFO("Actual radius: " << actual_radius << " m");
        INFO("Error: " << radius_error << " m");

        REQUIRE(radius_error < POSITION_TOLERANCE_METERS);

        CHECK(molniya->global_position.z != 0.0);
        INFO("At apogee, satellite should be at northernmost point (max z)");
    }

    SECTION("Position at true_anomaly = 3π/2 (270°)") {
        molniya->orbit.true_anomaly = 3.0 * M_PI / 2.0;
        initialize_orbital_objects(sim);

        double expected_radius = SEMI_MAJOR_AXIS * (1.0 - ECCENTRICITY * ECCENTRICITY) / (1.0 + ECCENTRICITY * cos(3.0 * M_PI / 2.0));
        double actual_radius = vec3_magnitude(vec3_sub(molniya->global_position, earth->global_position));
        double radius_error = fabs(actual_radius - expected_radius);

        INFO("Expected radius at ν=3π/2: " << expected_radius << " m");
        INFO("Actual radius: " << actual_radius << " m");
        INFO("Error: " << radius_error << " m");

        REQUIRE(radius_error < POSITION_TOLERANCE_METERS);

        CHECK(molniya->global_position.z != 0.0);
        INFO("At ν=270°, satellite should be at southernmost point (min z)");
    }

    destroy_simulation(sim);
}

TEST_CASE("Molniya orbit - orbital period verification", "[inclined][molniya][period]") {
    const double TIME_STEP = 60.0;
    const double SECONDS_PER_HOUR = 3600.0;
    const double MAX_SIMULATION_HOURS = 15.0;
    // Relaxed tolerance for highly elliptical orbit with 60s timestep
    const double MOLNIYA_PERIOD_TOLERANCE_SECONDS = 1800.0;  // 30 minutes

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_inclined_orbits.toml"));

    Spacecraft* molniya = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

    double semi_major_axis = molniya->orbit.semi_major_axis;
    double mu = G * earth->mass;
    double theoretical_period_seconds = 2.0 * M_PI * sqrt(pow(semi_major_axis, 3) / mu);
    double theoretical_period_hours = theoretical_period_seconds / SECONDS_PER_HOUR;

    INFO("Semi-major axis: " << semi_major_axis << " m");
    INFO("Theoretical period from Kepler's 3rd law: " << theoretical_period_hours << " hours");

    OrbitTracker* tracker = create_orbit_tracker_3d(0, 0.01,
                                                     molniya->orbit.inclination,
                                                     molniya->orbit.longitude_of_ascending_node,
                                                     molniya->orbit.argument_of_periapsis);

    double max_time = MAX_SIMULATION_HOURS * SECONDS_PER_HOUR;
    while (sim->time < max_time && !tracker->orbit_completed) {
        update_simulation(sim);
        update_orbit_tracker(tracker, (CelestialBody*)molniya, earth, sim->time);
    }

    REQUIRE(tracker->orbit_completed);

    double measured_period_hours = tracker->time_at_completion / SECONDS_PER_HOUR;
    double period_error_hours = fabs(measured_period_hours - theoretical_period_hours);

    INFO("Measured period: " << measured_period_hours << " hours");
    INFO("Period error: " << period_error_hours << " hours");
    INFO("Period error: " << (period_error_hours / theoretical_period_hours * 100.0) << "%");

    REQUIRE(period_error_hours * SECONDS_PER_HOUR < MOLNIYA_PERIOD_TOLERANCE_SECONDS);

    destroy_orbit_tracker(tracker);
    destroy_simulation(sim);
}

TEST_CASE("Generic inclined orbit - moderate inclination", "[inclined][generic]") {
    const double TIME_STEP = 60.0;
    const double SEMI_MAJOR_AXIS = 10000000.0;
    const double ECCENTRICITY = 0.5;
    const double INCLINATION_DEG = 45.0;
    const double INCLINATION_RAD = INCLINATION_DEG * M_PI / 180.0;

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_inclined_orbits.toml"));

    Spacecraft* craft = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

    craft->orbit.semi_major_axis = SEMI_MAJOR_AXIS;
    craft->orbit.eccentricity = ECCENTRICITY;
    craft->orbit.true_anomaly = 0.0;
    craft->orbit.inclination = INCLINATION_RAD;
    craft->orbit.longitude_of_ascending_node = 0.0;
    craft->orbit.argument_of_periapsis = M_PI / 2.0;

    initialize_orbital_objects(sim);

    SECTION("Z-coordinate is non-zero for inclined orbit") {
        double z_position = craft->global_position.z;
        INFO("Z-coordinate: " << z_position << " m");

        REQUIRE(z_position != 0.0);
    }

    SECTION("Position magnitude matches orbital radius") {
        double position_vector_mag = vec3_magnitude(craft->global_position);
        double orbital_radius = vec3_magnitude(vec3_sub(craft->global_position, earth->global_position));
        double magnitude_error = fabs(position_vector_mag - orbital_radius);

        INFO("Position vector magnitude: " << position_vector_mag << " m");
        INFO("Orbital radius: " << orbital_radius << " m");
        INFO("Error: " << magnitude_error << " m");

        REQUIRE(magnitude_error < POSITION_TOLERANCE_METERS);
    }

    destroy_simulation(sim);
}

TEST_CASE("Inclined orbit - inclination parameter is preserved", "[inclined][config]") {
    const double TIME_STEP = 60.0;
    const double EXPECTED_INCLINATION_RAD = 1.107;
    const double EXPECTED_INCLINATION_DEG = EXPECTED_INCLINATION_RAD * 180.0 / M_PI;

    SimulationState* sim = create_simulation(10, 1, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_inclined_orbits.toml"));

    Spacecraft* molniya = &sim->spacecraft[0];

    INFO("Loaded inclination: " << (molniya->orbit.inclination * 180.0 / M_PI) << " degrees");
    INFO("Expected inclination: " << EXPECTED_INCLINATION_DEG << " degrees");

    REQUIRE_THAT(molniya->orbit.inclination, Catch::Matchers::WithinAbs(EXPECTED_INCLINATION_RAD, 0.01));

    destroy_simulation(sim);
}