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

const double POSITION_TOLERANCE = 1.0e6;
const double VELOCITY_TOLERANCE = 10.0;
const double ELEMENT_TOLERANCE = 1.0e-6;

TEST_CASE("Round-trip conversion: orbital elements → state vectors → orbital elements", "[cartesian][elements][roundtrip]") {
    const double TIME_STEP = 60.0;

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

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

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

    OrbitalElements original_elements = craft->orbit;

    Vec3 position_from_elements;
    Vec3 velocity_from_elements;
    orbital_elements_to_cartesian(original_elements, sim->bodies[0].mass, &position_from_elements, &velocity_from_elements);

    INFO("Original orbital elements:");
    INFO("  semi_major_axis: " << original_elements.semi_major_axis << " m");
    INFO("  eccentricity: " << original_elements.eccentricity);
    INFO("  true_anomaly: " << original_elements.true_anomaly << " rad");
    INFO("  inclination: " << original_elements.inclination << " rad");
    INFO("  longitude_of_ascending_node: " << original_elements.longitude_of_ascending_node << " rad");
    INFO("  argument_of_periapsis: " << original_elements.argument_of_periapsis << " rad");

    INFO("State vectors from orbital elements:");
    INFO("  position: (" << position_from_elements.x << ", " << position_from_elements.y << ", " << position_from_elements.z << ") m");
    INFO("  velocity: (" << velocity_from_elements.x << ", " << velocity_from_elements.y << ", " << velocity_from_elements.z << ") m/s");

    OrbitalElements converted_elements = cartesian_to_orbital_elements(position_from_elements, velocity_from_elements, sim->bodies[0].mass);

    INFO("Converted orbital elements:");
    INFO("  semi_major_axis: " << converted_elements.semi_major_axis << " m");
    INFO("  eccentricity: " << converted_elements.eccentricity);
    INFO("  true_anomaly: " << converted_elements.true_anomaly << " rad");
    INFO("  inclination: " << converted_elements.inclination << " rad");
    INFO("  longitude_of_ascending_node: " << converted_elements.longitude_of_ascending_node << " rad");
    INFO("  argument_of_periapsis: " << converted_elements.argument_of_periapsis << " rad");

    double semi_major_error = fabs(converted_elements.semi_major_axis - original_elements.semi_major_axis);
    double eccentricity_error = fabs(converted_elements.eccentricity - original_elements.eccentricity);
    double inclination_error = fabs(converted_elements.inclination - original_elements.inclination);

    INFO("Semi-major axis error: " << semi_major_error << " m");
    INFO("Eccentricity error: " << eccentricity_error);
    INFO("Inclination error: " << inclination_error << " rad");

    REQUIRE(semi_major_error < fabs(original_elements.semi_major_axis) * ELEMENT_TOLERANCE);
    REQUIRE(eccentricity_error < ELEMENT_TOLERANCE);
    REQUIRE(inclination_error < ELEMENT_TOLERANCE);

    destroy_simulation(sim);
}

TEST_CASE("Position magnitude preservation through conversion", "[cartesian][elements][position]") {
    const double TIME_STEP = 60.0;

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

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

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

    Vec3 position_1;
    Vec3 velocity_1;
    orbital_elements_to_cartesian(craft->orbit, sim->bodies[0].mass, &position_1, &velocity_1);

    double radius_1 = vec3_magnitude(position_1);
    INFO("Original radius: " << radius_1 << " m");

    OrbitalElements elements = cartesian_to_orbital_elements(position_1, velocity_1, sim->bodies[0].mass);

    Vec3 position_2;
    Vec3 velocity_2;
    orbital_elements_to_cartesian(elements, sim->bodies[0].mass, &position_2, &velocity_2);

    double radius_2 = vec3_magnitude(position_2);
    INFO("Reconstructed radius: " << radius_2 << " m");

    double radius_error = fabs(radius_2 - radius_1);
    INFO("Radius error: " << radius_error << " m");

    REQUIRE(radius_error < POSITION_TOLERANCE);

    destroy_simulation(sim);
}

TEST_CASE("Velocity magnitude preservation through conversion", "[cartesian][elements][velocity]") {
    const double TIME_STEP = 60.0;

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

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

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

    Vec3 position_1;
    Vec3 velocity_1;
    orbital_elements_to_cartesian(craft->orbit, sim->bodies[0].mass, &position_1, &velocity_1);

    double v_mag_1 = vec3_magnitude(velocity_1);
    INFO("Original velocity magnitude: " << v_mag_1 << " m/s");

    OrbitalElements elements = cartesian_to_orbital_elements(position_1, velocity_1, sim->bodies[0].mass);

    Vec3 position_2;
    Vec3 velocity_2;
    orbital_elements_to_cartesian(elements, sim->bodies[0].mass, &position_2, &velocity_2);

    double v_mag_2 = vec3_magnitude(velocity_2);
    INFO("Reconstructed velocity magnitude: " << v_mag_2 << " m/s");

    double velocity_error = fabs(v_mag_2 - v_mag_1);
    INFO("Velocity error: " << velocity_error << " m/s");

    REQUIRE(velocity_error < VELOCITY_TOLERANCE);

    destroy_simulation(sim);
}

TEST_CASE("Semi-major axis accuracy", "[cartesian][elements][semi_major]") {
    const double TIME_STEP = 60.0;

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

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

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

    double expected_a = craft->orbit.semi_major_axis;

    Vec3 position;
    Vec3 velocity;
    orbital_elements_to_cartesian(craft->orbit, sim->bodies[0].mass, &position, &velocity);

    OrbitalElements elements = cartesian_to_orbital_elements(position, velocity, sim->bodies[0].mass);

    double actual_a = elements.semi_major_axis;

    double a_error = fabs(actual_a - expected_a);
    double relative_error = a_error / fabs(expected_a);

    INFO("Expected semi-major axis: " << expected_a << " m");
    INFO("Actual semi-major axis: " << actual_a << " m");
    INFO("Absolute error: " << a_error << " m");
    INFO("Relative error: " << relative_error * 100.0 << "%");

    REQUIRE(relative_error < ELEMENT_TOLERANCE);

    destroy_simulation(sim);
}

TEST_CASE("Eccentricity accuracy", "[cartesian][elements][eccentricity]") {
    const double TIME_STEP = 60.0;

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

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

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

    double expected_e = craft->orbit.eccentricity;

    Vec3 position;
    Vec3 velocity;
    orbital_elements_to_cartesian(craft->orbit, sim->bodies[0].mass, &position, &velocity);

    OrbitalElements elements = cartesian_to_orbital_elements(position, velocity, sim->bodies[0].mass);

    double actual_e = elements.eccentricity;

    double e_error = fabs(actual_e - expected_e);

    INFO("Expected eccentricity: " << expected_e);
    INFO("Actual eccentricity: " << actual_e);
    INFO("Absolute error: " << e_error);

    REQUIRE(e_error < ELEMENT_TOLERANCE);

    destroy_simulation(sim);
}