/*
 * od_solver.c -- Batch weighted least-squares differential correction
 *
 * Implements TLE fitting from observations per Vallado & Crawford (2008)
 * AIAA 2008-6770. Uses equinoctial elements to avoid singularities,
 * SGP4/SDP4 as the propagation engine, and LAPACK dgelss_() for SVD.
 *
 * Algorithm summary:
 *   1. Convert seed TLE to equinoctial elements
 *   2. For each iteration:
 *      a. Propagate nominal state to all observation times
 *      b. Compute residuals (observed - computed)
 *      c. Build Jacobian by finite-differencing each element
 *      d. Solve min ||Ax - b|| via SVD
 *      e. Apply step limiting (Vallado 2008 Section V.B)
 *      f. Check convergence
 *   3. Convert final equinoctial → Keplerian → Kozai → TLE
 *
 * Memory: uses palloc/pfree when compiled with PostgreSQL,
 *         malloc/free otherwise (controlled by OD_USE_PALLOC).
 */

#include "od_solver.h"
#include "od_math.h"
#include "od_iod.h"
#include "norad.h"
#include "norad_in.h"

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

/* When compiled as part of the PostgreSQL extension, use palloc.
 * The Makefile sets PG_CPPFLAGS which pulls in postgres.h via types.h,
 * but od_solver.c doesn't include types.h directly. We detect via
 * the presence of PGXS-defined macros. */
#ifdef PG_MODULE_MAGIC
#include "postgres.h"
#define od_alloc(sz)    palloc(sz)
#define od_free(p)      pfree(p)
#else
#include <stdlib.h>
#define od_alloc(sz)    malloc(sz)
#define od_free(p)      free(p)
#endif

/* ── LAPACK interface ──────────────────────────────────── */

/*
 * dgelss_: solve min ||Ax - b|| via SVD.
 * A is M x N, b is M x 1, solution returned in b.
 * Column-major storage (Fortran convention).
 */
extern void dgelss_(int *m, int *n, int *nrhs,
                    double *a, int *lda,
                    double *b, int *ldb,
                    double *s, double *rcond, int *rank,
                    double *work, int *lwork, int *info);

/*
 * dpotrf_: Cholesky factorization of a symmetric positive-definite matrix.
 * dpotri_: Invert using the Cholesky factor from dpotrf_.
 * Used together to compute (H^T H)^{-1} for covariance output.
 */
extern void dpotrf_(char *uplo, int *n, double *a, int *lda, int *info);
extern void dpotri_(char *uplo, int *n, double *a, int *lda, int *info);


/* ── Internal propagation wrapper ─────────────────────── */

/*
 * Propagate a tle_t to tsince minutes, returning pos[3] (km)
 * and vel[3] (km/min). Reuses a pre-allocated params buffer.
 * Returns SGP4/SDP4 error code (0 = success).
 */
static int
propagate_with_params(const tle_t *sat, double tsince,
                      double *params, int is_deep,
                      double pos[3], double vel[3])
{
    if (is_deep)
        return SDP4(tsince, sat, params, pos, vel);
    else
        return SGP4(tsince, sat, params, pos, vel);
}

static void
init_params(const tle_t *sat, double *params, int is_deep)
{
    if (is_deep)
        SDP4_init(params, sat);
    else
        SGP4_init(params, sat);
}


/* ── Equinoctial ↔ TLE conversion ─────────────────────── */

/*
 * Extract equinoctial elements from a tle_t.
 *
 * TLE stores Kozai mean motion. We first convert to Brouwer mean
 * motion (which is what the equinoctial elements represent in the
 * fitted state), then compute the equinoctial set.
 */
static void
tle_to_equinoctial(const tle_t *tle, od_equinoctial_t *eq)
{
    od_keplerian_t kep;

    /* TLE xno is Kozai mean motion → convert to Brouwer */
    kep.n    = od_kozai_to_brouwer_n(tle->xno, tle->eo, tle->xincl);
    kep.ecc  = tle->eo;
    kep.inc  = tle->xincl;
    kep.raan = tle->xnodeo;
    kep.argp = tle->omegao;
    kep.M    = tle->xmo;
    kep.bstar = tle->bstar;

    od_keplerian_to_equinoctial(&kep, eq);
}

/*
 * Build a tle_t from equinoctial elements and a template TLE
 * (for metadata: NORAD ID, classification, intl desig, etc).
 *
 * Converts equinoctial → Keplerian → Kozai mean motion.
 */
static void
equinoctial_to_tle(const od_equinoctial_t *eq, const tle_t *tmpl,
                   tle_t *out)
{
    od_keplerian_t kep;

    od_equinoctial_to_keplerian(eq, &kep);

    memset(out, 0, sizeof(tle_t));

    /* Orbital elements */
    out->epoch   = tmpl->epoch;
    out->xincl   = kep.inc;
    out->xnodeo  = od_normalize_angle(kep.raan);
    out->eo      = kep.ecc;
    out->omegao  = od_normalize_angle(kep.argp);
    out->xmo     = od_normalize_angle(kep.M);

    /* Brouwer n → Kozai n for TLE storage */
    out->xno     = od_brouwer_to_kozai_n(kep.n, kep.ecc, kep.inc);

    /* Drag terms */
    out->bstar   = eq->bstar;
    out->xndt2o  = 0.0;  /* not fitted */
    out->xndd6o  = 0.0;  /* not fitted */

    /* Copy metadata from template */
    out->norad_number      = tmpl->norad_number;
    out->bulletin_number   = tmpl->bulletin_number;
    out->revolution_number = tmpl->revolution_number;
    out->classification    = tmpl->classification;
    out->ephemeris_type    = tmpl->ephemeris_type;
    memcpy(out->intl_desig, tmpl->intl_desig, 9);
}


/* ── Compute residuals ─────────────────────────────────── */

/*
 * Compute residual vector for ECI observations.
 * residuals[i*6 .. i*6+5] = observed[i] - propagated[i]
 * Returns RMS position error in km. Returns -1.0 on propagation failure.
 */
static double
compute_residuals_eci(const tle_t *tle, const od_observation_t *obs,
                      int n_obs, double *residuals)
{
    double *params;
    int is_deep;
    double sum_sq = 0.0;
    int i;

    is_deep = select_ephemeris(tle);
    if (is_deep < 0)
        return -1.0;

    params = (double *)od_alloc(sizeof(double) * N_SAT_PARAMS);
    init_params(tle, params, is_deep);

    for (i = 0; i < n_obs; i++)
    {
        double tsince = (obs[i].jd - tle->epoch) * 1440.0;
        double pos[3], vel[3];
        int err;
        double dx, dy, dz;

        err = propagate_with_params(tle, tsince, params, is_deep, pos, vel);
        if (err != 0 && err != SXPX_WARN_ORBIT_WITHIN_EARTH &&
            err != SXPX_WARN_PERIGEE_WITHIN_EARTH)
        {
            od_free(params);
            return -1.0;
        }

        /* SGP4 outputs vel in km/min; convert to km/s for comparison */
        vel[0] /= 60.0;
        vel[1] /= 60.0;
        vel[2] /= 60.0;

        /* Residual = observed - computed */
        dx = obs[i].data[0] - pos[0];
        dy = obs[i].data[1] - pos[1];
        dz = obs[i].data[2] - pos[2];

        residuals[i * 6 + 0] = dx;
        residuals[i * 6 + 1] = dy;
        residuals[i * 6 + 2] = dz;
        residuals[i * 6 + 3] = obs[i].data[3] - vel[0];
        residuals[i * 6 + 4] = obs[i].data[4] - vel[1];
        residuals[i * 6 + 5] = obs[i].data[5] - vel[2];

        sum_sq += dx * dx + dy * dy + dz * dz;
    }

    od_free(params);
    return sqrt(sum_sq / n_obs);
}

/*
 * Compute residual vector for topocentric observations.
 * residuals[i*3 .. i*3+2] = observed[i] - propagated[i]
 * Components: (az_diff, el_diff, range_diff) in (rad, rad, km).
 * Returns RMS range error in km. Returns -1.0 on propagation failure.
 */
static double
compute_residuals_topo(const tle_t *tle, const od_observation_t *obs,
                       int n_obs, const od_observer_t *observers,
                       double *residuals)
{
    double *params;
    int is_deep;
    double sum_sq = 0.0;
    int i;

    is_deep = select_ephemeris(tle);
    if (is_deep < 0)
        return -1.0;

    params = (double *)od_alloc(sizeof(double) * N_SAT_PARAMS);
    init_params(tle, params, is_deep);

    for (i = 0; i < n_obs; i++)
    {
        const od_observer_t *observer = &observers[obs[i].observer_idx];
        double tsince = (obs[i].jd - tle->epoch) * 1440.0;
        double pos[3], vel[3];
        int err;
        double gmst;
        double pos_ecef[3], vel_ecef[3];
        double cos_g, sin_g;
        double dx, dy, dz;
        double sin_lat, cos_lat, sin_lon, cos_lon;
        double south, east, up;
        double range_km, az, el;
        double az_diff, el_diff, range_diff;

        err = propagate_with_params(tle, tsince, params, is_deep, pos, vel);
        if (err != 0 && err != SXPX_WARN_ORBIT_WITHIN_EARTH &&
            err != SXPX_WARN_PERIGEE_WITHIN_EARTH)
        {
            od_free(params);
            return -1.0;
        }

        /* TEME → ECEF (replicate coord_funcs.c pipeline) */
        gmst = od_gmst_from_jd(obs[i].jd);
        cos_g = cos(gmst);
        sin_g = sin(gmst);

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

        /* vel in km/min → km/s for ECEF velocity */
        vel[0] /= 60.0;
        vel[1] /= 60.0;
        vel[2] /= 60.0;

        vel_ecef[0] =  cos_g * vel[0] + sin_g * vel[1]
                     + 7.2921158553e-5 * pos_ecef[1];
        vel_ecef[1] = -sin_g * vel[0] + cos_g * vel[1]
                     - 7.2921158553e-5 * pos_ecef[0];
        vel_ecef[2] = vel[2];

        /* ECEF → topocentric */
        dx = pos_ecef[0] - observer->ecef[0];
        dy = pos_ecef[1] - observer->ecef[1];
        dz = pos_ecef[2] - observer->ecef[2];

        sin_lat = sin(observer->lat);
        cos_lat = cos(observer->lat);
        sin_lon = sin(observer->lon);
        cos_lon = cos(observer->lon);

        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;

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

        /* Residuals in topo space */
        az_diff = obs[i].data[0] - az;
        /* Handle azimuth wrap-around */
        if (az_diff > M_PI) az_diff -= 2.0 * M_PI;
        if (az_diff < -M_PI) az_diff += 2.0 * M_PI;

        el_diff = obs[i].data[1] - el;
        range_diff = obs[i].data[2] - range_km;

        residuals[i * 3 + 0] = az_diff * range_km * cos(el);  /* scale to km */
        residuals[i * 3 + 1] = el_diff * range_km;             /* scale to km */
        residuals[i * 3 + 2] = range_diff;

        sum_sq += range_diff * range_diff;

        (void)vel_ecef;  /* reserved for range rate fitting */
    }

    od_free(params);
    return sqrt(sum_sq / n_obs);
}


/* ── Step limiting ─────────────────────────────────────── */

/*
 * Apply Vallado 2008 Section V.B step limits to the correction vector.
 *
 * Each element has a maximum allowable step. If the correction exceeds
 * the limit, it's scaled down. B* gets special treatment because it
 * operates on a different scale.
 */
static void
apply_step_limits(double *dx, int nstate, const od_equinoctial_t *eq,
                  double adaptive_factor)
{
    /* Step limits for equinoctial elements (Vallado 2008 Table 2) */
    static const double limits_6[6] = {
        0.01,   /* n: 1% of typical LEO mean motion */
        0.1,    /* af: eccentricity-related */
        0.1,    /* ag: eccentricity-related */
        0.1,    /* chi: inclination-related */
        0.1,    /* psi: inclination-related */
        0.5     /* L0: mean longitude */
    };

    int i;
    double ratio, max_ratio = 0.0;

    /* Find the maximum ratio of correction to limit, scaled by
     * adaptive_factor (trust-region-like damping when diverging) */
    for (i = 0; i < 6; i++)
    {
        double scale;
        if (i == 0)
            scale = fabs(eq->n) > 1e-10 ? fabs(eq->n) : 1e-3;
        else
            scale = 1.0;

        ratio = fabs(dx[i]) / (limits_6[i] * scale * adaptive_factor);
        if (ratio > max_ratio)
            max_ratio = ratio;
    }

    /* Scale all corrections proportionally if any exceeds its limit */
    if (max_ratio > 1.0)
    {
        double scale_factor = 1.0 / max_ratio;
        for (i = 0; i < 6; i++)
            dx[i] *= scale_factor;
    }

    /* B* special treatment: 40% limit when ratio > 1.0 */
    if (nstate == OD_NSTATE_7)
    {
        double bstar_limit = fabs(eq->bstar) > 1e-10
                           ? fabs(eq->bstar) * 0.4 * adaptive_factor
                           : 1e-5 * adaptive_factor;
        if (fabs(dx[6]) > bstar_limit)
            dx[6] = (dx[6] > 0.0 ? bstar_limit : -bstar_limit);
    }
}


/* ── Physical bounds enforcement ───────────────────────── */

/*
 * Clamp equinoctial elements to physically valid ranges.
 * Prevents the solver from wandering into nonsense.
 */
static void
enforce_bounds(od_equinoctial_t *eq)
{
    double ecc = sqrt(eq->af * eq->af + eq->ag * eq->ag);

    /* Eccentricity must be in [0, 0.999] */
    if (ecc > 0.999)
    {
        double scale = 0.999 / ecc;
        eq->af *= scale;
        eq->ag *= scale;
    }

    /* Mean motion must be positive */
    if (eq->n < 1e-8)
        eq->n = 1e-8;

    /* Semi-major axis must be >= 1 Earth radius (WGS-72) */
    {
        double a_er = pow(xke / eq->n, two_thirds);
        if (a_er < 1.0)
        {
            eq->n = xke;  /* set to a = 1 ER */
        }
    }
}


/* ── Initial guess from observations ───────────────────── */

/*
 * Helper: populate a TLE from Keplerian elements and an epoch.
 */
static void
kep_to_seed_tle(const od_keplerian_t *kep, double epoch_jd, tle_t *guess)
{
    memset(guess, 0, sizeof(tle_t));
    guess->epoch   = epoch_jd;
    guess->xincl   = kep->inc;
    guess->xnodeo  = od_normalize_angle(kep->raan);
    guess->eo      = kep->ecc;
    guess->omegao  = od_normalize_angle(kep->argp);
    guess->xmo     = od_normalize_angle(kep->M);
    guess->xno     = od_brouwer_to_kozai_n(kep->n, kep->ecc, kep->inc);
    guess->bstar   = 0.0;
    guess->norad_number = 99999;
    guess->classification = 'U';
    guess->ephemeris_type = '0';
}

/*
 * Compute initial orbit from ECI observations.
 *
 * ECI observations include full velocity, so single-obs r,v→elements
 * is exact for two-body and typically superior to Gibbs (which only
 * uses positions). Gibbs is only attempted as fallback when the
 * single-obs conversion fails (degenerate orbit).
 */
static void
initial_guess_from_eci(const od_observation_t *obs, int n_obs,
                       tle_t *guess)
{
    od_keplerian_t kep;

    /* Primary: use first observation's position/velocity (exact for
     * two-body, excellent for short-arc LEO) */
    if (od_eci_to_keplerian(obs[0].data, obs[0].data + 3, &kep) == 0)
    {
        kep_to_seed_tle(&kep, obs[0].jd, guess);
        return;
    }

    /* Fallback: Gibbs from 3 well-spaced positions */
    if (n_obs >= 3)
    {
        int i0 = 0, i1 = n_obs / 2, i2 = n_obs - 1;
        od_iod_result_t iod;

        if (od_gibbs(obs[i0].data, obs[i1].data, obs[i2].data,
                     obs[i0].jd, obs[i1].jd, obs[i2].jd,
                     &iod) == 0)
        {
            kep_to_seed_tle(&iod.kep, iod.epoch_jd, guess);
            return;
        }
    }

    /* Last resort: zero elements (solver will struggle) */
    memset(guess, 0, sizeof(tle_t));
    guess->epoch = obs[0].jd;
    guess->norad_number = 99999;
    guess->classification = 'U';
    guess->ephemeris_type = '0';
}

/*
 * Compute initial orbit from topocentric observations.
 *
 * Converts 3 well-spaced topo observations to TEME positions
 * via topo→ECEF→TEME, then uses Gibbs to recover the orbit.
 * Returns 0 on success, -1 if Gibbs fails.
 */
static int
initial_guess_from_topo(const od_observation_t *obs, int n_obs,
                        const od_observer_t *observers,
                        tle_t *guess)
{
    int idx[3];
    double pos_teme[3][3];
    double jd[3];
    int k;
    od_iod_result_t iod;

    if (n_obs < 3)
        return -1;

    idx[0] = 0;
    idx[1] = n_obs / 2;
    idx[2] = n_obs - 1;

    for (k = 0; k < 3; k++)
    {
        const od_observation_t *o = &obs[idx[k]];
        const od_observer_t *observer = &observers[o->observer_idx];
        double sat_ecef[3];
        double gmst;
        double vel_zero[3] = {0, 0, 0};

        /* Topo → ECEF */
        od_topocentric_to_ecef(o->data[0], o->data[1], o->data[2],
                               observer->ecef, observer->lat, observer->lon,
                               sat_ecef);

        /* ECEF → TEME */
        gmst = od_gmst_from_jd(o->jd);
        od_ecef_to_teme(sat_ecef, vel_zero, gmst,
                        pos_teme[k], vel_zero);

        jd[k] = o->jd;
    }

    if (od_gibbs(pos_teme[0], pos_teme[1], pos_teme[2],
                 jd[0], jd[1], jd[2], &iod) != 0)
        return -1;

    kep_to_seed_tle(&iod.kep, iod.epoch_jd, guess);
    return 0;
}


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

int
od_fit_tle(const od_observation_t *obs, int n_obs,
           const tle_t *seed, const od_config_t *config,
           od_result_t *result)
{
    int nstate;         /* 6 or 7 */
    int ncomp;          /* components per observation: 6 (ECI) or 3 (topo) */
    int nrows;          /* total residual components: ncomp * n_obs */
    od_equinoctial_t eq;
    tle_t current_tle;
    tle_t seed_tle;
    double *residuals = NULL;
    double *jacobian = NULL;
    double *dx = NULL;
    double *sv = NULL;
    double *work = NULL;
    double rms_prev, rms_cur;
    double adaptive_factor = 1.0;   /* trust-region damping */
    double condition_number = 0.0;  /* s_max / s_min from SVD */
    int iter;
    int max_iter;
    int converged = 0;
    int j_cov, k_cov;              /* covariance loop indices */

    /* Validate inputs */
    nstate = config->fit_bstar ? OD_NSTATE_7 : OD_NSTATE_6;
    ncomp  = (config->obs_type == OD_OBS_ECI) ? 6 : 3;

    if (n_obs < nstate)
    {
        snprintf(result->status, sizeof(result->status),
                 "insufficient observations: %d < %d", n_obs, nstate);
        return -1;
    }

    nrows = ncomp * n_obs;
    max_iter = config->max_iter > 0 ? config->max_iter : OD_MAX_ITER;
    if (max_iter > OD_MAX_ITER)
        max_iter = OD_MAX_ITER;

    /* Initialize seed TLE */
    if (seed)
    {
        memcpy(&seed_tle, seed, sizeof(tle_t));
    }
    else
    {
        if (config->obs_type == OD_OBS_ECI)
            initial_guess_from_eci(obs, n_obs, &seed_tle);
        else
        {
            int rc_iod = initial_guess_from_topo(obs, n_obs,
                                                 config->observers,
                                                 &seed_tle);
            if (rc_iod != 0)
            {
                snprintf(result->status, sizeof(result->status),
                         "IOD bootstrap failed (Gibbs): need seed TLE");
                return -1;
            }
        }
    }

    /* Extract equinoctial elements from seed */
    tle_to_equinoctial(&seed_tle, &eq);

    /* Allocate working arrays */
    residuals = (double *)od_alloc(sizeof(double) * nrows);
    jacobian  = (double *)od_alloc(sizeof(double) * nrows * nstate);
    dx        = (double *)od_alloc(sizeof(double) * (nrows > nstate ? nrows : nstate));
    sv        = (double *)od_alloc(sizeof(double) * nstate);

    /* LAPACK workspace query */
    {
        int m = nrows, n = nstate, nrhs = 1, lda = nrows, ldb = nrows;
        int rank, info, lwork = -1;
        double rcond = -1.0;
        double work_query;

        dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
                sv, &rcond, &rank, &work_query, &lwork, &info);
        lwork = (int)work_query + 1;
        work = (double *)od_alloc(sizeof(double) * lwork);
    }

    /* Build initial TLE from equinoctial state */
    equinoctial_to_tle(&eq, &seed_tle, &current_tle);

    /* Compute initial RMS */
    if (config->obs_type == OD_OBS_ECI)
        rms_cur = compute_residuals_eci(&current_tle, obs, n_obs, residuals);
    else
        rms_cur = compute_residuals_topo(&current_tle, obs, n_obs,
                                         config->observers, residuals);

    if (rms_cur < 0.0)
    {
        snprintf(result->status, sizeof(result->status),
                 "initial propagation failed");
        od_free(residuals);
        od_free(jacobian);
        od_free(dx);
        od_free(sv);
        od_free(work);
        return -1;
    }

    result->rms_initial = rms_cur;
    rms_prev = rms_cur;

    /* Already converged (perfect seed)? Skip DC loop but still
     * compute covariance — users need uncertainty estimates even
     * when the initial guess is exact. */
    if (rms_cur < OD_RMS_ABS_TOL)
    {
        converged = 1;
        iter = 0;
        goto compute_covariance;
    }

    /* ── DC iteration loop ─────────────────────────────── */

    for (iter = 0; iter < max_iter; iter++)
    {
        int j, k;
        int m, n, nrhs, lda, ldb, rank, info, lwork_val;
        double rcond;

        /* Build Jacobian by finite differencing */
        for (j = 0; j < nstate; j++)
        {
            od_equinoctial_t eq_pert;
            tle_t tle_pert;
            double *resid_pert;
            double h;
            double elem_val;

            memcpy(&eq_pert, &eq, sizeof(od_equinoctial_t));

            /* Element value for scaling the perturbation */
            switch (j)
            {
                case 0: elem_val = eq.n;     break;
                case 1: elem_val = eq.af;    break;
                case 2: elem_val = eq.ag;    break;
                case 3: elem_val = eq.chi;   break;
                case 4: elem_val = eq.psi;   break;
                case 5: elem_val = eq.L0;    break;
                case 6: elem_val = eq.bstar; break;
                default: elem_val = 0.0;     break;
            }

            /* Perturbation step: max of relative and absolute */
            h = fabs(elem_val) * 1e-6;
            if (h < 1e-10)
                h = 1e-10;

            /* Apply perturbation */
            switch (j)
            {
                case 0: eq_pert.n     += h; break;
                case 1: eq_pert.af    += h; break;
                case 2: eq_pert.ag    += h; break;
                case 3: eq_pert.chi   += h; break;
                case 4: eq_pert.psi   += h; break;
                case 5: eq_pert.L0    += h; break;
                case 6: eq_pert.bstar += h; break;
            }

            equinoctial_to_tle(&eq_pert, &seed_tle, &tle_pert);

            resid_pert = (double *)od_alloc(sizeof(double) * nrows);

            if (config->obs_type == OD_OBS_ECI)
                compute_residuals_eci(&tle_pert, obs, n_obs, resid_pert);
            else
                compute_residuals_topo(&tle_pert, obs, n_obs,
                                       config->observers, resid_pert);

            /* Jacobian column j (column-major for LAPACK)
             * H = dG/dx where G is the computed observation function.
             * resid = obs - G(x), resid_pert = obs - G(x + h*e_j), so:
             * (resid - resid_pert) / h = (G(x+h) - G(x)) / h = dG/dx_j */
            for (k = 0; k < nrows; k++)
                jacobian[j * nrows + k] = (residuals[k] - resid_pert[k]) / h;

            od_free(resid_pert);
        }

        /* Copy residuals into dx (LAPACK overwrites with solution).
         * Jacobian is H = dG/dx, and residual = obs - G(x), so
         * LAPACK solves H*delta = residual (standard DC). */
        memcpy(dx, residuals, sizeof(double) * nrows);

        /* Solve via SVD */
        m = nrows;
        n = nstate;
        nrhs = 1;
        lda = nrows;
        ldb = nrows;
        rcond = -1.0;  /* use machine epsilon */
        {
            /* Re-query workspace since dimensions haven't changed */
            int lwork_query = -1;
            double wq;
            dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
                    sv, &rcond, &rank, &wq, &lwork_query, &info);
            lwork_val = (int)wq + 1;

            /* Reallocate work if needed */
            od_free(work);
            work = (double *)od_alloc(sizeof(double) * lwork_val);
        }

        dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
                sv, &rcond, &rank, work, &lwork_val, &info);

        if (info != 0)
        {
            snprintf(result->status, sizeof(result->status),
                     "LAPACK dgelss_ failed: info=%d", info);
            break;
        }

        /* Store condition number from SVD singular values */
        if (sv[nstate - 1] > 1e-30)
            condition_number = sv[0] / sv[nstate - 1];

        /* Apply step limiting (scaled by adaptive factor) */
        apply_step_limits(dx, nstate, &eq, adaptive_factor);

        /* Update equinoctial elements */
        eq.n     += dx[0];
        eq.af    += dx[1];
        eq.ag    += dx[2];
        eq.chi   += dx[3];
        eq.psi   += dx[4];
        eq.L0    += dx[5];
        if (nstate == OD_NSTATE_7)
            eq.bstar += dx[6];

        eq.L0 = od_normalize_angle(eq.L0);

        /* Enforce physical bounds */
        enforce_bounds(&eq);

        /* Rebuild TLE and compute new residuals */
        equinoctial_to_tle(&eq, &seed_tle, &current_tle);

        if (config->obs_type == OD_OBS_ECI)
            rms_cur = compute_residuals_eci(&current_tle, obs, n_obs, residuals);
        else
            rms_cur = compute_residuals_topo(&current_tle, obs, n_obs,
                                             config->observers, residuals);

        if (rms_cur < 0.0)
        {
            snprintf(result->status, sizeof(result->status),
                     "propagation failed at iteration %d", iter + 1);
            break;
        }

        /* Adaptive step control: adjust trust-region size based on
         * whether the solver is converging or diverging. Halve the
         * step on divergence (prevents oscillation with poor initial
         * guesses), relax toward full step when converging well. */
        if (rms_cur > rms_prev * 1.01)
        {
            adaptive_factor *= 0.5;
            if (adaptive_factor < 0.05)
                adaptive_factor = 0.05;
        }
        else if (rms_cur < rms_prev * 0.9)
        {
            adaptive_factor = fmin(adaptive_factor * 1.3, 1.0);
        }

        /* Check convergence: either absolute RMS is small enough,
         * or relative change has plateaued (solver found its best fit,
         * common for SDP4 deep-space orbits where the numerical floor
         * is ~meters). */
        {
            double rms_change = fabs(rms_cur - rms_prev);
            double rms_rel = (rms_prev > 1e-15)
                           ? rms_change / rms_prev
                           : rms_change;

            if (rms_rel < OD_RMS_REL_TOL || rms_cur < OD_RMS_ABS_TOL)
            {
                converged = 1;
                iter++;  /* count this iteration */
                break;
            }
        }

        rms_prev = rms_cur;
    }

compute_covariance:
    /* ── Covariance computation ─────────────────────────── */

    /*
     * Compute formal covariance matrix (H^T H)^{-1} at the converged
     * solution. This requires rebuilding the Jacobian at the final state
     * (the DC loop overwrites it each iteration via LAPACK).
     *
     * The covariance gives parameter uncertainty estimates — diagonal
     * elements are variances, off-diagonals are correlations. Essential
     * for conjunction screening probability computations.
     */
    result->cov_size = 0;
    result->condition_number = condition_number;

    if (converged || iter >= max_iter)
    {
        /* Rebuild Jacobian at final state (same finite-diff as DC loop) */
        for (j_cov = 0; j_cov < nstate; j_cov++)
        {
            od_equinoctial_t eq_pert;
            tle_t tle_pert;
            double *resid_pert;
            double h, elem_val;

            memcpy(&eq_pert, &eq, sizeof(od_equinoctial_t));

            switch (j_cov)
            {
                case 0: elem_val = eq.n;     break;
                case 1: elem_val = eq.af;    break;
                case 2: elem_val = eq.ag;    break;
                case 3: elem_val = eq.chi;   break;
                case 4: elem_val = eq.psi;   break;
                case 5: elem_val = eq.L0;    break;
                case 6: elem_val = eq.bstar; break;
                default: elem_val = 0.0;     break;
            }

            h = fabs(elem_val) * 1e-6;
            if (h < 1e-10)
                h = 1e-10;

            switch (j_cov)
            {
                case 0: eq_pert.n     += h; break;
                case 1: eq_pert.af    += h; break;
                case 2: eq_pert.ag    += h; break;
                case 3: eq_pert.chi   += h; break;
                case 4: eq_pert.psi   += h; break;
                case 5: eq_pert.L0    += h; break;
                case 6: eq_pert.bstar += h; break;
            }

            equinoctial_to_tle(&eq_pert, &seed_tle, &tle_pert);

            resid_pert = (double *)od_alloc(sizeof(double) * nrows);

            if (config->obs_type == OD_OBS_ECI)
                compute_residuals_eci(&tle_pert, obs, n_obs, resid_pert);
            else
                compute_residuals_topo(&tle_pert, obs, n_obs,
                                       config->observers, resid_pert);

            for (k_cov = 0; k_cov < nrows; k_cov++)
                jacobian[j_cov * nrows + k_cov] =
                    (residuals[k_cov] - resid_pert[k_cov]) / h;

            od_free(resid_pert);
        }

        /* Compute H^T H before SVD destroys the Jacobian */
        {
            double *hth;
            int ci, cj, ri, info_cov;
            char uplo = 'U';

            hth = (double *)od_alloc(sizeof(double) * nstate * nstate);
            memset(hth, 0, sizeof(double) * nstate * nstate);

            for (ci = 0; ci < nstate; ci++)
            {
                for (cj = ci; cj < nstate; cj++)
                {
                    double dot = 0.0;
                    for (ri = 0; ri < nrows; ri++)
                        dot += jacobian[ci * nrows + ri] *
                               jacobian[cj * nrows + ri];
                    hth[ci + cj * nstate] = dot;
                    hth[cj + ci * nstate] = dot;
                }
            }

            /* SVD of Jacobian for condition number (destroys jacobian) */
            {
                int m = nrows, n = nstate, nrhs = 1;
                int lda = nrows, ldb = nrows, rank, info_svd;
                double rcond = -1.0;

                memcpy(dx, residuals, sizeof(double) * nrows);

                od_free(work);
                {
                    int lw = -1;
                    double wq;
                    dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
                            sv, &rcond, &rank, &wq, &lw, &info_svd);
                    lw = (int)wq + 1;
                    work = (double *)od_alloc(sizeof(double) * lw);
                    dgelss_(&m, &n, &nrhs, jacobian, &lda, dx, &ldb,
                            sv, &rcond, &rank, work, &lw, &info_svd);
                }

                if (info_svd == 0 && sv[nstate - 1] > 1e-30)
                    condition_number = sv[0] / sv[nstate - 1];
            }

            result->condition_number = condition_number;

            /* Cholesky factorize H^T H, then invert for covariance */
            dpotrf_(&uplo, &nstate, hth, &nstate, &info_cov);

            if (info_cov == 0)
            {
                dpotri_(&uplo, &nstate, hth, &nstate, &info_cov);

                if (info_cov == 0)
                {
                    int idx = 0;
                    for (ci = 0; ci < nstate; ci++)
                        for (cj = ci; cj < nstate; cj++)
                            result->covariance[idx++] = hth[ci + cj * nstate];
                    result->cov_size = idx;  /* 21 or 28 */
                }
            }

            od_free(hth);
        }
    }

    /* ── Build result ──────────────────────────────────── */

    memcpy(&result->fitted_tle, &current_tle, sizeof(tle_t));
    result->iterations = iter;
    result->rms_final  = rms_cur;
    result->converged  = converged;

    if (converged)
        snprintf(result->status, sizeof(result->status), "converged");
    else if (iter >= max_iter && result->status[0] == '\0')
        snprintf(result->status, sizeof(result->status), "max_iterations");

    /* Cleanup */
    od_free(residuals);
    od_free(jacobian);
    od_free(dx);
    od_free(sv);
    od_free(work);

    return 0;
}