/*
 * test_od_iod.c -- Standalone unit tests for Gibbs IOD
 *
 * No PostgreSQL dependency. Exercises:
 *   - ISS-like 3-position Gibbs recovery
 *   - GEO-like orbit with wide time separation
 *   - Coplanar failure detection
 *   - Near-circular orbit
 *
 * Build:  cc -Wall -Werror -Isrc -o test/test_od_iod \
 *             test/test_od_iod.c src/od_iod.c src/od_math.c -lm
 * Run:    ./test/test_od_iod
 */

#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)


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

static void
test_gibbs_iss(void)
{
    /* ISS-like orbit: 408 km altitude, 51.6 deg inclination.
     * Generate 3 positions from known Keplerian elements. */
    od_keplerian_t kep;
    double pos1[3], vel1[3], pos2[3], vel2[3], pos3[3], vel3[3];
    od_iod_result_t result;
    int rc;
    double n_rad_s, period_s, dt;

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

    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;

    /* Period in seconds, time step = period/6 */
    n_rad_s = kep.n / 60.0;
    period_s = 2.0 * M_PI / n_rad_s;
    dt = period_s / 6.0;  /* ~15 min */

    od_keplerian_to_eci(&kep, pos1, vel1);

    /* Advance M by dt */
    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos2, vel2);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos3, vel3);

    /* Julian dates (arbitrary epoch + dt in days) */
    rc = od_gibbs(pos1, pos2, pos3,
                  2451545.0,
                  2451545.0 + dt / 86400.0,
                  2451545.0 + 2.0 * dt / 86400.0,
                  &result);

    RUN(rc == 0, "Gibbs ISS returns success");
    RUN(result.valid == 1, "result is valid");
    CLOSE(result.kep.ecc, 0.0007, 0.01, "eccentricity recovered");
    CLOSE(result.kep.inc, 0.9012, 0.02, "inclination recovered");
    RUN(result.kep.n > 0.0, "positive mean motion");
    CLOSE(result.kep.n, 0.001127, 0.0001, "mean motion recovered");
}


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

static void
test_gibbs_geo(void)
{
    od_keplerian_t kep;
    double pos1[3], vel1[3], pos2[3], vel2[3], pos3[3], vel3[3];
    od_iod_result_t result;
    int rc;
    double dt;

    fprintf(stderr, "\n--- Gibbs: GEO-like orbit ---\n");

    kep.n    = 0.0000729;  /* ~1 rev/day */
    kep.ecc  = 0.0001;
    kep.inc  = 0.001;      /* near-equatorial */
    kep.raan = 0.0;
    kep.argp = 0.0;
    kep.M    = 0.0;
    kep.bstar = 0.0;

    /* 2 hours between observations */
    dt = 7200.0;

    od_keplerian_to_eci(&kep, pos1, vel1);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos2, vel2);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos3, vel3);

    rc = od_gibbs(pos1, pos2, pos3,
                  2451545.0,
                  2451545.0 + dt / 86400.0,
                  2451545.0 + 2.0 * dt / 86400.0,
                  &result);

    RUN(rc == 0, "Gibbs GEO returns success");
    RUN(result.valid == 1, "result is valid");
    RUN(result.kep.ecc < 0.01, "near-circular eccentricity");
    CLOSE(result.kep.n, 0.0000729, 0.00001, "GEO mean motion recovered");
}


/* ── Test: coplanar failure ────────────────────────────── */

static void
test_gibbs_coplanar_fail(void)
{
    /* Three vectors that are NOT coplanar (large out-of-plane angle) */
    double pos1[3] = {6778.0, 0.0, 0.0};
    double pos2[3] = {0.0, 6778.0, 0.0};
    double pos3[3] = {0.0, 0.0, 6778.0};
    od_iod_result_t result;
    int rc;

    fprintf(stderr, "\n--- Gibbs: coplanarity failure ---\n");

    rc = od_gibbs(pos1, pos2, pos3,
                  2451545.0, 2451545.01, 2451545.02,
                  &result);

    RUN(rc != 0, "non-coplanar vectors rejected");
    RUN(result.valid == 0, "result marked invalid");
}


/* ── Test: near-circular orbit ─────────────────────────── */

static void
test_gibbs_circular(void)
{
    od_keplerian_t kep;
    double pos1[3], vel1[3], pos2[3], vel2[3], pos3[3], vel3[3];
    od_iod_result_t result;
    int rc;
    double dt;

    fprintf(stderr, "\n--- Gibbs: near-circular orbit ---\n");

    kep.n    = 0.001127;
    kep.ecc  = 0.00001;    /* nearly perfect circle */
    kep.inc  = 0.5;
    kep.raan = 1.0;
    kep.argp = 0.0;
    kep.M    = 0.0;
    kep.bstar = 0.0;

    dt = 900.0;  /* 15 min */

    od_keplerian_to_eci(&kep, pos1, vel1);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos2, vel2);

    kep.M = od_normalize_angle(kep.M + kep.n * (dt / 60.0));
    od_keplerian_to_eci(&kep, pos3, vel3);

    rc = od_gibbs(pos1, pos2, pos3,
                  2451545.0,
                  2451545.0 + dt / 86400.0,
                  2451545.0 + 2.0 * dt / 86400.0,
                  &result);

    RUN(rc == 0, "Gibbs circular returns success");
    RUN(result.kep.ecc < 0.001, "eccentricity near zero");
    CLOSE(result.kep.n, 0.001127, 0.0001, "mean motion recovered");
}


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

    test_gibbs_iss();
    test_gibbs_geo();
    test_gibbs_coplanar_fail();
    test_gibbs_circular();

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