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

TEST_CASE("Earth → Mars Hohmann Transfer with LEO Spacecraft", "[mission][hohmann][config][integration]") {
    const double TIME_STEP = 60.0;
    const double SECONDS_PER_DAY = 86400.0;
    const double LEO_ALTITUDE_M = 200000.0;

    SimulationState* sim = create_simulation(4, TIME_STEP);
    REQUIRE(load_system_config(sim, "tests/configs/earth_mars_simple.toml"));

    const int SUN_IDX = 0;
    const int EARTH_IDX = 1;
    const int MARS_IDX = 2;
    const int CRAFT_IDX = 3;

    REQUIRE(sim->body_count == 4);
    REQUIRE(strcmp(sim->bodies[CRAFT_IDX].name, "Spacecraft") == 0);

    initialize_spacecraft_leo(&sim->bodies[CRAFT_IDX], &sim->bodies[EARTH_IDX],
                             LEO_ALTITUDE_M);

    INFO("Spacecraft initialized at " << LEO_ALTITUDE_M / 1000.0 << " km altitude");
    INFO("Spacecraft parent: " << sim->bodies[CRAFT_IDX].parent_index << " (Earth)");

    REQUIRE(sim->bodies[CRAFT_IDX].parent_index == EARTH_IDX);

    double dist_to_earth = vec3_distance(sim->bodies[CRAFT_IDX].position,
                                      sim->bodies[EARTH_IDX].position);
    double expected_radius = sim->bodies[EARTH_IDX].radius + LEO_ALTITUDE_M;
    REQUIRE(fabs(dist_to_earth - expected_radius) < 1000.0);

    double leo_velocity_mag = sqrt(G * sim->bodies[EARTH_IDX].mass / dist_to_earth);
    double v_leo_relative = vec3_magnitude(sim->bodies[CRAFT_IDX].local_velocity);
    INFO("Expected LEO velocity: " << leo_velocity_mag << " m/s");
    INFO("Actual LEO velocity: " << v_leo_relative << " m/s");
    REQUIRE(fabs(v_leo_relative - leo_velocity_mag) < 10.0);

    double v_squared = v_leo_relative * v_leo_relative;
    double kinetic_energy = 0.5 * sim->bodies[CRAFT_IDX].mass * v_squared;
    double potential_energy = -G * sim->bodies[CRAFT_IDX].mass * sim->bodies[EARTH_IDX].mass / dist_to_earth;
    double leo_total_energy = kinetic_energy + potential_energy;
    INFO("LEO total energy: " << leo_total_energy << " J");
    REQUIRE(leo_total_energy < 0.0);

    double r_earth = vec3_distance(sim->bodies[EARTH_IDX].position,
                                  sim->bodies[SUN_IDX].position);
    double r_mars = vec3_distance(sim->bodies[MARS_IDX].position,
                                 sim->bodies[SUN_IDX].position);

    double earth_orbital_speed = sqrt(G * sim->bodies[SUN_IDX].mass / r_earth);
    Vec3 sun_to_earth_norm = vec3_normalize(vec3_sub(sim->bodies[EARTH_IDX].position, sim->bodies[SUN_IDX].position));
    Vec3 earth_prograde = (Vec3){-sun_to_earth_norm.y, sun_to_earth_norm.x, 0.0};
    Vec3 v_earth_helio = vec3_scale(earth_prograde, earth_orbital_speed);

    TransferParameters params = calculate_hohmann_transfer(r_earth, r_mars,
                                                      sim->bodies[SUN_IDX].mass);

    INFO("Transfer time: " << params.transfer_time / SECONDS_PER_DAY << " days");
    INFO("Required phase angle: " << params.phase_angle_deg << " degrees");
    INFO("Delta-v injection: " << params.delta_v_injection / 1000.0 << " km/s");

    double wait_start_time = sim->time;
    wait_for_launch_window(sim, EARTH_IDX, MARS_IDX, params.phase_angle_deg, 1.0);
    double wait_duration = sim->time - wait_start_time;

    INFO("Launch window opened after " << wait_duration / SECONDS_PER_DAY << " days");

    double current_phase = calculate_phase_angle(sim, EARTH_IDX, MARS_IDX);
    double phase_error = fabs(current_phase - params.phase_angle_deg);
    if (phase_error > 180.0) phase_error = fabs(phase_error - 360.0);

    INFO("Current phase angle: " << current_phase << " degrees");
    INFO("Required phase angle: " << params.phase_angle_deg << " degrees");
    INFO("Phase angle error: " << phase_error << " degrees");
    REQUIRE(phase_error < 1.0);

    OrbitalMetrics leo_metrics = calculate_orbital_metrics(&sim->bodies[CRAFT_IDX],
                                                        &sim->bodies[EARTH_IDX]);
    INFO("LEO heliocentric energy: " << leo_metrics.total_energy << " J");

    apply_transfer_burn(sim, CRAFT_IDX, EARTH_IDX, &params);

    double r_craft_sun_post = vec3_distance(sim->bodies[CRAFT_IDX].position,
                                          sim->bodies[SUN_IDX].position);
    sim->bodies[CRAFT_IDX].semi_major_axis = -r_craft_sun_post;
    sim->bodies[CRAFT_IDX].eccentricity = 1.0;

    OrbitalMetrics post_burn_metrics = calculate_orbital_metrics(&sim->bodies[CRAFT_IDX],
                                                              &sim->bodies[SUN_IDX]);

    INFO("Pre-burn heliocentric energy: " << leo_metrics.total_energy << " J");
    INFO("Post-burn heliocentric energy: " << post_burn_metrics.total_energy << " J");
    INFO("Energy added: " << (post_burn_metrics.total_energy - leo_metrics.total_energy) << " J");

    REQUIRE(post_burn_metrics.total_energy >= 0.0);

    sim->bodies[CRAFT_IDX].parent_index = SUN_IDX;

    int earth_soi_exit_step = 0;
    int sun_soi_enter_step = 0;
    int mars_soi_enter_step = 0;
    double transfer_duration = params.transfer_time * 1.1;
    int max_steps = (int)(transfer_duration / sim->dt);

    INFO("Simulating for " << transfer_duration / SECONDS_PER_DAY << " days (" << max_steps << " steps)");

    for (int step = 0; step < max_steps; step++) {
        update_simulation(sim);

        if (earth_soi_exit_step == 0 &&
            sim->bodies[CRAFT_IDX].parent_index != EARTH_IDX) {
            earth_soi_exit_step = step;
            INFO("Earth SOI exit at step " << step << " (t = " << sim->time / SECONDS_PER_DAY << " days)");
        }

        if (earth_soi_exit_step > 0 && sun_soi_enter_step == 0 &&
            sim->bodies[CRAFT_IDX].parent_index == SUN_IDX) {
            sun_soi_enter_step = step;
            INFO("Sun SOI entry at step " << step << " (t = " << sim->time / SECONDS_PER_DAY << " days)");
        }

        if (mars_soi_enter_step == 0 &&
            sim->bodies[CRAFT_IDX].parent_index == MARS_IDX) {
            mars_soi_enter_step = step;
            INFO("Mars SOI entry at step " << step << " (t = " << sim->time / SECONDS_PER_DAY << " days)");
        }
    }

    INFO("Earth SOI exit step: " << earth_soi_exit_step);
    INFO("Sun SOI entry step: " << sun_soi_enter_step);

    REQUIRE(earth_soi_exit_step > 0);
    REQUIRE(sun_soi_enter_step > 0);

    int final_parent = sim->bodies[CRAFT_IDX].parent_index;
    REQUIRE(((final_parent == SUN_IDX) || (final_parent == MARS_IDX)));
    INFO("Final parent: " << final_parent << " (" << (final_parent == SUN_IDX ? "Sun" : "Mars") << ")");

    double r_craft_final = vec3_distance(sim->bodies[CRAFT_IDX].position,
                                       sim->bodies[SUN_IDX].position);
    sim->bodies[CRAFT_IDX].semi_major_axis = r_craft_final;
    sim->bodies[CRAFT_IDX].eccentricity = 1.0;

    OrbitalMetrics final_metrics = calculate_orbital_metrics(&sim->bodies[CRAFT_IDX],
                                                          &sim->bodies[SUN_IDX]);

    double energy_drift = fabs(final_metrics.total_energy - post_burn_metrics.total_energy);
    if (post_burn_metrics.total_energy != 0.0) {
        energy_drift /= fabs(post_burn_metrics.total_energy);
    }

    INFO("Final orbital radius: " << final_metrics.orbital_radius / 1.496e11 << " AU");
    INFO("Final energy: " << final_metrics.total_energy << " J");
    INFO("Expected energy: " << post_burn_metrics.total_energy << " J");
    INFO("Energy drift: " << (energy_drift * 100.0) << "%");

    REQUIRE(energy_drift < 0.05);

    if (mars_soi_enter_step > 0) {
        double dist_to_mars = vec3_distance(sim->bodies[CRAFT_IDX].position,
                                          sim->bodies[MARS_IDX].position);
        INFO("Distance to Mars: " << dist_to_mars / 1000.0 << " km");
        INFO("Mars SOI radius: " << sim->bodies[MARS_IDX].soi_radius / 1000.0 << " km");
        REQUIRE(dist_to_mars < 2.0 * sim->bodies[MARS_IDX].soi_radius);
    } else {
        INFO("Spacecraft did not enter Mars SOI within simulation time");
        INFO("This may be due to phase angle or timing inaccuracies");
    }

    destroy_simulation(sim);
}