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

using Catch::Matchers::WithinAbs;

const double POSITION_TOLERANCE_METERS = 1.0e6;
const double VELOCITY_TOLERANCE_MS = 1.0;
const double ELEMENT_TOLERANCE = 1e-6;
const double ANGULAR_TOLERANCE = 1e-4;

TEST_CASE("Rotation matrix behavior at extreme inclination/eccentricity combinations", "[extreme][orientation][mixed]") {
    const double TIME_STEP = 60.0;
    SimulationState* sim = create_simulation(10, 5, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_extreme_orientation_mixed.toml"));
    CelestialBody* earth = &sim->bodies[0];

    SECTION("Multiple spacecraft with extreme orientation parameters") {
        Spacecraft* high_inc_high_ecc = &sim->spacecraft[0];
        Spacecraft* polar_moderate_ecc = &sim->spacecraft[1];
        Spacecraft* near_parabolic = &sim->spacecraft[2];

        SECTION("Position vectors are correctly oriented in 3D space") {
            Vec3 pos1, vel1;
            orbital_elements_to_cartesian(high_inc_high_ecc->orbit, earth->mass, &pos1, &vel1);

            Vec3 pos2, vel2;
            orbital_elements_to_cartesian(polar_moderate_ecc->orbit, earth->mass, &pos2, &vel2);

            Vec3 pos3, vel3;
            orbital_elements_to_cartesian(near_parabolic->orbit, earth->mass, &pos3, &vel3);

            INFO("Spacecraft 1 (i=1.2 rad, e=0.85): pos=(" << pos1.x << ", " << pos1.y << ", " << pos1.z << ")");
            INFO("Spacecraft 2 (i=1.4 rad, e=0.5): pos=(" << pos2.x << ", " << pos2.y << ", " << pos2.z << ")");
            INFO("Spacecraft 3 (e=0.99, i=0.5 rad): pos=(" << pos3.x << ", " << pos3.y << ", " << pos3.z << ")");

            double r1 = vec3_magnitude(pos1);
            double r2 = vec3_magnitude(pos2);
            double r3 = vec3_magnitude(pos3);

            REQUIRE(r1 > 0.0);
            REQUIRE(r2 > 0.0);
            REQUIRE(r3 > 0.0);

            double expected_r1 = high_inc_high_ecc->orbit.semi_major_axis * (1.0 - high_inc_high_ecc->orbit.eccentricity);
            double expected_r2 = polar_moderate_ecc->orbit.semi_major_axis * (1.0 - polar_moderate_ecc->orbit.eccentricity);
            double expected_r3 = near_parabolic->orbit.semi_major_axis * (1.0 - near_parabolic->orbit.eccentricity);

            REQUIRE_THAT(r1, WithinAbs(expected_r1, POSITION_TOLERANCE_METERS));
            REQUIRE_THAT(r2, WithinAbs(expected_r2, POSITION_TOLERANCE_METERS));
            REQUIRE_THAT(r3, WithinAbs(expected_r3, POSITION_TOLERANCE_METERS));
        }

        SECTION("Velocity vectors are correctly computed") {
            Vec3 pos1, vel1;
            orbital_elements_to_cartesian(high_inc_high_ecc->orbit, earth->mass, &pos1, &vel1);

            Vec3 pos2, vel2;
            orbital_elements_to_cartesian(polar_moderate_ecc->orbit, earth->mass, &pos2, &vel2);

            Vec3 pos3, vel3;
            orbital_elements_to_cartesian(near_parabolic->orbit, earth->mass, &pos3, &vel3);

            double v1 = vec3_magnitude(vel1);
            double v2 = vec3_magnitude(vel2);
            double v3 = vec3_magnitude(vel3);

            INFO("Spacecraft 1: v=" << v1 << " m/s");
            INFO("Spacecraft 2: v=" << v2 << " m/s");
            INFO("Spacecraft 3: v=" << v3 << " m/s");

            double mu = G * earth->mass;

            double expected_v1_sq = mu * (2.0 / vec3_magnitude(pos1) - 1.0 / high_inc_high_ecc->orbit.semi_major_axis);
            double expected_v2_sq = mu * (2.0 / vec3_magnitude(pos2) - 1.0 / polar_moderate_ecc->orbit.semi_major_axis);
            double expected_v3_sq = mu * (2.0 / vec3_magnitude(pos3) - 1.0 / near_parabolic->orbit.semi_major_axis);

            if (expected_v1_sq > 0.0) {
                double expected_v1 = sqrt(expected_v1_sq);
                REQUIRE_THAT(v1, WithinAbs(expected_v1, VELOCITY_TOLERANCE_MS * 100.0));
            }
            if (expected_v2_sq > 0.0) {
                double expected_v2 = sqrt(expected_v2_sq);
                REQUIRE_THAT(v2, WithinAbs(expected_v2, VELOCITY_TOLERANCE_MS * 100.0));
            }
            if (expected_v3_sq > 0.0) {
                double expected_v3 = sqrt(expected_v3_sq);
                REQUIRE_THAT(v3, WithinAbs(expected_v3, VELOCITY_TOLERANCE_MS * 100.0));
            }
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Longitude of ascending node (Ω) singularity handling", "[extreme][orientation][omega]") {
    const double TIME_STEP = 60.0;
    SimulationState* sim = create_simulation(10, 5, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_extreme_orientation_mixed.toml"));
    CelestialBody* earth = &sim->bodies[0];

    SECTION("Spacecraft with Ω = 0 (ascending node at reference direction)") {
        Spacecraft* omega_zero = &sim->spacecraft[3];

        SECTION("Rotation matrices handle Ω = 0 without numerical instability") {
            Vec3 pos, vel;
            orbital_elements_to_cartesian(omega_zero->orbit, earth->mass, &pos, &vel);

            double r = vec3_magnitude(pos);
            double expected_r = omega_zero->orbit.semi_major_axis * (1.0 - omega_zero->orbit.eccentricity);

            INFO("Ω=0: radius=" << r << " m, expected=" << expected_r << " m");
            INFO("Position: (" << pos.x << ", " << pos.y << ", " << pos.z << ")");

            REQUIRE_THAT(r, WithinAbs(expected_r, POSITION_TOLERANCE_METERS));
            REQUIRE(pos.x > 0.0);
        }

        SECTION("Velocity vector orientation is correct") {
            Vec3 pos, vel;
            orbital_elements_to_cartesian(omega_zero->orbit, earth->mass, &pos, &vel);

            Vec3 angular_momentum = vec3_cross(pos, vel);

            INFO("|h| = " << vec3_magnitude(angular_momentum));

            REQUIRE(vec3_magnitude(angular_momentum) > 0.0);
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Argument of periapsis (ω) singularity handling", "[extreme][orientation][arg_peri]") {
    const double TIME_STEP = 60.0;
    SimulationState* sim = create_simulation(10, 5, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_extreme_orientation_mixed.toml"));
    CelestialBody* earth = &sim->bodies[0];

    SECTION("Spacecraft with ω = 0 (periapsis at ascending node)") {
        Spacecraft* arg_peri_zero = &sim->spacecraft[4];

        SECTION("Rotation matrices handle ω = 0 without numerical instability") {
            Vec3 pos, vel;
            orbital_elements_to_cartesian(arg_peri_zero->orbit, earth->mass, &pos, &vel);

            double r = vec3_magnitude(pos);
            double expected_r = arg_peri_zero->orbit.semi_major_axis * (1.0 - arg_peri_zero->orbit.eccentricity);

            INFO("ω=0: radius=" << r << " m, expected=" << expected_r << " m");
            INFO("Position: (" << pos.x << ", " << pos.y << ", " << pos.z << ")");

            REQUIRE_THAT(r, WithinAbs(expected_r, POSITION_TOLERANCE_METERS));
        }

        SECTION("True anomaly references are correct") {
            double r = vec3_magnitude(arg_peri_zero->global_position);
            double expected_r_perigee = arg_peri_zero->orbit.semi_major_axis * (1.0 - arg_peri_zero->orbit.eccentricity);

            INFO("At ν=0 (perigee), r should equal r_perigee");
            INFO("r=" << r << " m, r_perigee=" << expected_r_perigee << " m");

            REQUIRE_THAT(r, WithinAbs(expected_r_perigee, POSITION_TOLERANCE_METERS));
        }

        SECTION("Testing at multiple true anomaly values") {
            double true_anomalies[] = {0.0, M_PI / 2.0, M_PI, 3.0 * M_PI / 2.0};

            for (int i = 0; i < 4; i++) {
                arg_peri_zero->orbit.true_anomaly = true_anomalies[i];

                Vec3 pos, vel;
                orbital_elements_to_cartesian(arg_peri_zero->orbit, earth->mass, &pos, &vel);

                double r = vec3_magnitude(pos);
                double v = vec3_magnitude(vel);

                INFO("ν=" << true_anomalies[i] << " rad: r=" << r << " m, v=" << v << " m/s");

                REQUIRE(r > 0.0);
                REQUIRE(v > 0.0);
            }
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Velocity vector orientation at perigee and apogee for extreme orbits", "[extreme][orientation][velocity]") {
    const double TIME_STEP = 60.0;
    SimulationState* sim = create_simulation(10, 5, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_extreme_orientation_mixed.toml"));
    CelestialBody* earth = &sim->bodies[0];

    for (int craft_idx = 0; craft_idx < sim->craft_count; craft_idx++) {
        Spacecraft* craft = &sim->spacecraft[craft_idx];

        SECTION("Spacecraft " + std::to_string(craft_idx) + ": velocity orientation at apsides") {
            double true_anomalies[] = {0.0, M_PI};

            for (int i = 0; i < 2; i++) {
                craft->orbit.true_anomaly = true_anomalies[i];

                Vec3 pos, vel;
                orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos, &vel);

                double pos_dot_vel = vec3_dot(pos, vel);
                Vec3 angular_momentum = vec3_cross(pos, vel);

                INFO("Spacecraft " << craft_idx << " (e=" << craft->orbit.eccentricity << ", i=" << craft->orbit.inclination << ")");
                INFO("ν=" << true_anomalies[i] << " rad: pos·vel = " << pos_dot_vel);

                REQUIRE_THAT(pos_dot_vel, WithinAbs(0.0, VELOCITY_TOLERANCE_MS * 1000.0));
                REQUIRE(vec3_magnitude(angular_momentum) > 0.0);
            }
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Velocity follows vis-viva equation for extreme orbits", "[extreme][orientation][visviva]") {
    const double TIME_STEP = 60.0;
    SimulationState* sim = create_simulation(10, 5, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_extreme_orientation_mixed.toml"));
    CelestialBody* earth = &sim->bodies[0];

    for (int craft_idx = 0; craft_idx < sim->craft_count; craft_idx++) {
        Spacecraft* craft = &sim->spacecraft[craft_idx];

        SECTION("Spacecraft " + std::to_string(craft_idx) + ": vis-viva at multiple true anomalies") {
            double true_anomalies[] = {0.0, M_PI / 2.0, M_PI, 3.0 * M_PI / 2.0};

            for (int i = 0; i < 4; i++) {
                craft->orbit.true_anomaly = true_anomalies[i];

                Vec3 pos, vel;
                orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos, &vel);

                double r = vec3_magnitude(pos);
                double v = vec3_magnitude(vel);
                double a = craft->orbit.semi_major_axis;
                double mu = G * earth->mass;

                double expected_v_sq = mu * (2.0 / r - 1.0 / a);

                if (expected_v_sq > 0.0) {
                    double expected_v = sqrt(expected_v_sq);
                    double relative_error = fabs(v - expected_v) / expected_v;

                    INFO("ν=" << true_anomalies[i] << " rad: v=" << v << " m/s, expected=" << expected_v << " m/s");
                    INFO("Relative error: " << relative_error * 100.0 << "%");

                    REQUIRE(relative_error < VELOCITY_TOLERANCE_MS * 10.0);
                }
            }
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Round-trip conversion for extreme orientation parameters", "[extreme][orientation][round_trip]") {
    const double TIME_STEP = 60.0;
    SimulationState* sim = create_simulation(10, 5, 0, TIME_STEP);

    REQUIRE(load_system_config(sim, "tests/test_extreme_orientation_mixed.toml"));
    CelestialBody* earth = &sim->bodies[0];

    for (int craft_idx = 0; craft_idx < sim->craft_count; craft_idx++) {
        Spacecraft* craft = &sim->spacecraft[craft_idx];

        SECTION("Spacecraft " + std::to_string(craft_idx) + ": round-trip conversion") {
            Vec3 pos, vel;
            orbital_elements_to_cartesian(craft->orbit, earth->mass, &pos, &vel);

            OrbitalElements recovered = cartesian_to_orbital_elements(pos, vel, earth->mass);

            INFO("Spacecraft " << craft_idx << ": " << craft->name);
            INFO("  Semi-major axis: " << craft->orbit.semi_major_axis << " -> " << recovered.semi_major_axis);
            INFO("  Eccentricity: " << craft->orbit.eccentricity << " -> " << recovered.eccentricity);
            INFO("  Inclination: " << craft->orbit.inclination << " -> " << recovered.inclination);
            INFO("  Ω: " << craft->orbit.longitude_of_ascending_node << " -> " << recovered.longitude_of_ascending_node);
            INFO("  ω: " << craft->orbit.argument_of_periapsis << " -> " << recovered.argument_of_periapsis);

            REQUIRE_THAT(recovered.semi_major_axis, WithinAbs(craft->orbit.semi_major_axis, fabs(craft->orbit.semi_major_axis) * 0.01));
            REQUIRE_THAT(recovered.eccentricity, WithinAbs(craft->orbit.eccentricity, ELEMENT_TOLERANCE));
            REQUIRE_THAT(recovered.inclination, WithinAbs(craft->orbit.inclination, ANGULAR_TOLERANCE));

            if (craft->orbit.longitude_of_ascending_node > 1e-6 || craft->orbit.longitude_of_ascending_node < -1e-6) {
                REQUIRE_THAT(recovered.longitude_of_ascending_node, WithinAbs(craft->orbit.longitude_of_ascending_node, ANGULAR_TOLERANCE * 10.0));
            }

            if (craft->orbit.argument_of_periapsis > 1e-6 || craft->orbit.argument_of_periapsis < -1e-6) {
                REQUIRE_THAT(recovered.argument_of_periapsis, WithinAbs(craft->orbit.argument_of_periapsis, ANGULAR_TOLERANCE * 10.0));
            }

            SECTION("Round-trip preserves position and velocity") {
                Vec3 pos2, vel2;
                orbital_elements_to_cartesian(recovered, earth->mass, &pos2, &vel2);

                double pos_error = vec3_magnitude(vec3_sub(pos, pos2));
                double vel_error = vec3_magnitude(vec3_sub(vel, vel2));

                INFO("Spacecraft " << craft_idx << ": " << craft->name);
                INFO("  Position error: " << pos_error << " m");
                INFO("  Velocity error: " << vel_error << " m/s");

                REQUIRE_THAT(pos_error, WithinAbs(0.0, POSITION_TOLERANCE_METERS));
                REQUIRE_THAT(vel_error, WithinAbs(0.0, VELOCITY_TOLERANCE_MS * 1000.0));
            }
        }
    }

    destroy_simulation(sim);
}

TEST_CASE("Rotation matrix verification for extreme parameters", "[extreme][orientation][matrices]") {
    const double TIME_STEP = 60.0;
    SimulationState* sim = create_simulation(10, 5, 0, TIME_STEP);

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

    for (int craft_idx = 0; craft_idx < sim->craft_count; craft_idx++) {
        Spacecraft* craft = &sim->spacecraft[craft_idx];

        SECTION("Spacecraft " + std::to_string(craft_idx) + ": rotation matrix properties") {
            double omega = craft->orbit.argument_of_periapsis;
            double i = craft->orbit.inclination;
            double Omega = craft->orbit.longitude_of_ascending_node;

            Mat3 R_orbital = mat3_rotation_orbital(omega, i, Omega);

            SECTION("Rotation matrix preserves vector magnitudes (orthogonal)") {
                Vec3 unit_x = {1.0, 0.0, 0.0};
                Vec3 unit_y = {0.0, 1.0, 0.0};
                Vec3 unit_z = {0.0, 0.0, 1.0};

                Vec3 rot_x = mat3_multiply_vec3(R_orbital, unit_x);
                Vec3 rot_y = mat3_multiply_vec3(R_orbital, unit_y);
                Vec3 rot_z = mat3_multiply_vec3(R_orbital, unit_z);

                double mag_x = vec3_magnitude(rot_x);
                double mag_y = vec3_magnitude(rot_y);
                double mag_z = vec3_magnitude(rot_z);

                INFO("Spacecraft " << craft_idx << ": " << craft->name);
                INFO("  |R·x| = " << mag_x << " (expected 1.0)");
                INFO("  |R·y| = " << mag_y << " (expected 1.0)");
                INFO("  |R·z| = " << mag_z << " (expected 1.0)");

                REQUIRE_THAT(mag_x, WithinAbs(1.0, 1e-10));
                REQUIRE_THAT(mag_y, WithinAbs(1.0, 1e-10));
                REQUIRE_THAT(mag_z, WithinAbs(1.0, 1e-10));
            }

            SECTION("Rotated vectors remain orthogonal") {
                Vec3 unit_x = {1.0, 0.0, 0.0};
                Vec3 unit_y = {0.0, 1.0, 0.0};
                Vec3 unit_z = {0.0, 0.0, 1.0};

                Vec3 rot_x = mat3_multiply_vec3(R_orbital, unit_x);
                Vec3 rot_y = mat3_multiply_vec3(R_orbital, unit_y);
                Vec3 rot_z = mat3_multiply_vec3(R_orbital, unit_z);

                double xy_dot = vec3_dot(rot_x, rot_y);
                double yz_dot = vec3_dot(rot_y, rot_z);
                double xz_dot = vec3_dot(rot_x, rot_z);

                INFO("Spacecraft " << craft_idx << ": " << craft->name);
                INFO("  (R·x)·(R·y) = " << xy_dot);
                INFO("  (R·y)·(R·z) = " << yz_dot);
                INFO("  (R·x)·(R·z) = " << xz_dot);

                REQUIRE_THAT(xy_dot, WithinAbs(0.0, 1e-10));
                REQUIRE_THAT(yz_dot, WithinAbs(0.0, 1e-10));
                REQUIRE_THAT(xz_dot, WithinAbs(0.0, 1e-10));
            }
        }
    }

    destroy_simulation(sim);
}