#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/simulation.h"
#include "../src/orbital_objects.h"
#include "../src/rendezvous_hohmann.h"
#include "../src/config_loader.h"
#include <cmath>
#include <cstring>


using Catch::Matchers::WithinAbs;


// ============================================================================
// Helper Functions
// ============================================================================

// TODO: Add find_spacecraft_by_name() to simulation.h interface
// FIXME: This helper should be part of the public simulation API for testability
static int find_spacecraft_by_name(SimulationState* sim, const char* name) {
    for (int i = 0; i < sim->craft_count; i++) {
        if (strcmp(sim->spacecraft[i].name, name) == 0) {
            return i;
        }
    }
    return -1;
}


TEST_CASE("Config loading for Hohmann transfer", "[rendezvous_hohmann][config]") {
    const double TIME_STEP = 30.0;

    SimulationState* sim = create_simulation(3, 5, 10, TIME_STEP);

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

    REQUIRE(sim->body_count == 1);
    REQUIRE(std::string(sim->bodies[0].name) == "Earth");

    REQUIRE(sim->craft_count == 3);
    REQUIRE(std::string(sim->spacecraft[0].name) == "Target_Satellite");
    REQUIRE(std::string(sim->spacecraft[1].name) == "Chaser_Lower");
    REQUIRE(std::string(sim->spacecraft[2].name) == "Chaser_Higher");

    REQUIRE(sim->spacecraft[0].parent_index == 0);
    REQUIRE(sim->spacecraft[1].parent_index == 0);
    REQUIRE(sim->spacecraft[2].parent_index == 0);

    // Verify initial orbits
    REQUIRE_THAT(sim->spacecraft[0].orbit.semi_major_axis,
                 WithinAbs(6.771e6, 1.0));  // 400 km altitude
    REQUIRE_THAT(sim->spacecraft[1].orbit.semi_major_axis,
                 WithinAbs(6.671e6, 1.0));  // 300 km altitude
    REQUIRE_THAT(sim->spacecraft[2].orbit.semi_major_axis,
                 WithinAbs(6.871e6, 1.0));  // 500 km altitude

    // Verify initial true anomalies
    REQUIRE_THAT(sim->spacecraft[0].orbit.true_anomaly,
                 WithinAbs(0.0, 0.001));
    REQUIRE_THAT(sim->spacecraft[1].orbit.true_anomaly,
                 WithinAbs(4.71238898038469, 0.001));  // 270° = 3π/2
    REQUIRE_THAT(sim->spacecraft[2].orbit.true_anomaly,
                 WithinAbs(1.5707963267948966, 0.001));  // 90° = π/2

    destroy_simulation(sim);
}

TEST_CASE("Calculate wait time for Hohmann transfer (lower to higher)", "[rendezvous_hohmann][phasing]") {
    const double TIME_STEP = 30.0;

    SimulationState* sim = create_simulation(3, 5, 10, TIME_STEP);

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

    int target_idx = find_spacecraft_by_name(sim, "Target_Satellite");
    int chaser_lower_idx = find_spacecraft_by_name(sim, "Chaser_Lower");

    REQUIRE(target_idx >= 0);
    REQUIRE(chaser_lower_idx >= 0);

    Spacecraft* target = &sim->spacecraft[target_idx];
    Spacecraft* chaser = &sim->spacecraft[chaser_lower_idx];
    CelestialBody* earth = &sim->bodies[0];

    initialize_orbital_objects(sim);

    double r1 = vec3_magnitude(chaser->local_position);
    double r2 = vec3_magnitude(target->local_position);

    SECTION("Zero angular separation - immediate transfer not possible") {
        // If chaser is directly behind target, need to wait for target to move ahead
        double angular_separation = 0.0;
        double wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_separation, earth->mass);

        INFO("Wait time: " << wait_time << " s");
        // Since lower orbit is faster, chaser will catch up, so wait time should be positive
        REQUIRE_THAT(wait_time, WithinAbs(1358.16, 1.0));
    }

    SECTION("Small angular separation") {
        double angular_separation = 0.5;  // ~29 degrees
        double wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_separation, earth->mass);

        INFO("Angular separation: " << angular_separation << " rad");
        INFO("Wait time: " << wait_time << " s");

        REQUIRE_THAT(wait_time, WithinAbs(-18192.7, 1.0));
    }

    SECTION("Large angular separation (near 2π)") {
        double angular_separation = 6.0;  // ~344 degrees
        double wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_separation, earth->mass);

        INFO("Angular separation: " << angular_separation << " rad");
        INFO("Wait time: " << wait_time << " s");

        REQUIRE_THAT(wait_time, WithinAbs(12431.2, 1.0));
    }

    destroy_simulation(sim);
}

TEST_CASE("Calculate wait time for Hohmann transfer (higher to lower)", "[rendezvous_hohmann][phasing]") {
    const double TIME_STEP = 30.0;

    SimulationState* sim = create_simulation(3, 5, 10, TIME_STEP);

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

    int target_idx = find_spacecraft_by_name(sim, "Target_Satellite");
    int chaser_higher_idx = find_spacecraft_by_name(sim, "Chaser_Higher");

    REQUIRE(target_idx >= 0);
    REQUIRE(chaser_higher_idx >= 0);

    Spacecraft* target = &sim->spacecraft[target_idx];
    Spacecraft* chaser = &sim->spacecraft[chaser_higher_idx];
    CelestialBody* earth = &sim->bodies[0];

    initialize_orbital_objects(sim);

    double r1 = vec3_magnitude(chaser->local_position);
    double r2 = vec3_magnitude(target->local_position);

    SECTION("Zero angular separation - target must catch up") {
        // Higher orbit is slower, so target must catch up
        double angular_separation = 0.0;
        double wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_separation, earth->mass);

        INFO("Wait time: " << wait_time << " s");
        REQUIRE_THAT(wait_time, WithinAbs(1414.46, 1.0));
    }

    SECTION("Small angular separation") {
        double angular_separation = 0.3;  // ~17 degrees
        double wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_separation, earth->mass);

        INFO("Angular separation: " << angular_separation << " rad");
        INFO("Wait time: " << wait_time << " s");

        REQUIRE_THAT(wait_time, WithinAbs(13586.2, 1.0));
    }

    destroy_simulation(sim);
}

TEST_CASE("Calculate required separation for Hohmann transfer", "[rendezvous_hohmann][phasing]") {
    const double TIME_STEP = 30.0;

    SimulationState* sim = create_simulation(3, 5, 10, TIME_STEP);

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

    int target_idx = find_spacecraft_by_name(sim, "Target_Satellite");
    int chaser_lower_idx = find_spacecraft_by_name(sim, "Chaser_Lower");
    int chaser_higher_idx = find_spacecraft_by_name(sim, "Chaser_Higher");

    REQUIRE(target_idx >= 0);
    REQUIRE(chaser_lower_idx >= 0);
    REQUIRE(chaser_higher_idx >= 0);

    Spacecraft* target = &sim->spacecraft[target_idx];
    Spacecraft* chaser_lower = &sim->spacecraft[chaser_lower_idx];
    Spacecraft* chaser_higher = &sim->spacecraft[chaser_higher_idx];
    CelestialBody* earth = &sim->bodies[0];

    initialize_orbital_objects(sim);

    double r_lower = vec3_magnitude(chaser_lower->local_position);
    double r_target = vec3_magnitude(target->local_position);
    double r_higher = vec3_magnitude(chaser_higher->local_position);

    SECTION("Lower to higher transfer") {
        double required_separation = calculate_required_separation_for_hohmann(r_lower, r_target, earth->mass);

        INFO("Required separation: " << required_separation << " rad");
        INFO("Required separation (deg): " << required_separation * 180.0 / M_PI << "°");

        REQUIRE_THAT(required_separation, WithinAbs(0.034734, 0.001));
    }

    SECTION("Higher to lower transfer") {
        double required_separation = calculate_required_separation_for_hohmann(r_higher, r_target, earth->mass);

        INFO("Required separation: " << required_separation << " rad");
        INFO("Required separation (deg): " << required_separation * 180.0 / M_PI << "°");

        REQUIRE_THAT(required_separation, WithinAbs(-0.0348625, 0.001));
    }

    SECTION("Equal radii - no transfer needed") {
        double required_separation = calculate_required_separation_for_hohmann(r_target, r_target, earth->mass);

        INFO("Required separation: " << required_separation << " rad");

        REQUIRE_THAT(required_separation, WithinAbs(0.0, 0.001));
    }

    destroy_simulation(sim);
}

SCENARIO("Complete Hohmann transfer phasing workflow", "[rendezvous_hohmann][workflow]") {
    const double TIME_STEP = 10.0;

    SimulationState* sim = create_simulation(3, 5, 10, TIME_STEP);

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

    int target_idx = find_spacecraft_by_name(sim, "Target_Satellite");
    int chaser_lower_idx = find_spacecraft_by_name(sim, "Chaser_Lower");

    REQUIRE(target_idx >= 0);
    REQUIRE(chaser_lower_idx >= 0);

    Spacecraft* target = &sim->spacecraft[target_idx];
    Spacecraft* chaser = &sim->spacecraft[chaser_lower_idx];
    CelestialBody* earth = &sim->bodies[0];

    initialize_orbital_objects(sim);

    SECTION("Calculate and verify phasing for lower-to-higher transfer") {
        double r1 = vec3_magnitude(chaser->local_position);
        double r2 = vec3_magnitude(target->local_position);

        INFO("Chaser orbit radius: " << r1 << " m");
        INFO("Target orbit radius: " << r2 << " m");

        // Calculate current angular separation
        double current_sep = angular_distance(chaser->orbit.true_anomaly, target->orbit.true_anomaly);

        INFO("Current angular separation: " << current_sep << " rad");
        INFO("Current angular separation (deg): " << current_sep * 180.0 / M_PI << "°");

        // Calculate required separation for Hohmann
        double required_sep = calculate_required_separation_for_hohmann(r1, r2, earth->mass);

        INFO("Required separation: " << required_sep << " rad");
        INFO("Required separation (deg): " << required_sep * 180.0 / M_PI << "°");

        // Calculate wait time
        double wait_time = calculate_wait_time_for_hohmann(r1, r2, current_sep, earth->mass);

        INFO("Wait time: " << wait_time << " s");

        REQUIRE_THAT(wait_time, WithinAbs(-60062.651728, 0.1));

        // Verify orbits are circular
        REQUIRE_THAT(chaser->orbit.eccentricity, WithinAbs(0.0, 0.001));
        REQUIRE_THAT(target->orbit.eccentricity, WithinAbs(0.0, 0.001));
    }

    SECTION("Calculate Hohmann transfer parameters") {
        double r1 = vec3_magnitude(chaser->local_position);
        double r2 = vec3_magnitude(target->local_position);

        // Use existing calculate_hohmann_transfer from maneuver.h
        HohmannTransfer hohmann = calculate_hohmann_transfer(r1, r2, earth->mass);

        INFO("First burn delta-v: " << hohmann.dv1 << " m/s");
        INFO("Second burn delta-v: " << hohmann.dv2 << " m/s");
        INFO("Transfer time: " << hohmann.transfer_time << " s");
        INFO("Target true anomaly: " << hohmann.true_anomaly_2 << " rad");

        REQUIRE_THAT(hohmann.dv1, WithinAbs(28.699077, 0.01));
        REQUIRE_THAT(hohmann.dv2, WithinAbs(28.592521, 0.01));
        REQUIRE_THAT(hohmann.transfer_time, WithinAbs(2741.813778, 0.1));
    }

    destroy_simulation(sim);
}