/*
 * od_iod.c -- Gibbs method for initial orbit determination
 *
 * Given 3 position vectors, computes the velocity at the middle
 * observation via vector cross-product geometry (Vallado Algorithm 54).
 *
 * The Gibbs method requires 3 coplanar position vectors (which all
 * orbit positions are, modulo perturbations and measurement noise).
 * It avoids iteration entirely -- pure linear algebra.
 *
 * Reference: Vallado, "Fundamentals of Astrodynamics", 4th ed., p.459
 */

#include "od_iod.h"
#include <math.h>

/* WGS-72 gravitational parameter -- must match SGP4 */
#define MU_KM3_S2   398600.8

/* Coplanarity tolerance: cos(angle) between r1 and (r2 x r3) plane.
 * 0.05 corresponds to about 3 degrees, generous for noisy obs. */
#define COPLANAR_TOL  0.05

static double
vec_mag(const double v[3])
{
    return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}

static void
vec_cross(const double a[3], const double b[3], double out[3])
{
    out[0] = a[1]*b[2] - a[2]*b[1];
    out[1] = a[2]*b[0] - a[0]*b[2];
    out[2] = a[0]*b[1] - a[1]*b[0];
}

static double
vec_dot(const double a[3], const double b[3])
{
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

int
od_gibbs(const double pos1[3], const double pos2[3],
         const double pos3[3],
         double jd1, double jd2, double jd3,
         od_iod_result_t *result)
{
    double r1_mag, r2_mag, r3_mag;
    double z12[3], z23[3], z31[3];
    double z23_mag;
    double coplanar;
    double D[3], N[3], S[3];
    double D_mag, N_mag;
    double v2[3];
    double v2_coeff;
    double D_cross_r2[3];
    int i, rc;

    result->valid = 0;

    r1_mag = vec_mag(pos1);
    r2_mag = vec_mag(pos2);
    r3_mag = vec_mag(pos3);

    if (r1_mag < 1.0 || r2_mag < 1.0 || r3_mag < 1.0)
        return -1;

    /* Cross products */
    vec_cross(pos1, pos2, z12);
    vec_cross(pos2, pos3, z23);
    vec_cross(pos3, pos1, z31);

    /* Coplanarity check: |r1 . z23| / (|r1| * |z23|) should be small */
    z23_mag = vec_mag(z23);
    if (z23_mag < 1e-10)
        return -1;  /* pos2 and pos3 are parallel (same direction) */

    coplanar = fabs(vec_dot(pos1, z23)) / (r1_mag * z23_mag);
    if (coplanar > COPLANAR_TOL)
        return -1;

    /* D = z12 + z23 + z31 */
    for (i = 0; i < 3; i++)
        D[i] = z12[i] + z23[i] + z31[i];

    /* N = |r1| * z23 + |r2| * z31 + |r3| * z12 */
    for (i = 0; i < 3; i++)
        N[i] = r1_mag * z23[i] + r2_mag * z31[i] + r3_mag * z12[i];

    /* S = (|r2| - |r3|) * r1 + (|r3| - |r1|) * r2 + (|r1| - |r2|) * r3 */
    for (i = 0; i < 3; i++)
        S[i] = (r2_mag - r3_mag) * pos1[i]
             + (r3_mag - r1_mag) * pos2[i]
             + (r1_mag - r2_mag) * pos3[i];

    D_mag = vec_mag(D);
    N_mag = vec_mag(N);

    if (D_mag < 1e-10 || N_mag < 1e-10)
        return -1;

    /* v2 = sqrt(mu / (N_mag * D_mag)) * (D x r2 / |r2| + S) */
    v2_coeff = sqrt(MU_KM3_S2 / (N_mag * D_mag));

    vec_cross(D, pos2, D_cross_r2);

    for (i = 0; i < 3; i++)
        v2[i] = v2_coeff * (D_cross_r2[i] / r2_mag + S[i]);

    /* Convert km/s velocity to the form expected by od_eci_to_keplerian */
    rc = od_eci_to_keplerian(pos2, v2, &result->kep);
    if (rc != 0)
        return -1;

    /* Sanity: eccentricity < 1 and positive mean motion */
    if (result->kep.ecc >= 1.0 || result->kep.n <= 0.0)
        return -1;

    result->epoch_jd = jd2;
    result->valid = 1;

    (void)jd1;
    (void)jd3;
    return 0;
}