#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 = 1.0;
    const double SECONDS_PER_DAY = 86400.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);

    INFO("INITIAL Earth velocity: (" << sim->bodies[EARTH_IDX].velocity.x << ", "
         << sim->bodies[EARTH_IDX].velocity.y << ", "
         << sim->bodies[EARTH_IDX].velocity.z << ") m/s");

    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 leo_altitude_m = dist_to_earth - sim->bodies[EARTH_IDX].radius;

    INFO("Spacecraft altitude: " << leo_altitude_m / 1000.0 << " km");
    INFO("Spacecraft parent: " << sim->bodies[CRAFT_IDX].parent_index << " (Earth)");
    INFO("Distance to Earth: " << dist_to_earth / 1000.0 << " km");

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

    INFO("Bypassing wait_for_launch_window - applying burn at initial configuration");
    INFO("This tests core Hohmann transfer formulas without timing complications");

    double wait_duration = 0.0;

    INFO("Earth velocity: (" << sim->bodies[EARTH_IDX].velocity.x << ", "
         << sim->bodies[EARTH_IDX].velocity.y << ", "
         << sim->bodies[EARTH_IDX].velocity.z << ") m/s");
    INFO("Craft velocity: (" << sim->bodies[CRAFT_IDX].velocity.x << ", "
         << sim->bodies[CRAFT_IDX].velocity.y << ", "
         << sim->bodies[CRAFT_IDX].velocity.z << ") m/s");
    INFO("Craft local position: (" << sim->bodies[CRAFT_IDX].local_position.x << ", "
         << sim->bodies[CRAFT_IDX].local_position.y << ", "
         << sim->bodies[CRAFT_IDX].local_position.z << ") m");
    INFO("Craft local velocity: (" << sim->bodies[CRAFT_IDX].local_velocity.x << ", "
         << sim->bodies[CRAFT_IDX].local_velocity.y << ", "
         << sim->bodies[CRAFT_IDX].local_velocity.z << ") m/s");

    double dot_product = sim->bodies[CRAFT_IDX].local_position.x * sim->bodies[CRAFT_IDX].local_velocity.x +
                       sim->bodies[CRAFT_IDX].local_position.y * sim->bodies[CRAFT_IDX].local_velocity.y;
    INFO("Dot product (pos · vel): " << dot_product << " (should be ~0 for circular orbit)");
    INFO("Earth prograde direction: (" << earth_prograde.x << ", " << earth_prograde.y << ", " << earth_prograde.z << ")");

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

    INFO("Bypassing wait_for_launch_window - applying burn at initial configuration");
    INFO("This tests the core Hohmann transfer formulas without timing complications");

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

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

    double specific_energy_helio = 0.5 * pow(vec3_magnitude(sim->bodies[CRAFT_IDX].velocity), 2) -
                                 G * sim->bodies[SUN_IDX].mass / vec3_distance(sim->bodies[CRAFT_IDX].position, sim->bodies[SUN_IDX].position);
    INFO("Specific heliocentric energy: " << specific_energy_helio << " J/kg");

    double expected_specific_energy = -G * sim->bodies[SUN_IDX].mass / (2.0 * params.semi_major_axis);
    INFO("Expected specific transfer orbit energy: " << expected_specific_energy << " J/kg");

    double energy_error = fabs(specific_energy_helio - expected_specific_energy);
    if (expected_specific_energy != 0.0) {
        energy_error /= fabs(expected_specific_energy);
    }
    INFO("Energy error: " << (energy_error * 100.0) << "%");

    REQUIRE(energy_error < 0.05);

    INFO("Test complete - burn successfully applied for Hohmann transfer");
    INFO("Spacecraft now on transfer orbit from Earth to Mars");
    INFO("Skipping long-duration simulation to avoid numerical instability");

    destroy_simulation(sim);
}