/*
 * test_od_gauss.c -- Standalone unit tests for Gauss angles-only IOD
 *
 * No PostgreSQL dependency. Exercises:
 *   - ISS-like orbit: generate RA/Dec from known state, recover via Gauss
 *   - GEO orbit with wider time spacing
 *   - Degenerate: observations too close in time
 *   - Gauss feeding into Gibbs/Herrick-Gibbs
 *
 * Build:  cc -Wall -Werror -Isrc -o test/test_od_gauss \
 *             test/test_od_gauss.c src/od_iod.c src/od_math.c -lm
 * Run:    ./test/test_od_gauss
 */

#include "od_iod.h"
#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)


/* ── Helper: compute RA/Dec from TEME pos + observer ECEF ── */

/*
 * Simulates an observation by computing RA/Dec of a satellite
 * from a ground observer. Uses od_teme_to_radec().
 */
static void
simulate_radec(const double pos_teme[3], const double obs_ecef[3],
               double jd, double *ra, double *dec)
{
    double gmst = od_gmst_from_jd(jd);
    od_teme_to_radec(pos_teme, obs_ecef, gmst, ra, dec);
}


/* ── Test: ISS-like orbit ──────────────────────────────── */

static void
test_gauss_iss(void)
{
    od_keplerian_t kep;
    double pos[3][3], vel[3][3];
    double ra[3], dec[3], jd[3];
    double obs_ecef[3][3];
    od_iod_result_t result;
    int rc, i;
    double dt;

    fprintf(stderr, "\n--- Gauss: ISS-like orbit ---\n");

    /* Known ISS-like orbit */
    kep.n    = 0.001127;   /* ~15.5 rev/day in rad/min */
    kep.ecc  = 0.0007;
    kep.inc  = 0.9012;     /* ~51.6 deg */
    kep.raan = 3.0;
    kep.argp = 0.5;
    kep.M    = 0.0;
    kep.bstar = 0.0;

    /* 30 minutes between observations — wider arc improves Gauss conditioning */
    dt = 1800.0;
    jd[0] = 2451545.0;
    jd[1] = jd[0] + dt / 86400.0;
    jd[2] = jd[0] + 2.0 * dt / 86400.0;

    /* Generate 3 positions */
    od_keplerian_to_eci(&kep, pos[0], vel[0]);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos[1], vel[1]);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos[2], vel[2]);

    /* Observer at lat=40N, lon=0, alt=0 — compute ECEF once for all obs */
    od_observer_to_ecef(40.0 * M_PI / 180.0, 0.0, 0.0, obs_ecef[0]);
    obs_ecef[1][0] = obs_ecef[0][0];
    obs_ecef[1][1] = obs_ecef[0][1];
    obs_ecef[1][2] = obs_ecef[0][2];
    obs_ecef[2][0] = obs_ecef[0][0];
    obs_ecef[2][1] = obs_ecef[0][1];
    obs_ecef[2][2] = obs_ecef[0][2];

    /* Compute RA/Dec for each observation */
    for (i = 0; i < 3; i++)
        simulate_radec(pos[i], obs_ecef[i], jd[i], &ra[i], &dec[i]);

    /* Run Gauss */
    rc = od_gauss(ra, dec, jd, obs_ecef, &result);

    RUN(rc == 0, "Gauss ISS returns success");
    RUN(result.valid == 1, "result is valid");
    RUN(result.kep.ecc < 1.0, "eccentricity is bound");
    RUN(result.kep.n > 0.0, "positive mean motion");

    /* Gauss accuracy is inherently low (angles-only loses range info).
     * It only needs to be close enough for the DC solver to converge.
     * A factor of 5x in mean motion is acceptable for a seed orbit. */
    RUN(result.kep.n > 0.0002 && result.kep.n < 0.006,
        "mean motion in plausible range");
    CLOSE(result.kep.inc, 0.9012, 0.3, "inclination within ~17 deg");
}


/* ── Test: MEO-like orbit ──────────────────────────────── */

static void
test_gauss_meo(void)
{
    od_keplerian_t kep;
    double pos[3][3], vel[3][3];
    double ra[3], dec[3], jd[3];
    double obs_ecef[3][3];
    od_iod_result_t result;
    int rc, i;
    double dt;

    fprintf(stderr, "\n--- Gauss: MEO-like orbit ---\n");

    /* GPS-like altitude, moderate inclination */
    kep.n    = 0.000262;   /* ~2 rev/day */
    kep.ecc  = 0.01;
    kep.inc  = 0.96;       /* ~55 deg */
    kep.raan = 1.0;
    kep.argp = 0.0;
    kep.M    = 0.0;
    kep.bstar = 0.0;

    /* 2 hours between observations — wider arc for higher altitude */
    dt = 7200.0;
    jd[0] = 2451545.0;
    jd[1] = jd[0] + dt / 86400.0;
    jd[2] = jd[0] + 2.0 * dt / 86400.0;

    od_keplerian_to_eci(&kep, pos[0], vel[0]);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos[1], vel[1]);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos[2], vel[2]);

    od_observer_to_ecef(35.0 * M_PI / 180.0, -106.0 * M_PI / 180.0,
                        1600.0, obs_ecef[0]);
    for (i = 1; i < 3; i++)
    {
        obs_ecef[i][0] = obs_ecef[0][0];
        obs_ecef[i][1] = obs_ecef[0][1];
        obs_ecef[i][2] = obs_ecef[0][2];
    }

    for (i = 0; i < 3; i++)
        simulate_radec(pos[i], obs_ecef[i], jd[i], &ra[i], &dec[i]);

    rc = od_gauss(ra, dec, jd, obs_ecef, &result);

    RUN(rc == 0, "Gauss MEO returns success");
    RUN(result.valid == 1, "result is valid");
    RUN(result.kep.n > 0.0, "positive mean motion");
    RUN(result.kep.n > 0.00005 && result.kep.n < 0.002,
        "mean motion in plausible MEO range");
}


/* ── Test: too-close observations fail ───────────────────── */

static void
test_gauss_too_close(void)
{
    double ra[3]  = {1.0, 1.001, 1.002};
    double dec[3] = {0.5, 0.501, 0.502};
    double jd[3]  = {2451545.0, 2451545.000005, 2451545.00001};
    double obs_ecef[3][3];
    od_iod_result_t result;
    int rc;

    fprintf(stderr, "\n--- Gauss: too-close observations ---\n");

    od_observer_to_ecef(40.0 * M_PI / 180.0, 0.0, 0.0, obs_ecef[0]);
    obs_ecef[1][0] = obs_ecef[0][0];
    obs_ecef[1][1] = obs_ecef[0][1];
    obs_ecef[1][2] = obs_ecef[0][2];
    obs_ecef[2][0] = obs_ecef[0][0];
    obs_ecef[2][1] = obs_ecef[0][1];
    obs_ecef[2][2] = obs_ecef[0][2];

    rc = od_gauss(ra, dec, jd, obs_ecef, &result);

    RUN(rc != 0, "too-close observations rejected");
    RUN(result.valid == 0, "result marked invalid");
}


/* ── Test: radec_to_los / teme_to_radec round-trip ─────── */

static void
test_radec_roundtrip(void)
{
    double ra_in = 1.5;     /* ~86 degrees */
    double dec_in = 0.3;    /* ~17 degrees */
    double los[3];
    double ra_out, dec_out;
    double rm;

    fprintf(stderr, "\n--- RA/Dec ↔ LOS round-trip ---\n");

    /* RA/Dec → LOS unit vector */
    od_radec_to_los(ra_in, dec_in, los);

    rm = sqrt(los[0]*los[0] + los[1]*los[1] + los[2]*los[2]);
    CLOSE(rm, 1.0, 1e-12, "LOS is unit vector");

    /* LOS → RA/Dec (inverse) */
    dec_out = asin(los[2]);
    ra_out = atan2(los[1], los[0]);
    if (ra_out < 0.0) ra_out += 2.0 * M_PI;

    CLOSE(ra_out, ra_in, 1e-12, "RA round-trip");
    CLOSE(dec_out, dec_in, 1e-12, "Dec round-trip");
}


/* ── Test: teme_to_radec consistency ──────────────────── */

static void
test_teme_to_radec(void)
{
    /* Place a satellite at known TEME position, compute RA/Dec from
     * a ground observer, verify it's in reasonable range */
    double pos_teme[3] = {6778.0, 0.0, 0.0};  /* on X-axis, LEO alt */
    double obs_ecef[3];
    double ra, dec;
    double jd = 2451545.0;

    fprintf(stderr, "\n--- teme_to_radec consistency ---\n");

    od_observer_to_ecef(0.0, 0.0, 0.0, obs_ecef);  /* equator, prime meridian */

    od_teme_to_radec(pos_teme, obs_ecef, od_gmst_from_jd(jd), &ra, &dec);

    RUN(ra >= 0.0 && ra < 2.0 * M_PI, "RA in [0, 2pi)");
    RUN(dec >= -M_PI / 2.0 && dec <= M_PI / 2.0, "Dec in [-pi/2, pi/2]");
}


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

    test_gauss_iss();
    test_gauss_meo();
    test_gauss_too_close();
    test_radec_roundtrip();
    test_teme_to_radec();

    fprintf(stderr, "\n%d/%d tests passed.\n", n_pass, n_run);
    return (n_pass == n_run) ? 0 : 1;
}