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

TEST_CASE("Parabolic orbit - energy and escape trajectory", "[parabolic][energy][escape]") {
    const double TIME_STEP = 60.0;
    const double DAYS_TO_SIMULATE = 300.0;
    const double SECONDS_PER_DAY = 86400.0;
    const double AU = 1.496e11;

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

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

    const int COMET_INDEX = 1;
    const int SUN_INDEX = 0;

    Vec3 initial_position = sim->bodies[COMET_INDEX].global_position;
    double initial_distance = vec3_magnitude(initial_position);
    double initial_velocity = vec3_magnitude(sim->bodies[COMET_INDEX].global_velocity);

    double initial_kinetic = calculate_kinetic_energy(&sim->bodies[COMET_INDEX]);
    double initial_potential = calculate_potential_energy_pair(&sim->bodies[COMET_INDEX],
                                                                &sim->bodies[SUN_INDEX]);
    double initial_total_energy = initial_kinetic + initial_potential;

    INFO("Initial distance: " << initial_distance / AU << " AU");
    INFO("Initial velocity: " << vec3_magnitude(sim->bodies[COMET_INDEX].global_velocity) / 1000.0 << " km/s");
    INFO("Initial kinetic energy: " << initial_kinetic);
    INFO("Initial potential energy: " << initial_potential);
    INFO("Initial total energy: " << initial_total_energy);

    REQUIRE(initial_total_energy >= -1e25);

    std::vector<double> distances;
    std::vector<double> velocities;
    std::vector<double> energies;

    double max_time = DAYS_TO_SIMULATE * SECONDS_PER_DAY;
    int step_count = 0;
    while (sim->time < max_time) {
        if (step_count % 1000 == 0) {
            double current_distance = vec3_magnitude(sim->bodies[COMET_INDEX].global_position);
            double current_velocity = vec3_magnitude(sim->bodies[COMET_INDEX].global_velocity);
            double current_kinetic = calculate_kinetic_energy(&sim->bodies[COMET_INDEX]);
            double current_potential = calculate_potential_energy_pair(&sim->bodies[COMET_INDEX],
                                                                       &sim->bodies[SUN_INDEX]);
            double current_total = current_kinetic + current_potential;

            distances.push_back(current_distance);
            velocities.push_back(current_velocity);
            energies.push_back(current_total);
        }

        update_simulation(sim);
        step_count++;
    }

    double final_distance = vec3_magnitude(sim->bodies[COMET_INDEX].global_position);
    double final_velocity = vec3_magnitude(sim->bodies[COMET_INDEX].global_velocity);

    double final_kinetic = calculate_kinetic_energy(&sim->bodies[COMET_INDEX]);
    double final_potential = calculate_potential_energy_pair(&sim->bodies[COMET_INDEX],
                                                               &sim->bodies[SUN_INDEX]);
    double final_total_energy = final_kinetic + final_potential;

    INFO("Final distance: " << final_distance / AU << " AU");
    INFO("Final velocity: " << final_velocity / 1000.0 << " km/s");
    INFO("Final kinetic energy: " << final_kinetic);
    INFO("Final potential energy: " << final_potential);
    INFO("Final total energy: " << final_total_energy);

    REQUIRE(final_distance > initial_distance);

    REQUIRE(final_velocity < initial_velocity);

    double energy_drift = fabs(final_total_energy - initial_total_energy);
    double avg_kinetic_energy = (initial_kinetic + final_kinetic) / 2.0;
    double energy_drift_percent = (energy_drift / avg_kinetic_energy) * 100.0;

    INFO("Energy drift: " << energy_drift << " J");
    INFO("Energy drift percent: " << energy_drift_percent << "%");

    REQUIRE(energy_drift_percent < 1.0);

    int velocity_decreases = 0;
    for (size_t i = 1; i < velocities.size(); i++) {
        if (velocities[i] < velocities[i-1]) {
            velocity_decreases++;
        }
    }

    INFO("Velocity decreases: " << velocity_decreases << " / " << (velocities.size() - 1));

    REQUIRE(velocity_decreases > static_cast<int>(velocities.size()) / 2);

    destroy_simulation(sim);
}

TEST_CASE("Parabolic orbit initial conditions", "[parabolic][initial]") {
    const double TIME_STEP = 60.0;

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

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

    const int COMET_INDEX = 1;
    const int SUN_INDEX = 0;
    CelestialBody* comet = &sim->bodies[COMET_INDEX];
    CelestialBody* sun = &sim->bodies[SUN_INDEX];

    double distance = vec3_magnitude(vec3_sub(comet->global_position, sun->global_position));
    double velocity = vec3_magnitude(comet->global_velocity);

    double escape_velocity = sqrt(2.0 * G * sun->mass / distance);
    double circular_velocity = sqrt(G * sun->mass / distance);

    INFO("Distance: " << distance / 1.496e11 << " AU");
    INFO("Actual velocity: " << velocity / 1000.0 << " km/s");
    INFO("Escape velocity: " << escape_velocity / 1000.0 << " km/s");
    INFO("Circular velocity: " << circular_velocity / 1000.0 << " km/s");

    double velocity_error = fabs(velocity - escape_velocity) / escape_velocity;
    INFO("Velocity error from escape velocity: " << velocity_error * 100.0 << "%");

    REQUIRE(velocity_error < 0.001);

    INFO("Eccentricity: " << comet->orbit.eccentricity);

    REQUIRE(fabs(comet->orbit.eccentricity - 1.0) < 0.0001);

    destroy_simulation(sim);
}