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


using Catch::Matchers::WithinAbs;


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

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.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);

    REQUIRE_THAT(sim->spacecraft[0].orbit.semi_major_axis, WithinAbs(6.771e6, 1.0));
    REQUIRE_THAT(sim->spacecraft[1].orbit.semi_major_axis, WithinAbs(6.671e6, 1.0));
    REQUIRE_THAT(sim->spacecraft[2].orbit.semi_major_axis, WithinAbs(6.871e6, 1.0));

    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));
    REQUIRE_THAT(sim->spacecraft[2].orbit.true_anomaly, WithinAbs(1.5707963267948966, 0.001));

    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.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") {
        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");
        // At 0 separation, chaser is faster and needs to be behind target
        // Wait time is negative (should have burned already)
        REQUIRE_THAT(wait_time, WithinAbs(-1358.16, 1.0));
    }

    SECTION("Small angular separation") {
        double angular_separation = 0.5;
        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(-20909.0, 1.0));
    }

    SECTION("Large angular separation (near 2pi)") {
        double angular_separation = 6.0;
        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(9714.9, 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.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") {
        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");
        // Higher orbit case: chaser is slower, needs to be ahead of target
        REQUIRE_THAT(wait_time, WithinAbs(-1414.46, 1.0));
    }

    SECTION("Small angular separation") {
        double angular_separation = 0.3;
        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(10757.3, 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.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 << " deg");

        REQUIRE_THAT(required_separation, WithinAbs(-0.034734, 0.001));  // Chaser faster, needs to be behind
    }

    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 << " deg");

        REQUIRE_THAT(required_separation, WithinAbs(0.0348625, 0.001));  // Chaser slower, needs to be ahead
    }

    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);
}

// ============================================================================
// New Test Cases for Validation and Next Wait Time
// ============================================================================

TEST_CASE("Validate Hohmann transfer parameters", "[rendezvous_hohmann][validation]") {
    const double TIME_STEP = 30.0;

    SimulationState* sim = create_simulation(3, 5, 10, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml"));
    initialize_orbital_objects(sim);

    Spacecraft* chaser_lower = &sim->spacecraft[1];
    Spacecraft* chaser_higher = &sim->spacecraft[2];
    Spacecraft* target = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

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

    SECTION("Valid lower to higher transfer") {
        bool valid = validate_hohmann_transfer_parameters(r_lower, r_target, earth->mass);
        INFO("Valid lower to higher: " << (valid ? "true" : "false"));
        REQUIRE(valid == true);
    }

    SECTION("Valid higher to lower transfer") {
        bool valid = validate_hohmann_transfer_parameters(r_higher, r_target, earth->mass);
        INFO("Valid higher to lower: " << (valid ? "true" : "false"));
        REQUIRE(valid == true);
    }

    SECTION("Equal radii - invalid (no relative motion)") {
        bool valid = validate_hohmann_transfer_parameters(r_target, r_target, earth->mass);
        INFO("Equal radii valid: " << (valid ? "true" : "false"));
        REQUIRE(valid == false);
    }

    SECTION("Negative initial radius - invalid") {
        bool valid = validate_hohmann_transfer_parameters(-1000.0, r_target, earth->mass);
        INFO("Negative radius valid: " << (valid ? "true" : "false"));
        REQUIRE(valid == false);
    }

    SECTION("Negative target radius - invalid") {
        bool valid = validate_hohmann_transfer_parameters(r_lower, -1000.0, earth->mass);
        INFO("Negative target radius valid: " << (valid ? "true" : "false"));
        REQUIRE(valid == false);
    }

    SECTION("Zero central mass - invalid") {
        bool valid = validate_hohmann_transfer_parameters(r_lower, r_target, 0.0);
        INFO("Zero mass valid: " << (valid ? "true" : "false"));
        REQUIRE(valid == false);
    }

    SECTION("Negative central mass - invalid") {
        bool valid = validate_hohmann_transfer_parameters(r_lower, r_target, -1.0);
        INFO("Negative mass valid: " << (valid ? "true" : "false"));
        REQUIRE(valid == false);
    }

    destroy_simulation(sim);
}

TEST_CASE("Calculate relative orbit period", "[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.toml"));
    initialize_orbital_objects(sim);

    Spacecraft* chaser_lower = &sim->spacecraft[1];
    Spacecraft* chaser_higher = &sim->spacecraft[2];
    Spacecraft* target = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

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

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

        INFO("Relative orbit period: " << rel_period << " s");
        INFO("Relative orbit period: " << rel_period / 3600.0 << " h");

        REQUIRE(rel_period > 0.0);
        REQUIRE_THAT(rel_period, WithinAbs(245683.24, 1.0));
    }

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

        INFO("Relative orbit period: " << rel_period << " s");
        INFO("Relative orbit period: " << rel_period / 3600.0 << " h");

        REQUIRE(rel_period > 0.0);
        REQUIRE_THAT(rel_period, WithinAbs(254924.71, 1.0));
    }

    SECTION("Very small radius difference - long period") {
        double r1 = 6770000.0;
        double r2 = 6771000.0;
        double rel_period = calculate_relative_orbit_period(r1, r2, earth->mass);

        INFO("Relative orbit period: " << rel_period << " s");
        INFO("Relative orbit period: " << rel_period / 3600.0 / 24.0 << " days");

        REQUIRE(rel_period > 0.0);
        REQUIRE_THAT(rel_period, WithinAbs(25025208.27, 1.0));
    }

    SECTION("Large radius difference - short period") {
        double r1 = 6571000.0;
        double r2 = 7071000.0;
        double rel_period = calculate_relative_orbit_period(r1, r2, earth->mass);

        INFO("Relative orbit period: " << rel_period << " s");
        INFO("Relative orbit period: " << rel_period / 3600.0 << " h");

        REQUIRE(rel_period > 0.0);
        REQUIRE_THAT(rel_period, WithinAbs(50889.08, 1.0));
    }

    destroy_simulation(sim);
}

TEST_CASE("Calculate next valid wait time 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.toml"));
    initialize_orbital_objects(sim);

    Spacecraft* chaser_lower = &sim->spacecraft[1];
    Spacecraft* chaser_higher = &sim->spacecraft[2];
    Spacecraft* target = &sim->spacecraft[0];
    CelestialBody* earth = &sim->bodies[0];

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

    SECTION("Lower to higher - positive wait time") {
        double angular_separation = 0.0;
        double next_wait = calculate_next_hohmann_wait_time(r_lower, r_target, angular_separation, earth->mass, TIME_STEP);

        INFO("Angular separation: " << angular_separation << " rad");
        INFO("Next wait time: " << next_wait << " s");
        INFO("Next wait time: " << next_wait / 3600.0 << " h");

        REQUIRE(next_wait >= TIME_STEP);
        REQUIRE_THAT(next_wait, WithinAbs(244325.08, 1.0));
    }

    SECTION("Higher to lower - negative wait time becomes next cycle") {
        double angular_separation = 0.0;
        double next_wait = calculate_next_hohmann_wait_time(r_higher, r_target, angular_separation, earth->mass, TIME_STEP);

        INFO("Angular separation: " << angular_separation << " rad");
        INFO("Next wait time: " << next_wait << " s");
        INFO("Next wait time: " << next_wait / 3600.0 << " h");

        REQUIRE(next_wait >= TIME_STEP);
        REQUIRE_THAT(next_wait, WithinAbs(253510.25, 1.0));
    }

    SECTION("Higher to lower - positive wait time (no adjustment needed)") {
        double angular_separation = 0.3;
        double next_wait = calculate_next_hohmann_wait_time(r_higher, r_target, angular_separation, earth->mass, TIME_STEP);

        INFO("Angular separation: " << angular_separation << " rad");
        INFO("Next wait time: " << next_wait << " s");

        REQUIRE(next_wait >= TIME_STEP);
        REQUIRE_THAT(next_wait, WithinAbs(10757.30, 1.0));
    }

    SECTION("At required separation - forced to next cycle by min_wait") {
        double req_sep = calculate_required_separation_for_hohmann(r_lower, r_target, earth->mass);
        double next_wait = calculate_next_hohmann_wait_time(r_lower, r_target, req_sep, earth->mass, TIME_STEP);

        INFO("Required separation: " << req_sep << " rad");
        INFO("Next wait time: " << next_wait << " s");
        INFO("Next wait time: " << next_wait / 3600.0 << " h");

        REQUIRE(next_wait >= TIME_STEP);
        REQUIRE_THAT(next_wait, WithinAbs(245683.24, 1.0));
    }

    SECTION("Force skip to next cycle with large min_wait") {
        double angular_separation = 0.5;
        double min_wait = 30000.0;
        double next_wait = calculate_next_hohmann_wait_time(r_lower, r_target, angular_separation, earth->mass, min_wait);

        INFO("Angular separation: " << angular_separation << " rad");
        INFO("Min wait: " << min_wait << " s");
        INFO("Next wait time: " << next_wait << " s");

        REQUIRE(next_wait >= min_wait);
        REQUIRE_THAT(next_wait, WithinAbs(224774.23, 1.0));
    }

    SECTION("Zero min_wait allows immediate transfer") {
        double angular_separation = 0.3;
        double next_wait = calculate_next_hohmann_wait_time(r_higher, r_target, angular_separation, earth->mass, 0.0);

        INFO("Angular separation: " << angular_separation << " rad");
        INFO("Next wait time: " << next_wait << " s");

        REQUIRE(next_wait >= 0.0);
        REQUIRE_THAT(next_wait, WithinAbs(10757.30, 1.0));
    }

    destroy_simulation(sim);
}

SCENARIO("Hohmann transfer rendezvous with validation", "[rendezvous_hohmann][integration]") {
    const double TIME_STEP = 0.1;

    SimulationState* sim = create_simulation(3, 5, 10, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/test_rendezvous.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("Execute validated Hohmann transfer and verify rendezvous") {
        double r1 = vec3_magnitude(chaser->local_position);
        double r2 = vec3_magnitude(target->local_position);

        INFO("\n=== HOHMANN TRANSFER RENDZVOUS TEST ===");
        INFO("Initial state:");
        INFO("  Chaser: r=" << r1 << " m, nu=" << chaser->orbit.true_anomaly << " rad");
        INFO("  Target: r=" << r2 << " m, nu=" << target->orbit.true_anomaly << " rad");

        // Calculate angular separation
        double angular_separation = chaser->orbit.true_anomaly - target->orbit.true_anomaly;
        while (angular_separation > M_PI) angular_separation -= 2.0 * M_PI;
        while (angular_separation < -M_PI) angular_separation += 2.0 * M_PI;
        INFO("  Angular separation: " << angular_separation << " rad");

        // Validate parameters before proceeding
        REQUIRE(validate_hohmann_transfer_parameters(r1, r2, earth->mass));
        INFO("  Validation passed");

        // Calculate Hohmann transfer parameters
        HohmannTransfer hohmann = calculate_hohmann_transfer(r1, r2, earth->mass);
        double wait_time = calculate_next_hohmann_wait_time(r1, r2, angular_separation, earth->mass, TIME_STEP);
        double arrival_time = wait_time + hohmann.transfer_time;

        INFO("\nTransfer parameters:");
        INFO("  Transfer time: " << hohmann.transfer_time << " s");
        INFO("  DV1 (departure): " << hohmann.dv1 << " m/s");
        INFO("  DV2 (arrival): " << hohmann.dv2 << " m/s");
        INFO("  Wait time: " << wait_time << " s");
        INFO("  Arrival time: " << arrival_time << " s");

        // Create departure maneuver
        Maneuver departure = create_maneuver(
            "Departure_Burn",
            chaser_lower_idx,
            BURN_PROGRADE,
            hohmann.dv1,
            TRIGGER_TIME,
            wait_time
        );
        int dep_idx = add_maneuver_to_simulation(sim, &departure);
        REQUIRE(dep_idx >= 0);

        // Create arrival maneuver
        Maneuver arrival = create_maneuver(
            "Circularization_Burn",
            chaser_lower_idx,
            BURN_PROGRADE,
            hohmann.dv2,
            TRIGGER_TIME,
            arrival_time
        );
        int arr_idx = add_maneuver_to_simulation(sim, &arrival);
        REQUIRE(arr_idx >= 0);

        // Dump initial state
        dump_simulation_state(sim, "INITIAL STATE");

        // Run simulation
        double sim_time = 0.0;
        const double DT = 1.0;
        const int MAX_STEPS = 700000;
        bool transfer_complete = false;

        for (int i = 0; i < MAX_STEPS; i++) {
            update_simulation(sim);
            sim_time += DT;

            // Dump state at key milestones
            if (i == 0) {
                dump_simulation_state(sim, "T=0 (initial)");
            }
            if (i == int(wait_time / DT)) {
                dump_simulation_state(sim, "JUST BEFORE DEPARTURE");
            }
            if (i == int(wait_time / DT) + 1) {
                dump_simulation_state(sim, "AFTER DEPARTURE BURN");
            }
            if (i == int(arrival_time / DT)) {
                dump_simulation_state(sim, "JUST BEFORE ARRIVAL");
            }
            if (sim->maneuvers[arr_idx].executed && !transfer_complete) {
                dump_simulation_state(sim, "AFTER ARRIVAL BURN");
                transfer_complete = true;
                break;
            }
        }

        INFO("\n=== FINAL STATE ===");
        INFO("Final time: " << sim_time << " s");
        INFO("Departure executed: " << sim->maneuvers[dep_idx].executed);
        INFO("Arrival executed: " << sim->maneuvers[arr_idx].executed);
        dump_simulation_state(sim, "FINAL STATE");

        // Verify maneuvers executed
        REQUIRE(sim->maneuvers[dep_idx].executed);
        REQUIRE(sim->maneuvers[arr_idx].executed);

        // Verify rendezvous quality
        double final_radius = vec3_magnitude(chaser->local_position);
        double radius_error = fabs(final_radius - r2);
        INFO("\nVerification:");
        INFO("  Radius error: " << radius_error << " m");
        INFO("  Chaser eccentricity: " << chaser->orbit.eccentricity);

        Vec3 chaser_vel = chaser->local_velocity;
        Vec3 target_vel = target->local_velocity;
        double chaser_speed = vec3_magnitude(chaser_vel);
        double target_speed = vec3_magnitude(target_vel);
        Vec3 separation = vec3_sub(chaser->local_position, target->local_position);
        double separation_distance = vec3_magnitude(separation);
        double relative_velocity = vec3_magnitude(vec3_sub(chaser_vel, target_vel));

        INFO("  Chaser speed: " << chaser_speed << " m/s");
        INFO("  Target speed: " << target_speed << " m/s");
        INFO("  Separation: " << separation_distance << " m");
        INFO("  Relative velocity: " << relative_velocity << " m/s");

        REQUIRE_THAT(radius_error, WithinAbs(10.0, 10.0));
        REQUIRE_THAT(chaser->orbit.eccentricity, WithinAbs(0.0, 0.001));
        REQUIRE_THAT(chaser_speed, WithinAbs(target_speed, 1.0));
        REQUIRE_THAT(separation_distance, WithinAbs(0.0, 100.0));
    }

    destroy_simulation(sim);
}