/*
 * od_math.c -- Orbital determination math implementation
 *
 * Element conversions and inverse coordinate transforms for the
 * TLE fitting pipeline. All orbital mechanics use WGS-72 constants
 * to maintain chain of custody with SGP4/SDP4.
 *
 * References:
 *   Vallado & Crawford, "SGP4 Orbit Determination", AIAA 2008-6770
 *   Vallado, "Fundamentals of Astrodynamics and Applications", 4th ed.
 *   Hoots & Roehrich, "Spacetrack Report No. 3", 1980
 */

#include "od_math.h"

/* WGS-72 gravitational parameter — must match SGP4's internal value.
 * 398600.8 km^3/s^2 (from STR#3), NOT the WGS-84 value 398600.4418. */
#define MU_KM3_S2   398600.8

/* WGS-72 equatorial radius in km */
#define RE_KM       6378.135

/* SGP4 internal constants from norad_in.h */
#define XKE         0.0743669161331734132
#define CK2         (0.5 * 1.082616e-3)
#define TWO_THIRDS  (2.0 / 3.0)

/* WGS-84 constants (coordinate transforms only) */
#define WGS84_A_KM  6378.137
#define WGS84_F     (1.0 / 298.257223563)
#define WGS84_E2    (WGS84_F * (2.0 - WGS84_F))

/* Earth rotation rate, rad/s */
#define OMEGA_EARTH 7.2921158553e-5


/* ================================================================
 * GMST and observer position (duplicated to avoid symbol coupling)
 * ================================================================
 */

double
od_gmst_from_jd(double jd)
{
    double t_ut1 = (jd - 2451545.0) / 36525.0;
    double gmst = 67310.54841
                + (876600.0 * 3600.0 + 8640184.812866) * t_ut1
                + 0.093104 * t_ut1 * t_ut1
                - 6.2e-6 * t_ut1 * t_ut1 * t_ut1;

    gmst = fmod(gmst * M_PI / 43200.0, 2.0 * M_PI);
    if (gmst < 0.0)
        gmst += 2.0 * M_PI;
    return gmst;
}

void
od_observer_to_ecef(double lat, double lon, double alt_m,
                    double pos_ecef[3])
{
    double sin_lat = sin(lat);
    double cos_lat = cos(lat);
    double N       = WGS84_A_KM / sqrt(1.0 - WGS84_E2 * sin_lat * sin_lat);
    double alt_km  = alt_m / 1000.0;

    pos_ecef[0] = (N + alt_km) * cos_lat * cos(lon);
    pos_ecef[1] = (N + alt_km) * cos_lat * sin(lon);
    pos_ecef[2] = (N * (1.0 - WGS84_E2) + alt_km) * sin_lat;
}


/* ================================================================
 * Inverse coordinate transforms
 * ================================================================
 */

/*
 * ECEF → TEME: inverse of teme_to_ecef (coord_funcs.c:84-105).
 * Rotate position by +GMST (instead of -GMST), subtract Earth
 * rotation cross-product from velocity.
 */
void
od_ecef_to_teme(const double pos_ecef[3], const double vel_ecef[3],
                double gmst,
                double pos_teme[3], double vel_teme[3])
{
    double cos_g = cos(gmst);
    double sin_g = sin(gmst);

    /* Position: rotate by +GMST (inverse of -GMST rotation) */
    pos_teme[0] =  cos_g * pos_ecef[0] - sin_g * pos_ecef[1];
    pos_teme[1] =  sin_g * pos_ecef[0] + cos_g * pos_ecef[1];
    pos_teme[2] = pos_ecef[2];

    if (vel_teme && vel_ecef)
    {
        /*
         * Forward: vel_ecef = R(-g)*vel_teme + omega x pos_ecef
         * Inverse: vel_teme = R(+g)*(vel_ecef - omega x pos_ecef)
         *
         * omega x pos_ecef = (-omega*y, +omega*x, 0) in ECEF
         * but the forward code adds omega*pos_ecef[1] to vel_ecef[0]
         * and subtracts omega*pos_ecef[0] from vel_ecef[1], so undo that.
         */
        double vel_inertial_ecef_x = vel_ecef[0] - OMEGA_EARTH * pos_ecef[1];
        double vel_inertial_ecef_y = vel_ecef[1] + OMEGA_EARTH * pos_ecef[0];

        vel_teme[0] =  cos_g * vel_inertial_ecef_x - sin_g * vel_inertial_ecef_y;
        vel_teme[1] =  sin_g * vel_inertial_ecef_x + cos_g * vel_inertial_ecef_y;
        vel_teme[2] = vel_ecef[2];
    }
}

/*
 * Topocentric (az, el, range) → ECEF satellite position.
 * Inverse of ecef_to_topocentric (coord_funcs.c:165-190).
 *
 * The forward transform computes SEU (South, East, Up) components
 * from the ECEF range vector, then derives az/el. We invert:
 *   1. Recover SEU from az/el/range
 *   2. Transpose the SEU rotation matrix (orthogonal → inverse = transpose)
 *   3. Add observer ECEF position
 */
void
od_topocentric_to_ecef(double az, double el, double range_km,
                       const double obs_ecef[3],
                       double obs_lat, double obs_lon,
                       double sat_ecef[3])
{
    /* Recover SEU from az/el/range.
     * Forward: az = atan2(east, -south), el = asin(up / range)
     * So: south = -range*cos(el)*cos(az), east = range*cos(el)*sin(az) */
    double cos_el = cos(el);
    double south  = -range_km * cos_el * cos(az);
    double east   =  range_km * cos_el * sin(az);
    double up     =  range_km * sin(el);

    /* The forward rotation (ECEF range → SEU) uses:
     *   south =  sin_lat*cos_lon*dx + sin_lat*sin_lon*dy - cos_lat*dz
     *   east  = -sin_lon*dx + cos_lon*dy
     *   up    =  cos_lat*cos_lon*dx + cos_lat*sin_lon*dy + sin_lat*dz
     *
     * This is R * [dx, dy, dz]^T where R is orthogonal.
     * Inverse: [dx, dy, dz]^T = R^T * [south, east, up]^T
     */
    double sin_lat = sin(obs_lat);
    double cos_lat = cos(obs_lat);
    double sin_lon = sin(obs_lon);
    double cos_lon = cos(obs_lon);

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

    sat_ecef[0] = obs_ecef[0] + dx;
    sat_ecef[1] = obs_ecef[1] + dy;
    sat_ecef[2] = obs_ecef[2] + dz;
}


/* ================================================================
 * ECI ↔ Keplerian element conversion
 * ================================================================
 */

/*
 * ECI state vector → classical Keplerian elements.
 *
 * Standard r,v → orbital elements conversion (Vallado Algorithm 9).
 * Uses WGS-72 mu for SGP4 consistency.
 *
 * Returns 0 on success, -1 if orbit is degenerate (r ≈ 0).
 */
int
od_eci_to_keplerian(const double pos[3], const double vel[3],
                    od_keplerian_t *kep)
{
    double r_mag, v_mag, rdotv;
    double h[3], h_mag;
    double n_vec[3], n_mag;
    double e_vec[3], ecc;
    double a, p;
    double inc, raan, argp, nu, M, E;

    /* Position and velocity magnitudes */
    r_mag = sqrt(pos[0]*pos[0] + pos[1]*pos[1] + pos[2]*pos[2]);
    v_mag = sqrt(vel[0]*vel[0] + vel[1]*vel[1] + vel[2]*vel[2]);

    if (r_mag < 1.0e-10)
        return -1;

    rdotv = pos[0]*vel[0] + pos[1]*vel[1] + pos[2]*vel[2];

    /* Angular momentum h = r x v */
    h[0] = pos[1]*vel[2] - pos[2]*vel[1];
    h[1] = pos[2]*vel[0] - pos[0]*vel[2];
    h[2] = pos[0]*vel[1] - pos[1]*vel[0];
    h_mag = sqrt(h[0]*h[0] + h[1]*h[1] + h[2]*h[2]);

    if (h_mag < 1.0e-10)
        return -1;

    /* Node vector n = k x h (k = [0,0,1]) */
    n_vec[0] = -h[1];
    n_vec[1] =  h[0];
    n_vec[2] = 0.0;
    n_mag = sqrt(n_vec[0]*n_vec[0] + n_vec[1]*n_vec[1]);

    /* Eccentricity vector: e = (v x h)/mu - r/|r| */
    e_vec[0] = (vel[1]*h[2] - vel[2]*h[1]) / MU_KM3_S2 - pos[0] / r_mag;
    e_vec[1] = (vel[2]*h[0] - vel[0]*h[2]) / MU_KM3_S2 - pos[1] / r_mag;
    e_vec[2] = (vel[0]*h[1] - vel[1]*h[0]) / MU_KM3_S2 - pos[2] / r_mag;
    ecc = sqrt(e_vec[0]*e_vec[0] + e_vec[1]*e_vec[1] + e_vec[2]*e_vec[2]);

    /* Semi-major axis from vis-viva */
    a = 1.0 / (2.0 / r_mag - v_mag * v_mag / MU_KM3_S2);
    p = h_mag * h_mag / MU_KM3_S2;

    /* Inclination */
    inc = acos(h[2] / h_mag);
    if (inc < 0.0) inc = 0.0;  /* clamp rounding errors */

    /* RAAN */
    if (n_mag > 1.0e-10)
    {
        raan = acos(n_vec[0] / n_mag);
        if (n_vec[1] < 0.0)
            raan = 2.0 * M_PI - raan;
    }
    else
    {
        raan = 0.0;  /* equatorial orbit */
    }

    /* Argument of perigee */
    if (n_mag > 1.0e-10 && ecc > 1.0e-10)
    {
        double ndote = (n_vec[0]*e_vec[0] + n_vec[1]*e_vec[1] +
                        n_vec[2]*e_vec[2]) / (n_mag * ecc);
        if (ndote > 1.0) ndote = 1.0;
        if (ndote < -1.0) ndote = -1.0;
        argp = acos(ndote);
        if (e_vec[2] < 0.0)
            argp = 2.0 * M_PI - argp;
    }
    else if (ecc > 1.0e-10)
    {
        /* Equatorial: measure from x-axis */
        argp = atan2(e_vec[1], e_vec[0]);
        if (argp < 0.0) argp += 2.0 * M_PI;
    }
    else
    {
        argp = 0.0;  /* circular */
    }

    /* True anomaly */
    if (ecc > 1.0e-10)
    {
        double edotr = (e_vec[0]*pos[0] + e_vec[1]*pos[1] +
                        e_vec[2]*pos[2]) / (ecc * r_mag);
        if (edotr > 1.0) edotr = 1.0;
        if (edotr < -1.0) edotr = -1.0;
        nu = acos(edotr);
        if (rdotv < 0.0)
            nu = 2.0 * M_PI - nu;
    }
    else
    {
        /* Circular: measure from node or x-axis */
        if (n_mag > 1.0e-10)
        {
            double ndotr = (n_vec[0]*pos[0] + n_vec[1]*pos[1]) / (n_mag * r_mag);
            if (ndotr > 1.0) ndotr = 1.0;
            if (ndotr < -1.0) ndotr = -1.0;
            nu = acos(ndotr);
            if (pos[2] < 0.0)
                nu = 2.0 * M_PI - nu;
        }
        else
        {
            nu = atan2(pos[1], pos[0]);
            if (nu < 0.0) nu += 2.0 * M_PI;
        }
    }

    /* True anomaly → eccentric anomaly → mean anomaly */
    E = atan2(sqrt(1.0 - ecc * ecc) * sin(nu), ecc + cos(nu));
    M = E - ecc * sin(E);
    if (M < 0.0) M += 2.0 * M_PI;

    /* Mean motion from semi-major axis (rad/s → rad/min) */
    kep->n    = sqrt(MU_KM3_S2 / (a * a * a)) * 60.0;
    kep->ecc  = ecc;
    kep->inc  = inc;
    kep->raan = od_normalize_angle(raan);
    kep->argp = od_normalize_angle(argp);
    kep->M    = od_normalize_angle(M);
    kep->bstar = 0.0;

    (void)p;  /* used implicitly in derivation */
    return 0;
}

/*
 * Classical Keplerian → ECI state vector.
 *
 * Standard perifocal frame computation (Vallado Algorithm 10).
 */
void
od_keplerian_to_eci(const od_keplerian_t *kep,
                    double pos[3], double vel[3])
{
    /* Mean motion rad/min → semi-major axis */
    double n_rad_s = kep->n / 60.0;
    double a = pow(MU_KM3_S2 / (n_rad_s * n_rad_s), 1.0 / 3.0);
    double ecc = kep->ecc;
    double inc = kep->inc;
    double raan = kep->raan;
    double argp = kep->argp;
    double M = kep->M;
    double E, dE;
    int i;
    double cos_E, sin_E;
    double r_peri[2], v_peri[2];
    double r_mag, v_coeff;

    /* Solve Kepler's equation: M = E - e*sin(E) */
    E = M;
    for (i = 0; i < 20; i++)
    {
        dE = (M - E + ecc * sin(E)) / (1.0 - ecc * cos(E));
        E += dE;
        if (fabs(dE) < 1.0e-14)
            break;
    }

    cos_E = cos(E);
    sin_E = sin(E);

    /* Perifocal coordinates */
    r_peri[0] = a * (cos_E - ecc);
    r_peri[1] = a * sqrt(1.0 - ecc * ecc) * sin_E;

    r_mag = a * (1.0 - ecc * cos_E);
    v_coeff = sqrt(MU_KM3_S2 * a) / r_mag;
    v_peri[0] = -v_coeff * sin_E;
    v_peri[1] =  v_coeff * sqrt(1.0 - ecc * ecc) * cos_E;

    /* Perifocal → ECI rotation */
    {
        double cos_raan = cos(raan), sin_raan = sin(raan);
        double cos_argp = cos(argp), sin_argp = sin(argp);
        double cos_inc  = cos(inc),  sin_inc  = sin(inc);

        /* Rotation matrix columns (P and Q vectors) */
        double Px =  cos_raan * cos_argp - sin_raan * sin_argp * cos_inc;
        double Py =  sin_raan * cos_argp + cos_raan * sin_argp * cos_inc;
        double Pz =  sin_argp * sin_inc;

        double Qx = -cos_raan * sin_argp - sin_raan * cos_argp * cos_inc;
        double Qy = -sin_raan * sin_argp + cos_raan * cos_argp * cos_inc;
        double Qz =  cos_argp * sin_inc;

        pos[0] = Px * r_peri[0] + Qx * r_peri[1];
        pos[1] = Py * r_peri[0] + Qy * r_peri[1];
        pos[2] = Pz * r_peri[0] + Qz * r_peri[1];

        vel[0] = Px * v_peri[0] + Qx * v_peri[1];
        vel[1] = Py * v_peri[0] + Qy * v_peri[1];
        vel[2] = Pz * v_peri[0] + Qz * v_peri[1];
    }
}


/* ================================================================
 * Keplerian ↔ equinoctial element conversion
 * ================================================================
 */

/*
 * Keplerian → equinoctial (Vallado 2008 Eq. 1-5)
 *
 * af = e * cos(omega + RAAN)
 * ag = e * sin(omega + RAAN)
 * chi = tan(i/2) * cos(RAAN)
 * psi = tan(i/2) * sin(RAAN)
 * L0 = M + omega + RAAN
 */
void
od_keplerian_to_equinoctial(const od_keplerian_t *kep,
                            od_equinoctial_t *eq)
{
    double omega_plus_raan = kep->argp + kep->raan;
    double tan_half_inc = tan(kep->inc / 2.0);

    eq->n    = kep->n;
    eq->af   = kep->ecc * cos(omega_plus_raan);
    eq->ag   = kep->ecc * sin(omega_plus_raan);
    eq->chi  = tan_half_inc * cos(kep->raan);
    eq->psi  = tan_half_inc * sin(kep->raan);
    eq->L0   = od_normalize_angle(kep->M + omega_plus_raan);
    eq->bstar = kep->bstar;
}

/*
 * Equinoctial → Keplerian (inverse of Vallado 2008 Eq. 1-5)
 *
 * e = sqrt(af^2 + ag^2)
 * i = 2 * atan(sqrt(chi^2 + psi^2))
 * RAAN = atan2(psi, chi)
 * omega = atan2(ag, af) - RAAN
 * M = L0 - omega - RAAN
 */
void
od_equinoctial_to_keplerian(const od_equinoctial_t *eq,
                            od_keplerian_t *kep)
{
    double ecc = sqrt(eq->af * eq->af + eq->ag * eq->ag);
    double inc = 2.0 * atan(sqrt(eq->chi * eq->chi + eq->psi * eq->psi));
    double raan = atan2(eq->psi, eq->chi);
    double omega_plus_raan = atan2(eq->ag, eq->af);
    double argp, M;

    if (raan < 0.0) raan += 2.0 * M_PI;

    argp = omega_plus_raan - raan;
    M = eq->L0 - omega_plus_raan;

    kep->n    = eq->n;
    kep->ecc  = ecc;
    kep->inc  = inc;
    kep->raan = od_normalize_angle(raan);
    kep->argp = od_normalize_angle(argp);
    kep->M    = od_normalize_angle(M);
    kep->bstar = eq->bstar;
}


/* ================================================================
 * Brouwer ↔ Kozai mean motion conversion
 * ================================================================
 */

/*
 * Forward function: Kozai n → Brouwer n.
 *
 * Mirrors sxpall_common_init() from common.c:36-54.
 * Given n_kozai (the TLE xno field), computes the Brouwer
 * mean motion xnodp that SGP4 uses internally.
 */
static double
kozai_to_brouwer_forward(double n_kozai, double ecc, double inc)
{
    double cosio = cos(inc);
    double cosio2 = cosio * cosio;
    double eosq = ecc * ecc;
    double betao2 = 1.0 - eosq;
    double betao = sqrt(betao2);
    double a1, del1, ao, delo, xnodp;
    double tval;

    a1 = pow(XKE / n_kozai, TWO_THIRDS);
    tval = 1.5 * CK2 * (3.0 * cosio2 - 1.0) / (betao * betao2);
    del1 = tval / (a1 * a1);
    ao = a1 * (1.0 - del1 * (1.0 / 3.0 + del1 * (1.0 + 134.0 / 81.0 * del1)));
    delo = tval / (ao * ao);
    xnodp = n_kozai / (1.0 + delo);

    return xnodp;
}

/*
 * Kozai → Brouwer: direct computation (no iteration needed).
 */
double
od_kozai_to_brouwer_n(double n_kozai, double ecc, double inc)
{
    return kozai_to_brouwer_forward(n_kozai, ecc, inc);
}

/*
 * Brouwer → Kozai: Newton-Raphson inversion.
 *
 * Solves f(n_kozai) = n_brouwer for n_kozai, where
 * f = kozai_to_brouwer_forward. Converges in 2-4 iterations.
 */
double
od_brouwer_to_kozai_n(double n_brouwer, double ecc, double inc)
{
    double n_k = n_brouwer;  /* initial guess */
    double h = 1.0e-10;
    int i;

    for (i = 0; i < 20; i++)
    {
        double f_k  = kozai_to_brouwer_forward(n_k, ecc, inc);
        double f_kh = kozai_to_brouwer_forward(n_k + h, ecc, inc);
        double fp   = (f_kh - f_k) / h;
        double delta;

        if (fabs(fp) < 1.0e-20)
            break;

        delta = (f_k - n_brouwer) / fp;
        n_k -= delta;

        if (fabs(delta) < 1.0e-14)
            break;
    }

    return n_k;
}