/*
 * test_od_math.c -- Standalone unit test for od_math element converters
 *
 * No PostgreSQL dependency. Exercises:
 *   - ECI ↔ Keplerian round-trip
 *   - Keplerian ↔ equinoctial round-trip
 *   - Brouwer ↔ Kozai inverse
 *   - ECEF ↔ TEME inverse
 *   - Topocentric ↔ ECEF inverse
 *
 * Build:  cc -Wall -Werror -Isrc -o test/test_od_math \
 *             test/test_od_math.c src/od_math.c -lm
 * Run:    ./test/test_od_math
 */

#include "od_math.h"

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/* ── Test harness ───────────────────────────────────────── */

static int n_run, n_pass;

#define RUN(cond, msg) do { \
    n_run++; \
    if (!(cond)) \
        fprintf(stderr, "FAIL: %s  [line %d]\n", (msg), __LINE__); \
    else { n_pass++; fprintf(stderr, "  ok: %s\n", (msg)); } \
} while (0)

#define CLOSE(a, b, tol, msg) do { \
    n_run++; \
    double _a = (a), _b = (b); \
    if (fabs(_a - _b) > (tol)) \
        fprintf(stderr, "FAIL: %s: %.15g vs %.15g (diff %.3e)  [line %d]\n", \
                (msg), _a, _b, fabs(_a - _b), __LINE__); \
    else { n_pass++; fprintf(stderr, "  ok: %s\n", (msg)); } \
} while (0)


/* ── Test: ECI ↔ Keplerian round-trip ──────────────────── */

static void
test_eci_keplerian_roundtrip(void)
{
    fprintf(stderr, "\n--- ECI <-> Keplerian round-trip ---\n");

    /* ISS-like LEO orbit */
    {
        double pos[3] = {-2500.0, 5500.0, 3500.0};  /* km */
        double vel[3] = {-5.5, -3.5, 2.5};           /* km/s */
        od_keplerian_t kep;
        double pos2[3], vel2[3];
        int rc;

        rc = od_eci_to_keplerian(pos, vel, &kep);
        RUN(rc == 0, "ISS-like ECI->Kep conversion succeeds");
        RUN(kep.ecc >= 0.0 && kep.ecc < 1.0, "ISS-like eccentricity valid");
        RUN(kep.n > 0.0, "ISS-like mean motion positive");

        od_keplerian_to_eci(&kep, pos2, vel2);
        CLOSE(pos2[0], pos[0], 1e-6, "ISS-like pos X round-trip");
        CLOSE(pos2[1], pos[1], 1e-6, "ISS-like pos Y round-trip");
        CLOSE(pos2[2], pos[2], 1e-6, "ISS-like pos Z round-trip");
        CLOSE(vel2[0], vel[0], 1e-9, "ISS-like vel X round-trip");
        CLOSE(vel2[1], vel[1], 1e-9, "ISS-like vel Y round-trip");
        CLOSE(vel2[2], vel[2], 1e-9, "ISS-like vel Z round-trip");
    }

    /* Near-circular orbit (e ≈ 0) */
    {
        /* Compute a nearly circular orbit state vector */
        double r = 7000.0;  /* km */
        double v = sqrt(398600.8 / r);  /* circular velocity */
        double pos[3] = {r, 0.0, 0.0};
        double vel[3] = {0.0, v * cos(0.4), v * sin(0.4)};  /* i ≈ 23 deg */
        od_keplerian_t kep;
        double pos2[3], vel2[3];

        od_eci_to_keplerian(pos, vel, &kep);
        RUN(kep.ecc < 0.01, "near-circular eccentricity < 0.01");

        od_keplerian_to_eci(&kep, pos2, vel2);
        CLOSE(pos2[0], pos[0], 1e-5, "near-circular pos X round-trip");
        CLOSE(pos2[1], pos[1], 1e-5, "near-circular pos Y round-trip");
        CLOSE(pos2[2], pos[2], 1e-5, "near-circular pos Z round-trip");
    }

    /* High-eccentricity orbit (e ≈ 0.7, Molniya-like) */
    {
        double pos[3] = {10000.0, 0.0, 0.0};
        double vel[3] = {0.0, 9.0, 2.0};  /* high velocity at perigee */
        od_keplerian_t kep;
        double pos2[3], vel2[3];

        od_eci_to_keplerian(pos, vel, &kep);
        RUN(kep.ecc > 0.3, "high-ecc eccentricity > 0.3");

        od_keplerian_to_eci(&kep, pos2, vel2);
        CLOSE(pos2[0], pos[0], 1e-5, "high-ecc pos X round-trip");
        CLOSE(pos2[1], pos[1], 1e-5, "high-ecc pos Y round-trip");
        CLOSE(pos2[2], pos[2], 1e-5, "high-ecc pos Z round-trip");
        CLOSE(vel2[0], vel[0], 1e-8, "high-ecc vel X round-trip");
        CLOSE(vel2[1], vel[1], 1e-8, "high-ecc vel Y round-trip");
        CLOSE(vel2[2], vel[2], 1e-8, "high-ecc vel Z round-trip");
    }

    /* GEO orbit */
    {
        double r = 42164.0;  /* km, GEO radius */
        double v = sqrt(398600.8 / r);
        double pos[3] = {r, 0.0, 0.0};
        double vel[3] = {0.0, v, 0.0};
        od_keplerian_t kep;
        double pos2[3], vel2[3];

        od_eci_to_keplerian(pos, vel, &kep);
        RUN(kep.ecc < 0.001, "GEO eccentricity near zero");

        od_keplerian_to_eci(&kep, pos2, vel2);
        CLOSE(pos2[0], pos[0], 1e-4, "GEO pos X round-trip");
        CLOSE(pos2[1], pos[1], 1e-4, "GEO pos Y round-trip");
        CLOSE(vel2[0], vel[0], 1e-8, "GEO vel X round-trip");
        CLOSE(vel2[1], vel[1], 1e-8, "GEO vel Y round-trip");
    }
}


/* ── Test: Keplerian ↔ equinoctial round-trip ──────────── */

static void
test_keplerian_equinoctial_roundtrip(void)
{
    fprintf(stderr, "\n--- Keplerian <-> equinoctial round-trip ---\n");

    /* Standard LEO */
    {
        od_keplerian_t kep = {
            .n = 0.06535, .ecc = 0.001, .inc = 0.9,
            .raan = 1.5, .argp = 2.0, .M = 0.5, .bstar = 0.0001
        };
        od_equinoctial_t eq;
        od_keplerian_t kep2;

        od_keplerian_to_equinoctial(&kep, &eq);
        od_equinoctial_to_keplerian(&eq, &kep2);

        CLOSE(kep2.n, kep.n, 1e-14, "LEO n round-trip");
        CLOSE(kep2.ecc, kep.ecc, 1e-14, "LEO ecc round-trip");
        CLOSE(kep2.inc, kep.inc, 1e-14, "LEO inc round-trip");
        CLOSE(kep2.raan, kep.raan, 1e-10, "LEO raan round-trip");
        CLOSE(kep2.argp, kep.argp, 1e-10, "LEO argp round-trip");
        CLOSE(kep2.M, kep.M, 1e-10, "LEO M round-trip");
        CLOSE(kep2.bstar, kep.bstar, 1e-14, "LEO bstar round-trip");
    }

    /* Near-equatorial (i ≈ 0.001 rad) */
    {
        od_keplerian_t kep = {
            .n = 0.003, .ecc = 0.01, .inc = 0.001,
            .raan = 0.5, .argp = 1.0, .M = 3.0, .bstar = 0.0
        };
        od_equinoctial_t eq;
        od_keplerian_t kep2;

        od_keplerian_to_equinoctial(&kep, &eq);
        RUN(fabs(eq.chi) < 0.01, "near-equatorial chi small");
        RUN(fabs(eq.psi) < 0.01, "near-equatorial psi small");

        od_equinoctial_to_keplerian(&eq, &kep2);
        CLOSE(kep2.ecc, kep.ecc, 1e-12, "near-equatorial ecc round-trip");
        CLOSE(kep2.inc, kep.inc, 1e-12, "near-equatorial inc round-trip");
    }

    /* Near-circular (e ≈ 1e-6) */
    {
        od_keplerian_t kep = {
            .n = 0.06535, .ecc = 1e-6, .inc = 0.9,
            .raan = 1.5, .argp = 0.0, .M = 2.0, .bstar = 0.0
        };
        od_equinoctial_t eq;
        od_keplerian_t kep2;

        od_keplerian_to_equinoctial(&kep, &eq);
        RUN(fabs(eq.af) < 1e-4, "near-circular af small");
        RUN(fabs(eq.ag) < 1e-4, "near-circular ag small");

        od_equinoctial_to_keplerian(&eq, &kep2);
        CLOSE(kep2.ecc, kep.ecc, 1e-10, "near-circular ecc round-trip");
    }
}


/* ── Test: Brouwer ↔ Kozai ─────────────────────────────── */

static void
test_brouwer_kozai_inverse(void)
{
    fprintf(stderr, "\n--- Brouwer <-> Kozai inverse ---\n");

    /* ISS-like: n ≈ 0.06535 rad/min, e = 0.0007, i = 51.6 deg */
    {
        double n_kozai = 0.06535;
        double ecc = 0.0007;
        double inc = 51.6 * M_PI / 180.0;

        double n_brouwer = od_kozai_to_brouwer_n(n_kozai, ecc, inc);
        double n_kozai_recovered = od_brouwer_to_kozai_n(n_brouwer, ecc, inc);

        CLOSE(n_kozai_recovered, n_kozai, 1e-14, "ISS Kozai round-trip");
    }

    /* GEO: n ≈ 0.00437 rad/min */
    {
        double n_kozai = 0.004375;
        double ecc = 0.0002;
        double inc = 0.05 * M_PI / 180.0;

        double n_brouwer = od_kozai_to_brouwer_n(n_kozai, ecc, inc);
        double n_kozai_recovered = od_brouwer_to_kozai_n(n_brouwer, ecc, inc);

        CLOSE(n_kozai_recovered, n_kozai, 1e-14, "GEO Kozai round-trip");
    }

    /* High-ecc: e = 0.7 (Molniya) */
    {
        double n_kozai = 0.00898;
        double ecc = 0.7;
        double inc = 63.4 * M_PI / 180.0;

        double n_brouwer = od_kozai_to_brouwer_n(n_kozai, ecc, inc);
        double n_kozai_recovered = od_brouwer_to_kozai_n(n_brouwer, ecc, inc);

        CLOSE(n_kozai_recovered, n_kozai, 1e-13, "Molniya Kozai round-trip");
    }

    /* Critical inclination (63.4 deg) with moderate ecc */
    {
        double n_kozai = 0.04;
        double ecc = 0.15;
        double inc = 63.4 * M_PI / 180.0;

        double n_brouwer = od_kozai_to_brouwer_n(n_kozai, ecc, inc);
        double n_kozai_recovered = od_brouwer_to_kozai_n(n_brouwer, ecc, inc);

        CLOSE(n_kozai_recovered, n_kozai, 1e-14, "critical-inc Kozai round-trip");
    }
}


/* ── Test: ECEF ↔ TEME ─────────────────────────────────── */

static void
test_ecef_teme_inverse(void)
{
    fprintf(stderr, "\n--- ECEF <-> TEME inverse ---\n");

    double gmst = 1.23456;  /* arbitrary GMST in radians */

    /* Position-only test */
    {
        double pos_teme[3] = {-2500.0, 5500.0, 3500.0};
        double pos_ecef[3], pos_teme2[3];
        double cos_g = cos(gmst), sin_g = sin(gmst);

        /* Forward: TEME → ECEF (replicating coord_funcs.c logic) */
        pos_ecef[0] =  cos_g * pos_teme[0] + sin_g * pos_teme[1];
        pos_ecef[1] = -sin_g * pos_teme[0] + cos_g * pos_teme[1];
        pos_ecef[2] = pos_teme[2];

        /* Inverse: ECEF → TEME */
        od_ecef_to_teme(pos_ecef, NULL, gmst, pos_teme2, NULL);

        CLOSE(pos_teme2[0], pos_teme[0], 1e-9, "ECEF->TEME pos X");
        CLOSE(pos_teme2[1], pos_teme[1], 1e-9, "ECEF->TEME pos Y");
        CLOSE(pos_teme2[2], pos_teme[2], 1e-9, "ECEF->TEME pos Z");
    }

    /* Position + velocity test */
    {
        double pos_teme[3] = {-2500.0, 5500.0, 3500.0};
        double vel_teme[3] = {-5.5, -3.5, 2.5};
        double pos_ecef[3], vel_ecef[3];
        double pos_teme2[3], vel_teme2[3];
        double cos_g = cos(gmst), sin_g = sin(gmst);
        double omega = 7.2921158553e-5;

        /* Forward: TEME → ECEF */
        pos_ecef[0] =  cos_g * pos_teme[0] + sin_g * pos_teme[1];
        pos_ecef[1] = -sin_g * pos_teme[0] + cos_g * pos_teme[1];
        pos_ecef[2] = pos_teme[2];

        vel_ecef[0] =  cos_g * vel_teme[0] + sin_g * vel_teme[1]
                     + omega * pos_ecef[1];
        vel_ecef[1] = -sin_g * vel_teme[0] + cos_g * vel_teme[1]
                     - omega * pos_ecef[0];
        vel_ecef[2] = vel_teme[2];

        /* Inverse */
        od_ecef_to_teme(pos_ecef, vel_ecef, gmst, pos_teme2, vel_teme2);

        CLOSE(pos_teme2[0], pos_teme[0], 1e-9, "ECEF->TEME pos+vel X");
        CLOSE(pos_teme2[1], pos_teme[1], 1e-9, "ECEF->TEME pos+vel Y");
        CLOSE(pos_teme2[2], pos_teme[2], 1e-9, "ECEF->TEME pos+vel Z");
        CLOSE(vel_teme2[0], vel_teme[0], 1e-9, "ECEF->TEME vel X");
        CLOSE(vel_teme2[1], vel_teme[1], 1e-9, "ECEF->TEME vel Y");
        CLOSE(vel_teme2[2], vel_teme[2], 1e-9, "ECEF->TEME vel Z");
    }
}


/* ── Test: topocentric ↔ ECEF ──────────────────────────── */

static void
test_topocentric_ecef_inverse(void)
{
    fprintf(stderr, "\n--- Topocentric <-> ECEF inverse ---\n");

    /* Observer at MIT (lat=42.36, lon=-71.09, alt=20m) */
    double obs_lat = 42.36 * M_PI / 180.0;
    double obs_lon = -71.09 * M_PI / 180.0;
    double obs_alt_m = 20.0;
    double obs_ecef[3];

    od_observer_to_ecef(obs_lat, obs_lon, obs_alt_m, obs_ecef);

    /* Test several satellite positions */
    struct {
        double sat_ecef[3];
        const char *name;
    } cases[] = {
        {{-2500.0, 5500.0, 3500.0}, "overhead pass"},
        {{6000.0, -2000.0, 4000.0}, "northeast horizon"},
        {{-5000.0, -3000.0, 1000.0}, "low elevation"},
    };

    for (int i = 0; i < 3; i++)
    {
        double *sat = cases[i].sat_ecef;
        char msg[100];

        /* Forward: ECEF → topocentric (replicate coord_funcs.c logic) */
        double dx = sat[0] - obs_ecef[0];
        double dy = sat[1] - obs_ecef[1];
        double dz = sat[2] - obs_ecef[2];

        double sin_lat = sin(obs_lat), cos_lat = cos(obs_lat);
        double sin_lon = sin(obs_lon), cos_lon = cos(obs_lon);

        double south = sin_lat*cos_lon*dx + sin_lat*sin_lon*dy - cos_lat*dz;
        double east  = -sin_lon*dx + cos_lon*dy;
        double up    = cos_lat*cos_lon*dx + cos_lat*sin_lon*dy + sin_lat*dz;

        double range_km = sqrt(dx*dx + dy*dy + dz*dz);
        double el = asin(up / range_km);
        double az = atan2(east, -south);
        if (az < 0.0) az += 2.0 * M_PI;

        /* Inverse: topocentric → ECEF */
        double sat_recovered[3];
        od_topocentric_to_ecef(az, el, range_km, obs_ecef,
                               obs_lat, obs_lon, sat_recovered);

        snprintf(msg, sizeof(msg), "%s pos X", cases[i].name);
        CLOSE(sat_recovered[0], sat[0], 1e-6, msg);
        snprintf(msg, sizeof(msg), "%s pos Y", cases[i].name);
        CLOSE(sat_recovered[1], sat[1], 1e-6, msg);
        snprintf(msg, sizeof(msg), "%s pos Z", cases[i].name);
        CLOSE(sat_recovered[2], sat[2], 1e-6, msg);
    }
}


/* ── Test: full pipeline ECI → topo → ECEF → TEME ─────── */

static void
test_full_inverse_pipeline(void)
{
    fprintf(stderr, "\n--- Full inverse pipeline ---\n");

    double jd = 2460000.5;  /* arbitrary epoch */
    double gmst = od_gmst_from_jd(jd);

    /* Start with TEME state */
    double pos_teme[3] = {-4000.0, 4000.0, 3000.0};
    double vel_teme[3] = {-4.0, -5.0, 3.0};

    /* Forward: TEME → ECEF */
    double cos_g = cos(gmst), sin_g = sin(gmst);
    double pos_ecef[3], vel_ecef[3];
    double omega = 7.2921158553e-5;

    pos_ecef[0] =  cos_g * pos_teme[0] + sin_g * pos_teme[1];
    pos_ecef[1] = -sin_g * pos_teme[0] + cos_g * pos_teme[1];
    pos_ecef[2] = pos_teme[2];

    vel_ecef[0] =  cos_g * vel_teme[0] + sin_g * vel_teme[1] + omega * pos_ecef[1];
    vel_ecef[1] = -sin_g * vel_teme[0] + cos_g * vel_teme[1] - omega * pos_ecef[0];
    vel_ecef[2] = vel_teme[2];

    /* ECEF → topocentric (observer at origin-ish, 0 lat 0 lon) */
    double obs_lat = 0.0, obs_lon = 0.0;
    double obs_ecef[3];
    od_observer_to_ecef(obs_lat, obs_lon, 0.0, obs_ecef);

    double dx = pos_ecef[0] - obs_ecef[0];
    double dy = pos_ecef[1] - obs_ecef[1];
    double dz = pos_ecef[2] - obs_ecef[2];

    double sin_lat = sin(obs_lat), cos_lat = cos(obs_lat);
    double sin_lon = sin(obs_lon), cos_lon = cos(obs_lon);

    double south = sin_lat*cos_lon*dx + sin_lat*sin_lon*dy - cos_lat*dz;
    double east  = -sin_lon*dx + cos_lon*dy;
    double up    = cos_lat*cos_lon*dx + cos_lat*sin_lon*dy + sin_lat*dz;

    double range_km = sqrt(dx*dx + dy*dy + dz*dz);
    double el = asin(up / range_km);
    double az = atan2(east, -south);
    if (az < 0.0) az += 2.0 * M_PI;

    /* Inverse pipeline: topo → ECEF → TEME */
    double sat_ecef_inv[3];
    od_topocentric_to_ecef(az, el, range_km, obs_ecef,
                           obs_lat, obs_lon, sat_ecef_inv);

    double pos_teme_inv[3];
    od_ecef_to_teme(sat_ecef_inv, NULL, gmst, pos_teme_inv, NULL);

    CLOSE(pos_teme_inv[0], pos_teme[0], 1e-6, "pipeline TEME X recovery");
    CLOSE(pos_teme_inv[1], pos_teme[1], 1e-6, "pipeline TEME Y recovery");
    CLOSE(pos_teme_inv[2], pos_teme[2], 1e-6, "pipeline TEME Z recovery");

    /* Also verify velocity through full inverse */
    double pos_teme_inv2[3], vel_teme_inv[3];
    od_ecef_to_teme(pos_ecef, vel_ecef, gmst, pos_teme_inv2, vel_teme_inv);

    CLOSE(vel_teme_inv[0], vel_teme[0], 1e-9, "pipeline vel X recovery");
    CLOSE(vel_teme_inv[1], vel_teme[1], 1e-9, "pipeline vel Y recovery");
    CLOSE(vel_teme_inv[2], vel_teme[2], 1e-9, "pipeline vel Z recovery");
}


/* ── Main ──────────────────────────────────────────────── */

int
main(void)
{
    fprintf(stderr, "pg_orrery OD math unit tests\n");
    fprintf(stderr, "============================\n");

    test_eci_keplerian_roundtrip();
    test_keplerian_equinoctial_roundtrip();
    test_brouwer_kozai_inverse();
    test_ecef_teme_inverse();
    test_topocentric_ecef_inverse();
    test_full_inverse_pipeline();

    fprintf(stderr, "\n%d/%d tests passed.\n", n_pass, n_run);

    return (n_pass == n_run) ? 0 : 1;
}