#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 <cmath>
#include <cstring>

// Tolerances for rendezvous testing
const double POSITION_TOLERANCE = 100.0;      // 100 m position tolerance for encounter
const double VELOCITY_TOLERANCE = 0.1;        // 0.1 m/s velocity tolerance
const double CW_SPATIAL_TOLERANCE = 0.001;    // 0.1% for CW validity checks
const double TIME_TOLERANCE = 1.0;            // 1 second time tolerance

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

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

void initialize_rendezvous_for_spacecraft(
    SimulationState* sim,
    const char* chaser_name,
    const char* target_name,
    double approach_distance,
    double capture_distance,
    double max_relative_velocity
) {
    int chaser_index = find_spacecraft_by_name(sim, chaser_name);
    int target_index = find_spacecraft_by_name(sim, target_name);

    REQUIRE(chaser_index >= 0);
    REQUIRE(target_index >= 0);

    Spacecraft* chaser = &sim->spacecraft[chaser_index];
    Spacecraft* target = &sim->spacecraft[target_index];

    // Initialize rendezvous target on chaser
    initialize_rendezvous_target(
        &chaser->rendezvous_target,
        target_index,
        true,  // is spacecraft target
        approach_distance,
        capture_distance,
        max_relative_velocity
    );

    // Initialize CW linearization time
    chaser->rendezvous_target.cw_linearization_time = sim->time;
}

double calculate_relative_distance(Spacecraft* chaser, Spacecraft* target) {
    Vec3 rel_pos = vec3_sub(target->local_position, chaser->local_position);
    return vec3_magnitude(rel_pos);
}

double calculate_relative_velocity_magnitude(Spacecraft* chaser, Spacecraft* target) {
    Vec3 rel_vel = vec3_sub(target->local_velocity, chaser->local_velocity);
    return vec3_magnitude(rel_vel);
}

// ============================================================================
// Test Cases
// ============================================================================

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

    SimulationState* sim = create_simulation(2, 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 == 2);
    REQUIRE(std::string(sim->spacecraft[0].name) == "Target_Satellite");
    REQUIRE(std::string(sim->spacecraft[1].name) == "Chaser_Satellite");

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

    // Verify initial orbits
    REQUIRE_THAT(sim->spacecraft[0].orbit.semi_major_axis,
                 Catch::Matchers::WithinAbs(6.771e6, 1.0));
    REQUIRE_THAT(sim->spacecraft[1].orbit.semi_major_axis,
                 Catch::Matchers::WithinAbs(6.821e6, 1.0));

    REQUIRE(sim->spacecraft[0].orbit.eccentricity == 0.0);
    REQUIRE(sim->spacecraft[1].orbit.eccentricity == 0.0);

    destroy_simulation(sim);
}

SCENARIO("CW validity check for close spacecraft", "[rendezvous][cw][validity]") {
    const double TIME_STEP = 30.0;

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

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

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

    // Initialize orbital positions
    initialize_orbital_objects(sim);

    Vec3 initial_chaser_pos = chaser->local_position;
    Vec3 initial_target_pos = target->local_position;

    SECTION("Valid when within 5% of orbital radius") {
        // Initial separation is small (different semi-major axes)
        CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time);

        INFO("Spatial fraction: " << validity.spatial_fraction);
        INFO("n*dt: " << validity.n_dt);
        INFO("Overall valid: " << validity.overall_valid);

        REQUIRE(validity.spatial_fraction < CW_SPATIAL_TOLERANCE * 10);  // Should be well within 5%
        REQUIRE(validity.overall_valid == true);
    }

    SECTION("Invalid when CW linearization is too old") {
        // Artificially set old linearization time
        double old_time = sim->time - 2000.0;  // 2000 seconds ago
        chaser->rendezvous_target.cw_linearization_time = old_time;

        CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time);

        INFO("Time since linearization: " << (sim->time - chaser->rendezvous_target.cw_linearization_time));
        INFO("n*dt: " << validity.n_dt);
        INFO("Overall valid: " << validity.overall_valid);

        // Should be invalid due to time limit (n*dt > 2.0)
        REQUIRE(validity.time_valid == false);
        REQUIRE(validity.overall_valid == false);
    }

    SECTION("Spatial fraction scales with orbital radius") {
        double orbital_radius = vec3_magnitude(chaser->local_position);
        double separation = calculate_relative_distance(chaser, target);
        double expected_fraction = separation / orbital_radius;

        INFO("Orbital radius: " << orbital_radius);
        INFO("Separation: " << separation);
        INFO("Expected fraction: " << expected_fraction);
        INFO("CW limit: " << CW_SPATIAL_LIMIT_FRACTION);

        REQUIRE(expected_fraction < CW_SPATIAL_LIMIT_FRACTION);
    }

    destroy_simulation(sim);
}

SCENARIO("CW guidance calculation for rendezvous", "[rendezvous][cw][guidance]") {
    const double TIME_STEP = 30.0;

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

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

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

    initialize_orbital_objects(sim);

    SECTION("Calculate guidance for 1-orbit intercept") {
        double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass));
        double intercept_time = orbital_period;  // 1 orbit

        CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time);

        INFO("Intercept time: " << intercept_time << " s");
        INFO("Solution valid: " << solution.valid);
        INFO("Delta-v magnitude: " << solution.delta_v_magnitude << " m/s");
        INFO("Burn direction radial: " << solution.burn_direction_radial);
        INFO("Burn direction along-track: " << solution.burn_direction_along_track);

        REQUIRE(solution.valid == true);
        REQUIRE(solution.delta_v_magnitude > 0.0);
        REQUIRE(solution.delta_v_magnitude < 100.0);  // Should be small for close orbits
    }

    SECTION("Calculate guidance for half-orbit intercept") {
        double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass));
        double intercept_time = orbital_period / 2.0;  // Half orbit

        CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time);

        INFO("Intercept time: " << intercept_time << " s");
        INFO("Delta-v magnitude: " << solution.delta_v_magnitude << " m/s");

        REQUIRE(solution.valid == true);
        REQUIRE(solution.delta_v_magnitude > 0.0);
    }

    SECTION("Optimal intercept time calculation") {
        // Get relative state in LVLH frame
        Vec3 r_hat, v_hat, h_hat;
        cartesian_to_lvlh_basis(chaser->local_position, chaser->local_velocity,
                               earth->mass, &r_hat, &v_hat, &h_hat);

        Vec3 rel_pos = vec3_sub(target->local_position, chaser->local_position);
        Vec3 rel_vel = vec3_sub(target->local_velocity, chaser->local_velocity);

        LVLHRelativeState lvlh;
        project_to_lvlh_frame(rel_pos, r_hat, v_hat, h_hat, rel_vel, &lvlh);

        double mean_motion = compute_mean_motion(earth->mass,
                                                 vec3_magnitude(chaser->local_position));

        double optimal_time = calculate_optimal_intercept_time(&lvlh, mean_motion);

        INFO("LVLH radial: " << lvlh.radial);
        INFO("LVLH along-track: " << lvlh.along_track);
        INFO("Mean motion: " << mean_motion);
        INFO("Optimal intercept time: " << optimal_time << " s");

        // Should be around quarter to half orbit
        double orbital_period = 2.0 * M_PI / mean_motion;
        REQUIRE(optimal_time > orbital_period * 0.2);
        REQUIRE(optimal_time < orbital_period * 0.6);
    }

    destroy_simulation(sim);
}

SCENARIO("Rendezvous execution with CW guidance", "[rendezvous][execution]") {
    const double TIME_STEP = 10.0;

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

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

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

    initialize_orbital_objects(sim);

    // Store initial positions
    Vec3 initial_chaser_pos = chaser->local_position;
    Vec3 initial_target_pos = target->local_position;

    SECTION("Execute single CW burn and verify encounter") {
        // Initialize rendezvous
        initialize_rendezvous_for_spacecraft(
            sim, "Chaser_Satellite", "Target_Satellite",
            5000.0,   // approach_distance: 5 km
            100.0,    // capture_distance: 100 m
            0.5       // max_relative_velocity: 0.5 m/s
        );

        double initial_distance = calculate_relative_distance(chaser, target);
        INFO("Initial distance: " << initial_distance << " m");

        // Calculate and execute CW guidance burn
        double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass));
        CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim->time);

        INFO("Calculated delta-v: " << solution.delta_v_magnitude << " m/s");

        apply_cw_guidance_burn(chaser, &solution, earth, sim->time);

        // Propagate for one orbital period
        double propagation_time = orbital_period;
        int num_steps = (int)(propagation_time / TIME_STEP);

        for (int i = 0; i < num_steps; i++) {
            update_spacecraft_physics(sim);
            compute_spacecraft_globals(sim);
            sim->time += TIME_STEP;
        }

        double final_distance = calculate_relative_distance(chaser, target);
        double final_rel_vel = calculate_relative_velocity_magnitude(chaser, target);

        INFO("Final distance: " << final_distance << " m");
        INFO("Final relative velocity: " << final_rel_vel << " m/s");
        INFO("Distance reduction: " << (initial_distance - final_distance) << " m");

        // Verify rendezvous success (within 100 m)
        REQUIRE(final_distance < POSITION_TOLERANCE);
        REQUIRE(final_rel_vel < VELOCITY_TOLERANCE);
    }

    SECTION("Update rendezvous state machine") {
        initialize_rendezvous_for_spacecraft(
            sim, "Chaser_Satellite", "Target_Satellite",
            5000.0, 100.0, 0.5
        );

        // Initially should be in PLANNING state
        REQUIRE(sim->spacecraft[1].rendezvous_target.state == RENDEZVOUS_PLANNING);

        // Execute burn to get into approach phase
        double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass));
        CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim->time);
        apply_cw_guidance_burn(chaser, &solution, earth, sim->time);

        // Propagate for half an orbit
        int num_steps = (int)(orbital_period / 2.0 / TIME_STEP);
        for (int i = 0; i < num_steps; i++) {
            update_spacecraft_physics(sim);
            compute_spacecraft_globals(sim);
            sim->time += TIME_STEP;
        }

        // Update state machine
        update_rendezvous_state(chaser, &chaser->rendezvous_target, earth, sim->time, target);

        INFO("Final rendezvous state: " << sim->spacecraft[1].rendezvous_target.state);

        // Should have progressed to APPROACHING or MATCHING
        REQUIRE(sim->spacecraft[1].rendezvous_target.state != RENDEZVOUS_NONE);
        REQUIRE(sim->spacecraft[1].rendezvous_target.state != RENDEZVOUS_FAILED);
    }

    destroy_simulation(sim);
}

SCENARIO("Rendezvous with different initial separations", "[rendezvous][separation]") {
    const double TIME_STEP = 30.0;

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

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

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

    initialize_orbital_objects(sim);

    SECTION("Small separation (1 km along-track)") {
        // Manually adjust chaser to be 1 km behind target
        Vec3 r_hat = vec3_normalize(chaser->local_position);
        Vec3 v_hat = vec3_normalize(chaser->local_velocity);

        Vec3 desired_pos = vec3_sub(chaser->local_position, vec3_scale(v_hat, 1000.0));
        chaser->local_position = desired_pos;

        // Reconstruct orbital elements
        chaser->orbit = cartesian_to_orbital_elements(chaser->local_position,
                                                      chaser->local_velocity,
                                                      earth->mass);

        initialize_rendezvous_for_spacecraft(
            sim, "Chaser_Satellite", "Target_Satellite",
            5000.0, 100.0, 0.5
        );

        double initial_distance = calculate_relative_distance(chaser, target);
        INFO("Initial distance: " << initial_distance << " m");

        REQUIRE(initial_distance < 10000.0);  // Should be ~1 km

        // Execute rendezvous
        double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass));
        CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim->time);
        apply_cw_guidance_burn(chaser, &solution, earth, sim->time);

        // Propagate
        int num_steps = (int)(orbital_period / TIME_STEP);
        for (int i = 0; i < num_steps; i++) {
            update_spacecraft_physics(sim);
            compute_spacecraft_globals(sim);
            sim->time += TIME_STEP;
        }

        double final_distance = calculate_relative_distance(chaser, target);
        REQUIRE(final_distance < POSITION_TOLERANCE);
    }

    SECTION("Medium separation (10 km radial)") {
        // Manually adjust chaser to be 10 km above target
        Vec3 r_hat = vec3_normalize(chaser->local_position);

        Vec3 desired_pos = vec3_add(chaser->local_position, vec3_scale(r_hat, 10000.0));
        chaser->local_position = desired_pos;

        chaser->orbit = cartesian_to_orbital_elements(chaser->local_position,
                                                      chaser->local_velocity,
                                                      earth->mass);

        initialize_rendezvous_for_spacecraft(
            sim, "Chaser_Satellite", "Target_Satellite",
            50000.0, 100.0, 0.5
        );

        double initial_distance = calculate_relative_distance(chaser, target);
        INFO("Initial distance: " << initial_distance << " m");

        REQUIRE(initial_distance < 20000.0);  // Should be ~10 km

        // Check CW validity (should still be valid at 10 km)
        CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time);
        INFO("Spatial fraction: " << validity.spatial_fraction);

        REQUIRE(validity.overall_valid == true);

        // Execute rendezvous
        double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass));
        CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim->time);
        apply_cw_guidance_burn(chaser, &solution, earth, sim->time);

        // Propagate
        int num_steps = (int)(orbital_period / TIME_STEP);
        for (int i = 0; i < num_steps; i++) {
            update_spacecraft_physics(sim);
            compute_spacecraft_globals(sim);
            sim->time += TIME_STEP;
        }

        double final_distance = calculate_relative_distance(chaser, target);
        INFO("Final distance: " << final_distance << " m");

        REQUIRE(final_distance < POSITION_TOLERANCE);
    }

    destroy_simulation(sim);
}

SCENARIO("Rendezvous with CW linearization updates", "[rendezvous][linearization]") {
    const double TIME_STEP = 30.0;

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

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

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

    initialize_orbital_objects(sim);

    SECTION("CW validity maintained with periodic linearization") {
        initialize_rendezvous_for_spacecraft(
            sim, "Chaser_Satellite", "Target_Satellite",
            5000.0, 100.0, 0.5
        );

        // Execute burn
        double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass));
        CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim->time);
        apply_cw_guidance_burn(chaser, &solution, earth, sim->time);

        // Propagate for 2 orbits with periodic linearization updates
        int num_steps = (int)(2.0 * orbital_period / TIME_STEP);
        double update_interval = 500.0;  // Update every 500 seconds
        double last_update_time = sim->time;

        for (int i = 0; i < num_steps; i++) {
            // Update CW linearization time periodically
            if (sim->time - last_update_time >= update_interval) {
                chaser->rendezvous_target.cw_linearization_time = sim->time;
                last_update_time = sim->time;
            }

            update_spacecraft_physics(sim);
            compute_spacecraft_globals(sim);
            sim->time += TIME_STEP;
        }

        double final_distance = calculate_relative_distance(chaser, target);
        INFO("Final distance: " << final_distance << " m");

        REQUIRE(final_distance < POSITION_TOLERANCE);
    }

    SECTION("CW validity lost without updates") {
        // Don't update linearization time
        initialize_rendezvous_for_spacecraft(
            sim, "Chaser_Satellite", "Target_Satellite",
            5000.0, 100.0, 0.5
        );

        // Artificially delay linearization
        chaser->rendezvous_target.cw_linearization_time = sim->time - 3000.0;  // 3000 seconds ago

        CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time);
        INFO("n*dt: " << validity.n_dt);
        INFO("Overall valid: " << validity.overall_valid);

        // Should be invalid due to old linearization
        REQUIRE(validity.overall_valid == false);
        REQUIRE(validity.time_valid == false);
    }

    destroy_simulation(sim);
}