#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>
#include <limits>

const double CONVERGENCE_TOLERANCE = 1.0e-10;
const int MAX_ITERATIONS = 50;

TEST_CASE("Newton-Raphson solver - very low eccentricity (e < 0.01)", "[newton][raphson][low_e]") {
    const double eccentricities[] = {0.001, 0.01};

    for (int i = 0; i < 2; i++) {
        double e = eccentricities[i];
        INFO("Testing eccentricity: " << e);

        double mean_anomaly = M_PI / 2.0;

        double eccentric_anomaly = solve_kepler_elliptical(mean_anomaly, e);

        // Verify Kepler's equation is satisfied: E - e*sin(E) = M
        // Note: E ≈ M only when e=0 exactly. For small e, E ≈ M + e*sin(M)
        double kepler_residual = eccentric_anomaly - e * sin(eccentric_anomaly) - mean_anomaly;
        double first_order_approx = mean_anomaly + e * sin(mean_anomaly);
        double approximation_error = fabs(eccentric_anomaly - first_order_approx);

        INFO("Eccentric anomaly: " << eccentric_anomaly << " rad");
        INFO("Mean anomaly: " << mean_anomaly << " rad");
        INFO("Kepler's equation residual |E - e·sin(E) - M|: " << kepler_residual);
        INFO("First-order approx E ≈ M + e·sin(M): " << first_order_approx << " rad");
        INFO("|E - approx|: " << approximation_error);

        // Verify solver correctly solves Kepler's equation
        REQUIRE(fabs(kepler_residual) < CONVERGENCE_TOLERANCE);
        // Verify result matches first-order approximation (valid for small e)
        REQUIRE(approximation_error < 0.01);
    }
}

TEST_CASE("Newton-Raphson solver - moderate eccentricity (0.1 < e < 0.5)", "[newton][raphson][moderate_e]") {
    const double eccentricities[] = {0.1, 0.3, 0.5};

    for (int i = 0; i < 3; i++) {
        double e = eccentricities[i];
        INFO("Testing eccentricity: " << e);

        double mean_anomaly = M_PI / 4.0;

        double eccentric_anomaly = solve_kepler_elliptical(mean_anomaly, e);

        double rhs = mean_anomaly + e * sin(eccentric_anomaly);
        double residual = eccentric_anomaly - rhs;

        INFO("Eccentric anomaly: " << eccentric_anomaly << " rad");
        INFO("Residual E - (M + e*sin(E)): " << residual);

        REQUIRE(fabs(residual) < CONVERGENCE_TOLERANCE);
    }
}

TEST_CASE("Newton-Raphson solver - high eccentricity (0.9 < e < 0.99)", "[newton][raphson][high_e]") {
    const double eccentricities[] = {0.9, 0.95, 0.99};

    for (int i = 0; i < 3; i++) {
        double e = eccentricities[i];
        INFO("Testing eccentricity: " << e);

        double mean_anomaly = M_PI / 2.0;

        int iterations = 0;
        double E = get_initial_trial_value(mean_anomaly, e);
        double E_prev = E + 2.0 * CONVERGENCE_TOLERANCE;

        while (fabs(E - E_prev) > CONVERGENCE_TOLERANCE && iterations < MAX_ITERATIONS) {
            E_prev = E;
            double sin_E = sin(E);
            E = E - (E - e * sin_E - mean_anomaly) / (1.0 - e * cos(E));
            iterations++;
        }

        INFO("Converged in " << iterations << " iterations");
        INFO("Eccentric anomaly: " << E << " rad");

        double rhs = mean_anomaly + e * sin(E);
        double residual = E - rhs;

        INFO("Residual E - (M + e*sin(E)): " << residual);

        REQUIRE(iterations < MAX_ITERATIONS);
        REQUIRE(fabs(residual) < CONVERGENCE_TOLERANCE);
    }
}

TEST_CASE("Newton-Raphson solver - mean anomaly near π (worst case)", "[newton][raphson][near_pi]") {
    const double eccentricity = 0.7;
    const double mean_anomalies[] = {M_PI - 0.01, M_PI, M_PI + 0.01};

    for (int i = 0; i < 3; i++) {
        double M = mean_anomalies[i];
        INFO("Testing mean anomaly: " << M << " rad (" << (M * 180.0 / M_PI) << "°)");

        int iterations = 0;
        double E = get_initial_trial_value(M, eccentricity);
        double E_prev = E + 2.0 * CONVERGENCE_TOLERANCE;

        while (fabs(E - E_prev) > CONVERGENCE_TOLERANCE && iterations < MAX_ITERATIONS) {
            E_prev = E;
            double sin_E = sin(E);
            E = E - (E - eccentricity * sin_E - M) / (1.0 - eccentricity * cos(E));
            iterations++;
        }

        INFO("Converged in " << iterations << " iterations");

        double rhs = M + eccentricity * sin(E);
        double residual = E - rhs;

        INFO("Residual E - (M + e*sin(E)): " << residual);

        REQUIRE(iterations < MAX_ITERATIONS);
        REQUIRE(fabs(residual) < CONVERGENCE_TOLERANCE);
    }
}

TEST_CASE("Newton-Raphson solver - large mean anomaly values (M > 1000)", "[newton][raphson][large_M]") {
    const double eccentricity = 0.3;
    const double mean_anomalies[] = {1000.0, 10000.0};

    for (int i = 0; i < 2; i++) {
        double M = mean_anomalies[i];
        INFO("Testing mean anomaly: " << M << " rad");

        int iterations = 0;
        double E = get_initial_trial_value(M, eccentricity);
        double E_prev = E + 2.0 * CONVERGENCE_TOLERANCE;

        while (fabs(E - E_prev) > CONVERGENCE_TOLERANCE && iterations < MAX_ITERATIONS) {
            E_prev = E;
            double sin_E = sin(E);
            E = E - (E - eccentricity * sin_E - M) / (1.0 - eccentricity * cos(E));
            iterations++;
        }

        INFO("Converged in " << iterations << " iterations");

        double rhs = M + eccentricity * sin(E);
        double residual = E - rhs;

        INFO("Residual E - (M + e*sin(E)): " << residual);

        REQUIRE(iterations < MAX_ITERATIONS);
        REQUIRE(fabs(residual) < CONVERGENCE_TOLERANCE);

        double M_reduced = fmod(E - eccentricity * sin(E), 2.0 * M_PI);
        double M_target = fmod(M, 2.0 * M_PI);
        double angle_diff = fabs(M_reduced - M_target);

        if (angle_diff > M_PI) {
            angle_diff = 2.0 * M_PI - angle_diff;
        }

        INFO("Reduced mean anomaly: " << M_reduced << " rad");
        INFO("Target reduced: " << M_target << " rad");
        INFO("Angle difference: " << angle_diff << " rad");

        REQUIRE(angle_diff < CONVERGENCE_TOLERANCE * 10.0);
    }
}

TEST_CASE("Newton-Raphson solver - eccentricity at boundaries (e ≈ 1.0)", "[newton][raphson][boundary]") {
    const double eccentricities[] = {0.9999, 1.0001};

    for (int i = 0; i < 2; i++) {
        double e = eccentricities[i];
        INFO("Testing eccentricity: " << e);

        double M = M_PI / 4.0;

        int iterations = 0;
        double E = get_initial_trial_value(M, e);
        double E_prev = E + 2.0 * CONVERGENCE_TOLERANCE;

        while (fabs(E - E_prev) > CONVERGENCE_TOLERANCE && iterations < MAX_ITERATIONS) {
            E_prev = E;
            double sin_E = sin(E);
            E = E - (E - e * sin_E - M) / (1.0 - e * cos(E));
            iterations++;
        }

        INFO("Converged in " << iterations << " iterations");

        if (fabs(1.0 - e * cos(E)) > 1.0e-10) {
            double rhs = M + e * sin(E);
            double residual = E - rhs;

            INFO("Residual E - (M + e*sin(E)): " << residual);

            REQUIRE(fabs(residual) < CONVERGENCE_TOLERANCE);
        }
    }
}

TEST_CASE("Newton-Raphson solver convergence rate", "[newton][raphson][convergence_rate]") {
    const double eccentricity = 0.8;
    const double mean_anomaly = M_PI / 3.0;

    double E = get_initial_trial_value(mean_anomaly, eccentricity);

    INFO("Initial guess: " << E << " rad");

    double previous_residual = std::numeric_limits<double>::max();
    int iteration = 0;
    int convergence_count = 0;

    for (int i = 0; i < 10; i++) {
        double sin_E = sin(E);
        double rhs = mean_anomaly + eccentricity * sin_E;
        double residual = fabs(E - rhs);

        INFO("Iteration " << i << ": E = " << E << ", residual = " << residual);

        if (residual < CONVERGENCE_TOLERANCE) {
            INFO("Converged at iteration " << i);
            break;
        }

        if (i > 0 && residual < previous_residual * 0.5) {
            convergence_count++;
        }

        previous_residual = residual;
        E = E - (E - eccentricity * sin_E - mean_anomaly) / (1.0 - eccentricity * cos(E));
        iteration++;
    }

    double convergence_ratio = (double)convergence_count / (double)iteration;
    INFO("Quadratic convergence ratio: " << convergence_ratio * 100.0 << "%");

    REQUIRE(convergence_ratio > 0.6);
}