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

TEST_CASE("Spacecraft loading from config", "[spacecraft][config]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 0, TIME_STEP);

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

    REQUIRE(sim->craft_count == 1);
    REQUIRE(std::string(sim->spacecraft[0].name) == "LEO_Satellite");
    REQUIRE(sim->spacecraft[0].parent_index == 1);

    destroy_simulation(sim);
}

TEST_CASE("Prograde burn increases orbital energy", "[spacecraft][burn][prograde]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 0, TIME_STEP);

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

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

    double initial_distance = vec3_distance(craft->global_position, earth->global_position);
    double initial_velocity = vec3_magnitude(craft->local_velocity);

    apply_impulsive_burn(craft, BURN_PROGRADE, 100.0);

    REQUIRE(vec3_magnitude(craft->local_velocity) > initial_velocity);

    const double SECONDS_TO_SIMULATE = 3600.0;
    double sim_time = 0.0;
    while (sim_time < SECONDS_TO_SIMULATE) {
        update_simulation(sim);
        sim_time += TIME_STEP;
    }

    double final_distance = vec3_distance(craft->global_position, earth->global_position);
    REQUIRE(final_distance > initial_distance);

    destroy_simulation(sim);
}

TEST_CASE("Retrograde burn decreases orbital energy", "[spacecraft][burn][retrograde]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 0, TIME_STEP);

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

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

    double initial_distance = vec3_distance(craft->global_position, earth->global_position);
    double initial_velocity = vec3_magnitude(craft->local_velocity);

    apply_impulsive_burn(craft, BURN_RETROGRADE, 100.0);

    REQUIRE(vec3_magnitude(craft->local_velocity) < initial_velocity);

    const double SECONDS_TO_SIMULATE = 3600.0;
    double sim_time = 0.0;
    while (sim_time < SECONDS_TO_SIMULATE) {
        update_simulation(sim);
        sim_time += TIME_STEP;
    }

    double final_distance = vec3_distance(craft->global_position, earth->global_position);
    REQUIRE(final_distance < initial_distance);

    destroy_simulation(sim);
}

TEST_CASE("Normal burn changes orbital plane", "[spacecraft][burn][normal]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 0, TIME_STEP);

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

    Spacecraft* craft = &sim->spacecraft[0];

    double initial_z = craft->local_position.z;

    apply_impulsive_burn(craft, BURN_NORMAL, 500.0);

    const double SECONDS_TO_SIMULATE = 3600.0;
    double sim_time = 0.0;
    while (sim_time < SECONDS_TO_SIMULATE) {
        update_simulation(sim);
        sim_time += TIME_STEP;
    }

    REQUIRE(fabs(craft->local_position.z - initial_z) > 1000.0);

    destroy_simulation(sim);
}

TEST_CASE("Custom burn applies arbitrary delta-v", "[spacecraft][burn][custom]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 0, TIME_STEP);

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

    Spacecraft* craft = &sim->spacecraft[0];

    Vec3 initial_vel = craft->local_velocity;
    Vec3 delta_v = {10.0, 20.0, 30.0};

    apply_custom_burn(craft, delta_v);

    REQUIRE(fabs(craft->local_velocity.x - initial_vel.x - 10.0) < 0.001);
    REQUIRE(fabs(craft->local_velocity.y - initial_vel.y - 20.0) < 0.001);
    REQUIRE(fabs(craft->local_velocity.z - initial_vel.z - 30.0) < 0.001);

    destroy_simulation(sim);
}

TEST_CASE("Spacecraft propagation maintains stability", "[spacecraft][propagation]") {
    const double TIME_STEP = 60.0;
    const double DAYS_TO_SIMULATE = 1.0;
    const double SECONDS_PER_DAY = 86400.0;

    SimulationState* sim = create_simulation(10, 10, 0, TIME_STEP);

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

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

    double initial_distance = vec3_distance(craft->global_position, earth->global_position);

    double total_time = DAYS_TO_SIMULATE * SECONDS_PER_DAY;
    double sim_time = 0.0;
    while (sim_time < total_time) {
        update_simulation(sim);
        sim_time += TIME_STEP;
    }

    double final_distance = vec3_distance(craft->global_position, earth->global_position);
    double distance_drift_percent = fabs((final_distance - initial_distance) / initial_distance) * 100.0;

    INFO("Initial distance: " << initial_distance << " m");
    INFO("Final distance: " << final_distance << " m");
    INFO("Distance drift: " << distance_drift_percent << "%");

    REQUIRE(distance_drift_percent < 1.0);

    destroy_simulation(sim);
}

TEST_CASE("Spacecraft state vectors at orbital quarters", "[spacecraft][state_vectors]") {
    const double TIME_STEP = 60.0;

    SimulationState* sim = create_simulation(10, 10, 0, TIME_STEP);

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

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

    double orbit_radius = vec3_magnitude(craft->local_position);
    double earth_mass = earth->mass;

    double orbital_period = 2.0 * M_PI * sqrt(pow(orbit_radius, 3.0) / (G * earth_mass));
    double quarter_orbit_time = orbital_period / 4.0;
    int steps_per_quarter = (int)(quarter_orbit_time / TIME_STEP);

    INFO("Orbital radius: " << orbit_radius << " m");
    INFO("Expected orbital period: " << orbital_period << " s (" << orbital_period / 3600.0 << " hours)");
    INFO("Steps per quarter: " << steps_per_quarter);

    double previous_angle = atan2(craft->local_position.y, craft->local_position.x);

    for (int quarter = 0; quarter <= 4; quarter++) {
        INFO("");
        INFO("=== Point " << quarter << "/4 (" << (quarter * 90) << "°) ===");
        INFO("Local position: (" << craft->local_position.x << ", " << craft->local_position.y << ", " << craft->local_position.z << ") m");
        INFO("Local velocity: (" << craft->local_velocity.x << ", " << craft->local_velocity.y << ", " << craft->local_velocity.z << ") m/s");

        double current_radius = vec3_magnitude(craft->local_position);
        double current_velocity = vec3_magnitude(craft->local_velocity);
        double current_angle = atan2(craft->local_position.y, craft->local_position.x);

        INFO("Radius: " << current_radius << " m");
        INFO("Velocity magnitude: " << current_velocity << " m/s");
        INFO("Angular position: " << current_angle << " rad (" << (current_angle * 180.0 / M_PI) << "°)");

        if (quarter > 0) {
            double angle_change = current_angle - previous_angle;
            if (angle_change < 0) angle_change += 2.0 * M_PI;
            INFO("Angle change from previous: " << angle_change << " rad (" << (angle_change * 180.0 / M_PI) << "°)");
            REQUIRE(fabs(angle_change - M_PI / 2.0) < 0.1);
        }

        if (quarter < 4) {
            for (int step = 0; step < steps_per_quarter; step++) {
                update_simulation(sim);
            }
        }

        previous_angle = current_angle;
    }

    INFO("");
    INFO("=== Final Summary ===");
    double final_radius = vec3_magnitude(craft->local_position);
    double final_velocity = vec3_magnitude(craft->local_velocity);
    double final_angle = atan2(craft->local_position.y, craft->local_position.x);
    double total_rotation = final_angle;

    if (total_rotation < 0) total_rotation += 2.0 * M_PI;

    INFO("Total rotation: " << total_rotation << " rad (" << (total_rotation * 180.0 / M_PI) << "°)");
    INFO("Radius change: " << ((final_radius - orbit_radius) / orbit_radius * 100.0) << "%");
    INFO("Velocity change: " << ((final_velocity - vec3_magnitude(craft->local_velocity)) / vec3_magnitude(craft->local_velocity) * 100.0) << "%");

    REQUIRE(fabs(total_rotation - 2.0 * M_PI) < 0.1);

    destroy_simulation(sim);
}