"""Checks the numbers quoted in orbit-determination.qmd, using the helper module _od.py and, for the key results,
independent routes (the topocentric helpers, exact derivatives by finite differences, symbolic Taylor expansions).

Run from the sfm-slides directory:   python3 codes/orbit-determination-verify.py
The Monte Carlo experiments use fixed seeds, so every number is reproducible.
"""
import sys
import numpy as np
import sympy as sp
sys.path.insert(0, "codes")
import _od as od
from _od import (gibbs, herrick_gibbs, lambert_velocity, laplace, laplace_core, gauss, lsq_angles, roots_poly8,
                 deriv_weights, truth_state, observe, noisy_radec, station, theta_g, lst, SITE, D2R, MU_E, OMEGA_E)
from _uv import propagate, coe, fg
from _odmc import crossover, lambert_vs_gibbs, noise_vs_arc, lsq_experiment
from _topo import observe as topo_observe, sez_matrix, R3, site_ecef, WE

RESULTS = []
ARCSEC = np.pi/(180*3600)


def check(name, value, stated, tol):
    ok = abs(value - stated) <= tol
    RESULTS.append(ok)
    print(f"{'PASS' if ok else 'FAIL'}  {name:<72} {value:18.10g}   page: {stated}")


def truth(name, cond):
    RESULTS.append(bool(cond))
    print(f"{'PASS' if cond else 'FAIL'}  {name}")


def show(name, value):
    print(f"----  {name:<72} {value}")


def unit(a, d):
    return np.array([np.cos(d)*np.cos(a), np.cos(d)*np.sin(a), np.sin(d)])


def round_to(x, q):
    return np.round(np.asarray(x)/q)*q


r0, v0 = truth_state()
el0 = coe(r0, v0)
n_mean = np.sqrt(MU_E/el0[0]**3)

# =====================================================================================================  the scenario
print("\n== The scenario")
check("omega_E in _od equals _topo", OMEGA_E, WE, 0.0)
show("truth elements a, e, i, Om, w, f", np.round(el0, 6))
show("period [min]", 2*np.pi/n_mean/60)
show("r0 [km]", np.round(r0, 4)); show("v0 [km/s]", np.round(v0, 6))
show("|r0|, |v0|", (np.linalg.norm(r0), np.linalg.norm(v0)))
show("perigee, apogee radius [km]", (el0[0]*(1 - el0[1]), el0[0]*(1 + el0[1])))
show("theta_G(0) [deg], LST(0) [deg]", (np.degrees(theta_g(0.0)) % 360, np.degrees(lst(0.0)) % 360))
Rs0, Rds0, Rdds0 = station(0.0)
show("station R(0) inertial [km]", np.round(Rs0, 4)); show("|R|", np.linalg.norm(Rs0))
show("station Rdot(0) [km/s]", np.round(Rds0, 6)); show("station Rddot(0) [km/s^2]", Rdds0)
# independent route: the topocentric helper reproduces azimuth, elevation, range
T = [-300, -60, 0, 60, 300]
OB = observe(T)
for o in OB:
    az, el, rng, *_ = topo_observe(o["r"], o["v"], theta_g(o["t"]), SITE["phi"], SITE["lam"], SITE["H"])
    truth(f"t={o['t']:5d}: _od.observe agrees with the topocentric note's observe (az, el, range)",
          abs(az - o["az"]) < 1e-9 and abs(el - o["el"]) < 1e-9 and abs(rng - o["range"]) < 1e-6)
    uu = R3(theta_g(o["t"])) @ sez_matrix(SITE["phi"], SITE["lam"]).T @ (np.array([-np.cos(el)*np.cos(az), np.cos(el)*np.sin(az), np.sin(el)]))
    truth(f"t={o['t']:5d}: line of sight from (A, el) by eq. radec equals the inertial one", np.linalg.norm(uu - o["u"]) < 1e-12)
for o in OB:
    print(f"      t={o['t']:5d}  range {o['range']:9.3f} km  az {np.degrees(o['az']):8.3f}  el {np.degrees(o['el']):7.3f}"
          f"  alpha {np.degrees(o['ra']):9.4f}  delta {np.degrees(o['dec']):8.4f}")
    print(f"             r = {np.round(o['r'], 3)}   v = {np.round(o['v'], 5)}")
# pass above 10 degrees
tt = np.arange(-900, 901, 1.0)
elv = np.degrees([x["el"] for x in observe(tt)])
show("maximum elevation [deg] and its time [s]", (elv.max(), tt[np.argmax(elv)]))
up = tt[elv > 10]
show("above 10 deg from, to [s]", (up[0], up[-1]))
show("above 0 deg from, to [s]", (tt[elv > 0][0], tt[elv > 0][-1]))

# =====================================================================================================  Gibbs
print("\n== Gibbs")
# the conic in vector form r + e.r = p
rr = [observe([t])[0]["r"] for t in (-300, 0, 300)]
hh = np.cross(r0, v0)
pp = hh @ hh/MU_E
ee = np.cross(v0, hh)/MU_E - r0/np.linalg.norm(r0)
for k, t in enumerate((-300, 0, 300)):
    rk = rr[k]
    check(f"r + e.r - p at t={t}", np.linalg.norm(rk) + ee @ rk - pp, 0.0, 1e-8)
c12, c23, c31 = np.cross(rr[0], rr[1]), np.cross(rr[1], rr[2]), np.cross(rr[2], rr[0])
ident = c23*rr[0] + c31*rr[1] + c12*rr[2]
check("coplanar identity (r2xr3) r1 + (r3xr1) r2 + (r1xr2) r3 = 0 [relative]", np.linalg.norm(ident)/np.linalg.norm(c12*rr[2]), 0.0, 1e-12)
nn_ = [np.linalg.norm(x) for x in rr]
Nv = nn_[0]*c23 + nn_[1]*c31 + nn_[2]*c12
Dv = c12 + c23 + c31
Sv = rr[0]*(nn_[1] - nn_[2]) + rr[1]*(nn_[2] - nn_[0]) + rr[2]*(nn_[0] - nn_[1])
check("N = p D  (p from the state) [relative]", np.linalg.norm(Nv - pp*Dv)/np.linalg.norm(Nv), 0.0, 1e-10)
check("D x e = S [relative]", np.linalg.norm(np.cross(Dv, ee) - Sv)/np.linalg.norm(Sv), 0.0, 1e-8)
check("p = |N|/|D| vs p of the truth [km]", np.linalg.norm(Nv)/np.linalg.norm(Dv), pp, 1e-6)
show("p, h-direction check (D parallel to h): cos angle", (Dv @ hh)/np.linalg.norm(Dv)/np.linalg.norm(hh))
v2g = gibbs(*rr)
check("noise-free Gibbs v2 error [km/s]", np.linalg.norm(v2g - v0), 0.0, 1e-9)
# the worked example: positions rounded to 1 m
rrr = [round_to(x, 1e-3) for x in rr]
show("rounded r1 [km]", rrr[0]); show("rounded r2 [km]", rrr[1]); show("rounded r3 [km]", rrr[2])
v2r, info = gibbs(*rrr, details=True)
show("Gibbs v2 from rounded [km/s]", np.round(v2r, 6))
show("truth v0", np.round(v0, 6))
show("|dv| [m/s]", 1e3*np.linalg.norm(v2r - v0))
show("|N|, |D|, |S|", (np.linalg.norm(info["N"]), np.linalg.norm(info["D"]), np.linalg.norm(info["S"])))
show("N [km^3]", info["N"]); show("D [km^2]", info["D"]); show("S [km]", info["S"])
show("p = |N|/|D| [km]", info["p"])
show("coplanarity (cos of angle between r1 and plane normal of r2,r3)", info["coplanarity"])
e_rec = (np.cross(info["S"], info["D"]))/(info["D"] @ info["D"])
show("e vector recovered |e|", np.linalg.norm(e_rec)); show("e true", np.linalg.norm(ee))
el_r = coe(rrr[1], v2r)
show("elements from (r2, v2): a e i Om w f", np.round(el_r, 5))
show("angle r1-r2, r2-r3, r1-r3 [deg]", [np.degrees(np.arccos(rr[i] @ rr[j]/np.linalg.norm(rr[i])/np.linalg.norm(rr[j]))) for i, j in ((0, 1), (1, 2), (0, 2))])
# time consistency check: time from r1 to r2 on the recovered orbit (Kepler)
fa = el_r[5]*D2R
def true_anomaly(r, v):
    return coe(r, v)[5]
F_, G_, *_ = fg(rrr[0], v2r, 300.0)
show("Kepler consistency: propagate (r2,v2) back by -300 s and compare with r1 [km]", np.linalg.norm(propagate(rrr[1], v2r, -300.0)[0] - rrr[0]))
show("... and forward by +300 s vs r3 [km]", np.linalg.norm(propagate(rrr[1], v2r, 300.0)[0] - rrr[2]))
show("rounding-free: same checks (km)", (np.linalg.norm(propagate(rr[1], v2g, -300.0)[0] - rr[0]), np.linalg.norm(propagate(rr[1], v2g, 300.0)[0] - rr[2])))
# time is not used
v2_alt = gibbs(*rrr)  # times never enter
show("Gibbs uses no times: (nothing to vary)", "-")

# =====================================================================================================  Herrick-Gibbs
print("\n== Herrick-Gibbs")
t1s, t3s, ts_ = sp.symbols("tau1 tau3 t", real=True)
cs = sp.symbols("c0:8")
rpoly = sum(cs[k]*ts_**k for k in range(8))                    # a generic polynomial history r(t) = sum c_k t^k
acc = sp.diff(rpoly, ts_, 2)                                    # its acceleration, which the gravity law supplies at the three times
v_true = sp.diff(rpoly, ts_).subs(ts_, 0)
quad = (t3s**2*(rpoly.subs(ts_, t1s) - rpoly.subs(ts_, 0)) - t1s**2*(rpoly.subs(ts_, t3s) - rpoly.subs(ts_, 0)))/(t1s*t3s*(t3s - t1s))
corr = (t3s*acc.subs(ts_, t1s) - (t1s + t3s)*acc.subs(ts_, 0) + t1s*acc.subs(ts_, t3s))/12
Pfull = sp.Poly(sp.expand(sp.cancel(sp.expand(quad + corr - v_true))), *cs)
Pquad = sp.Poly(sp.expand(sp.cancel(sp.expand(quad - v_true))), *cs)
truth("quadratic interpolation alone is exact for degree <= 2 (c0, c1, c2 drop out)", all(Pquad.coeff_monomial(cs[k]) == 0 for k in range(3)))
truth("quadratic interpolation: the error of c3 t^3 is -tau1 tau3 c3, i.e. -(tau1 tau3/6) r'''",
      sp.simplify(Pquad.coeff_monomial(cs[3]) + t1s*t3s) == 0)
truth("quadratic interpolation: the error of c4 t^4 is -tau1 tau3 (tau1 + tau3) c4, i.e. -(tau1 tau3 (tau1 + tau3)/24) r''''",
      sp.simplify(Pquad.coeff_monomial(cs[4]) + t1s*t3s*(t1s + t3s)) == 0)
truth("HG combination is exact for polynomials of degree <= 4 (c0 ... c4 drop out)", all(Pfull.coeff_monomial(cs[k]) == 0 for k in range(5)))
truth("HG leading error: c5 t^5 gives tau1 tau3 (2 tau1^2 - 3 tau1 tau3 + 2 tau3^2) c5/3",
      sp.simplify(Pfull.coeff_monomial(cs[5]) - t1s*t3s*(2*t1s**2 - 3*t1s*t3s + 2*t3s**2)/3) == 0)
Tsym = sp.Symbol("T", positive=True)
show("equal spacing tau1 = -T, tau3 = T: error coefficient of c5 / T^4", sp.simplify(Pfull.coeff_monomial(cs[5]).subs({t1s: -Tsym, t3s: Tsym})/Tsym**4))
# numeric example: +-10 s with 1 m rounding
rows = []
for tau in (10.0,):
    st = [observe([t])[0] for t in (-tau, 0.0, tau)]
    rp = [s["r"] for s in st]
    rpr = [round_to(x, 1e-3) for x in rp]
    for lab, pos in (("exact", rp), ("rounded 1 m", rpr)):
        vg = gibbs(*pos)
        vh = herrick_gibbs(*pos, -tau, 0.0, tau)
        show(f"tau={tau}, {lab}: Gibbs |dv| [m/s], HG |dv| [m/s]", (1e3*np.linalg.norm(vg - v0), 1e3*np.linalg.norm(vh - v0)))
    show("separation r1-r3 [deg]", np.degrees(np.arccos(rp[0] @ rp[2]/np.linalg.norm(rp[0])/np.linalg.norm(rp[2]))))
    show("rounded r1, r2, r3", [np.round(x, 3) for x in rpr])
    show("HG v2 (rounded) [km/s]", np.round(herrick_gibbs(*rpr, -tau, 0.0, tau), 6))
    show("Gibbs v2 (rounded) [km/s]", np.round(gibbs(*rpr), 6))
    for lab, pos in (("exact", rp),):
        c12_, c23_, c31_ = np.cross(pos[0], pos[1]), np.cross(pos[1], pos[2]), np.cross(pos[2], pos[0])
        D_ = c12_ + c23_ + c31_
        show("|D| for tau=10 [km^2], and |r1 x r2| [km^2]", (np.linalg.norm(D_), np.linalg.norm(c12_)))
        show("ratio |D|/|r1 x r2|", np.linalg.norm(D_)/np.linalg.norm(c12_))
        S_ = pos[0]*(np.linalg.norm(pos[1]) - np.linalg.norm(pos[2])) + pos[1]*(np.linalg.norm(pos[2]) - np.linalg.norm(pos[0])) + pos[2]*(np.linalg.norm(pos[0]) - np.linalg.norm(pos[1]))
        show("|S| [km^2*?]; |S|/(|r1| ...)", np.linalg.norm(S_))
# crossover experiment
print("   -- crossover experiment (noise = 10 m per coordinate, 400 trials, seed 11)")
XO = crossover()
for tau, sep, g0, h0, gm, hm in XO:
    print(f"      tau={tau:5.0f} s  sep={sep:7.2f} deg   noise-free: Gibbs {1e3*g0:10.3e} HG {1e3*h0:10.3e} m/s   "
          f"noisy median: Gibbs {1e3*gm:9.3f} HG {1e3*hm:9.3f} m/s")
def cross(x, y1, y2):
    d = np.log(y1) - np.log(y2)
    for k in range(len(x) - 1):
        if d[k]*d[k + 1] < 0:
            f = d[k]/(d[k] - d[k + 1])
            return np.exp(np.log(x[k]) + f*(np.log(x[k + 1]) - np.log(x[k])))
    return None
show("noise-free crossing at separation [deg]", cross(XO[:, 1], np.maximum(XO[:, 2], 1e-16), np.maximum(XO[:, 3], 1e-16)))
show("noisy crossing at separation [deg]", cross(XO[:, 1], XO[:, 4], XO[:, 5]))
kmin = np.argmin(XO[:, 5]); show("HG noisy minimum error [m/s] at separation [deg]", (1e3*XO[kmin, 5], XO[kmin, 1]))
k300 = list(XO[:, 0]).index(300.0)
show("noise-free HG error at tau=200 and 300 s (ratio should be 1.5^4 = 5.06)", (XO[k300 - 1, 3], XO[k300, 3]/XO[k300 - 1, 3]))


# =====================================================================================================  Lambert
print("\n== Lambert")
r_a, r_b = rr[0], rr[2]
va, vb = lambert_velocity(r_a, r_b, 600.0)
va_t, _ = (observe([-300])[0]["v"], None)
vb_t = observe([300])[0]["v"]
check("noise-free Lambert v1 error [km/s]", np.linalg.norm(va - va_t), 0.0, 1e-8)
check("noise-free Lambert v3 error [km/s]", np.linalg.norm(vb - vb_t), 0.0, 1e-8)
ra_r, rb_r = round_to(r_a, 1e-3), round_to(r_b, 1e-3)
va_r, vb_r = lambert_velocity(ra_r, rb_r, 600.0)
show("Lambert from rounded: |dv1| [m/s], |dv3| [m/s]", (1e3*np.linalg.norm(va_r - va_t), 1e3*np.linalg.norm(vb_r - vb_t)))
out = od.lambert_uv(ra_r, rb_r, 600.0)
show("Lambert z, transfer angle [deg]", (out["z"], np.degrees(out["dth"])))
r_mid, v_mid = propagate(ra_r, va_r, 300.0)
show("Lambert -> propagate to t=0: |dr| [km], |dv| [m/s]", (np.linalg.norm(r_mid - r0), 1e3*np.linalg.norm(v_mid - v0)))
show("Lambert v1 [km/s]", np.round(va_r, 6)); show("truth v1", np.round(va_t, 6))
show("rounded r_a, r_b", (np.round(ra_r, 3), np.round(rb_r, 3)))
# noise comparison with Gibbs (10 m noise), 400 trials
eLm, eGm = lambert_vs_gibbs()
show("noise 10 m, +-300 s: median |dv2| Lambert (propagated to t=0) [m/s], Gibbs [m/s]", (1e3*eLm, 1e3*eGm))


# =====================================================================================================  Laplace
print("\n== Laplace")
w1, w2 = deriv_weights(-60.0, 0.0, 60.0)
show("derivative weights (+-60 s): first", w1); show("second", w2)
ft = lambda t: 1 + 2*t + 3*t**2
tt3 = np.array([-60.0, 0.0, 60.0])
check("weights exact on a quadratic: first derivative", w1 @ ft(tt3), 2.0, 1e-12)
check("weights exact on a quadratic: second derivative", w2 @ ft(tt3), 6.0, 1e-12)
# exact derivatives of the line of sight at t=0 by a 9-point finite-difference stencil
def L_of(t):
    return observe([t])[0]["u"]
hh_ = 0.5
st9 = np.arange(-4, 5)*hh_
Ls = np.array([L_of(s) for s in st9])
c1d = np.array([1/280, -4/105, 1/5, -4/5, 0, 4/5, -1/5, 4/105, -1/280])/hh_
c2d = np.array([-1/560, 8/315, -1/5, 8/5, -205/72, 8/5, -1/5, 8/315, -1/560])/hh_**2
Ld_ex, Ldd_ex = c1d @ Ls, c2d @ Ls
o0 = observe([0.0])[0]
r2e, v2e, de = laplace_core(o0["u"], Ld_ex, Ldd_ex, o0["R"], o0["Rd"], o0["Rdd"])
check("Laplace with EXACT derivatives: position error [km]", np.linalg.norm(r2e - r0), 0.0, 1e-5)
check("Laplace with EXACT derivatives: velocity error [km/s]", np.linalg.norm(v2e - v0), 0.0, 1e-8)
show("positive roots with exact derivatives", de["roots"])
# the two relations of the derivation, at the truth
D_ = o0["u"] @ np.cross(Ld_ex, Ldd_ex)
n_ = np.cross(o0["u"], Ld_ex)
rho_t, rr_t = o0["range"], np.linalg.norm(r0)
check("rho = A + B/r^3 at the truth [km]", de["A"] + de["B"]/rr_t**3, rho_t, 1e-5)
check("r^2 = rho^2 + 2 C rho + R^2 at the truth [km^2]", rho_t**2 + 2*de["C"]*rho_t + o0["R"] @ o0["R"], rr_t**2, 1e-3)
check("polynomial vanishes at r = |r2| (relative)", (rr_t**8 + de["c6"]*rr_t**6 + de["c3"]*rr_t**3 + de["c0"])/rr_t**8, 0.0, 1e-9)
# worked example: +-60 s, RA/Dec rounded to 1e-6 deg
TW = [-60.0, 0.0, 60.0]
obw = observe(TW)
ra_w = round_to(np.degrees([o["ra"] for o in obw]), 1e-6); de_w = round_to(np.degrees([o["dec"] for o in obw]), 1e-6)
show("RA [deg] (rounded)", ra_w); show("Dec [deg] (rounded)", de_w)
Lw = [unit(ra_w[i]*D2R, de_w[i]*D2R) for i in range(3)]
Rw = [o["R"] for o in obw]
show("R_i [km]", [np.round(x, 3) for x in Rw])
show("Rdot_2 [km/s]", np.round(obw[1]["Rd"], 6)); show("Rddot_2 [km/s^2]", obw[1]["Rdd"])
r2l, v2l, dl = laplace(Lw, Rw, obw[1]["Rd"], obw[1]["Rdd"], TW, details=True)
show("L1, L2, L3 (rounded RA, Dec)", [np.round(x, 6) for x in Lw])
show("Ldot, Lddot [1/s, 1/s^2]", (dl["Ld"], dl["Ldd"]))
show("D, D1, D2", (dl["D"], dl["D1"], dl["D2"]))
show("A [km], B [km^4... = km^4?], C [km]", (dl["A"], dl["B"], dl["C"]))
show("c6, c3, c0", (dl["c6"], dl["c3"], dl["c0"]))
show("positive real roots [km]", dl["roots"])
show("all roots (complex)", np.sort_complex(np.roots([1, 0, dl["c6"], 0, 0, dl["c3"], 0, 0, dl["c0"]])))
show("Descartes: sign changes in (+, c6, c3, c0)", int(np.sum(np.diff(np.sign([1, dl["c6"], dl["c3"], dl["c0"]])) != 0)))
show("rho [km], rhodot [km/s]", (dl["rho"], dl["rhod"]))
show("true rho, rhodot", (o0["range"], (o0["rho"] @ (o0["v"] - o0["Rd"]))/o0["range"]))
show("r2 [km], v2 [km/s]", (np.round(r2l, 3), np.round(v2l, 5)))
show("|dr| [km], |dv| [m/s]", (np.linalg.norm(r2l - r0), 1e3*np.linalg.norm(v2l - v0)))
show("|r2| Laplace vs truth", (np.linalg.norm(r2l), np.linalg.norm(r0)))
el_l = coe(r2l, v2l)
show("Laplace elements a e i Om w f", np.round(el_l, 4))
# all roots of the three scenarios: how many positive roots in general?
for h in (10, 60, 120, 240):
    ob_ = observe([-h, 0, h])
    Lh_ = [o["u"] for o in ob_]; Rh_ = [o["R"] for o in ob_]
    _, _, d_ = laplace(Lh_, Rh_, ob_[1]["Rd"], ob_[1]["Rdd"], [-h, 0, h], details=True)
    show(f"h={h}: positive roots, c3 sign, error in r2 [km], in v2 [m/s]",
         (d_["roots"], np.sign(d_["c3"]), np.linalg.norm(laplace(Lh_, Rh_, ob_[1]["Rd"], ob_[1]["Rdd"], [-h, 0, h])[0] - r0),
          1e3*np.linalg.norm(laplace(Lh_, Rh_, ob_[1]["Rd"], ob_[1]["Rdd"], [-h, 0, h])[1] - v0)))
# the two curves of the figure: rho = A + B/r^3 and rho = -C + sqrt(r^2 - R^2 + C^2)
Rn2 = Rw[1] @ Rw[1]
show("sqrt branch at r_root: rho(geometry)", -dl["C"] + np.sqrt(dl["r"]**2 - Rn2 + dl["C"]**2))

# =====================================================================================================  Gauss
print("\n== Gauss")
tw1, tw3 = -60.0, 60.0
tauw = tw3 - tw1
# exact F, G between the middle and the outer observations
F1e, G1e, *_ = fg(r0, v0, tw1); F3e, G3e, *_ = fg(r0, v0, tw3)
den_e = F1e*G3e - F3e*G1e
c1e, c3e = G3e/den_e, -G1e/den_e
r1t, r3t = obw[0]["r"], obw[2]["r"]
check("r1 = F1 r2 + G1 v2 (error [km])", np.linalg.norm(F1e*r0 + G1e*v0 - r1t), 0.0, 1e-7)
check("r2 = c1 r1 + c3 r3 (error [km])", np.linalg.norm(c1e*r1t + c3e*r3t - r0), 0.0, 1e-7)
check("F1 G3 - F3 G1 vs tau (1 - u tau^2/6 + ...)", den_e/tauw, 1 - MU_E/np.linalg.norm(r0)**3*tauw**2/6, 5e-6)
u_ = MU_E/np.linalg.norm(r0)**3
show("u = mu/r^3 [1/s^2]", u_)
show("exact c1, c3", (c1e, c3e)); show("series c1, c3", (tw3/tauw*(1 + u_/6*(tauw**2 - tw3**2)), -tw1/tauw*(1 + u_/6*(tauw**2 - tw1**2))))
# range formulas with exact c1, c3 at the truth: check the three rho_i
Lt = [o["u"] for o in obw]
p1, p2, p3 = np.cross(Lt[1], Lt[2]), np.cross(Lt[0], Lt[2]), np.cross(Lt[0], Lt[1])
D0 = Lt[0] @ p1
Dm = np.array([[Rw[i] @ p for p in (p1, p2, p3)] for i in range(3)])
rho1 = (-Dm[0, 0] + Dm[1, 0]/c1e - c3e*Dm[2, 0]/c1e)/D0
rho2 = (-c1e*Dm[0, 1] + Dm[1, 1] - c3e*Dm[2, 1])/D0
rho3 = (-c1e*Dm[0, 2]/c3e + Dm[1, 2]/c3e - Dm[2, 2])/D0
check("rho1 from the Cramer formula with exact c1, c3", rho1, obw[0]["range"], 1e-6)
check("rho2", rho2, obw[1]["range"], 1e-6)
check("rho3", rho3, obw[2]["range"], 1e-6)
check("L2 . (L1 x L3) sign: D0 equals L1 . (L2 x L3)", D0, Lt[1] @ np.cross(Lt[2], Lt[0]), 1e-15)
# worked example
r2g, v2g_, dg = gauss(Lw, Rw, TW, details=True)
show("D0 (rounded data)", dg["D0"])
show("D_ij matrix", np.round(dg["Dm"], 6))
show("A [km], B [s^2], E=R2.L2 [km]", (dg["A"], dg["B"], dg["E"]))
show("c6, c3, c0", (dg["c6"], dg["c3"], dg["c0"]))
show("positive roots [km]", dg["roots"])
show("Descartes sign changes", int(np.sum(np.diff(np.sign([1, dg["c6"], dg["c3"], dg["c0"]])) != 0)))
f0 = dg["first"]
show("first pass: c1, c3", (f0["c1"], f0["c3"]))
show("first pass: rho1, rho2, rho3 [km]", f0["rho"])
show("first pass: r2 [km], v2 [km/s]", (np.round(f0["r2"], 3), np.round(f0["v2"], 5)))
show("first pass: |dr| [km], |dv| [m/s]", (np.linalg.norm(f0["r2"] - r0), 1e3*np.linalg.norm(f0["v2"] - v0)))
show("true ranges", [o["range"] for o in obw])
errs = [(np.linalg.norm(h_[1] - r0), 1e3*np.linalg.norm(h_[2] - v0), h_[0][1]) for h_ in dg["hist"]]
for k, (er, ev, rh) in enumerate(errs[:15]):
    print(f"      iteration {k:2d}:  rho2 = {rh:12.6f} km   |dr| = {er:11.5f} km   |dv| = {ev:10.5f} m/s")
show("iterations to converge (tol 1e-12 in rho)", dg["iterations"])
show("final |dr| [km], |dv| [m/s]", (np.linalg.norm(r2g - r0), 1e3*np.linalg.norm(v2g_ - v0)))
show("final r2, v2", (np.round(r2g, 4), np.round(v2g_, 6)))
el_g = coe(r2g, v2g_)
show("Gauss iterated elements a e i Om w f", np.round(el_g, 4))
e_arr = np.array([e[0] for e in errs])
ratios = e_arr[1:]/e_arr[:-1]
show("error ratio per iteration (later iterations)", np.round(ratios[8:16], 4))
# Why linear convergence: the rate is about (mu/r^3) tau^2 /something
show("u tau1^2 and u tau^2/6", (u_*tw1**2, u_*tauw**2/6))
# same, noise-free data, several arcs
for h in (10, 60, 120, 240, 480):
    ob_ = observe([-h, 0, h])
    Lh_ = [o["u"] for o in ob_]; Rh_ = [o["R"] for o in ob_]
    ra_, va_, da_ = gauss(Lh_, Rh_, [-h, 0, h], details=True)
    show(f"noise-free h={h}: first pass |dr| [km], iterated |dr| [km], iterations, D0",
         (np.linalg.norm(da_["first"]["r2"] - r0), np.linalg.norm(ra_ - r0), da_["iterations"], da_["D0"]))

# =====================================================================================================  noise and arc
print("\n== Noise and arc length (2 arcsec, 300 trials, seed 20261014)")
NA = noise_vs_arc()
for row in NA:
    print(f"      h={row['h']:4d} s  D0={row['D0']:9.3e}  out-of-plane {row['dev']:10.2f}\"  sep {row['sep']:7.2f} deg")
    for lab, key in (("Laplace", "laplace"), ("Gauss 1st", "gauss1"), ("Gauss it.", "gauss")):
        q = row[key]
        print(f"           |dr| {lab:<10} {q[0]:9.3f} {q[1]:9.3f} {q[2]:9.3f} km")
    print(f"           |dv| Gauss it. median {1e3*row['vgauss'][1]:9.3f} m/s")
show("2 arcsec at the range of the middle observation [m]", 2*ARCSEC*obw[1]["range"]*1e3)
show("D0 scaling exponent between h=10 and h=60 (should be 3)", np.log(NA[3]["D0"]/NA[0]["D0"])/np.log(6))
show("out-of-plane deviation at h = 10 s vs noise 2 arcsec: ratio", NA[0]["dev"]/2)
show("noise-free Laplace error growth h=120 -> 240 (ratio, 4 if ~h^2)", 407.62103452564486/127.80415766985344)


# overhead pass vs this pass
print("   -- pass geometry: D0 at +-60 s for different offsets (cross-track) of the sub-satellite point")
th0 = lst(0.0)
def truth_off(off_deg, head_deg=50.0, rmag=7600.0, gam_deg=3.0, a=7800.0):
    head, off = head_deg*D2R, off_deg*D2R
    lat = SITE["phi"] + off*np.cos(head - np.pi/2); dlon = off*np.sin(head - np.pi/2)/np.cos(lat)
    gam = gam_deg*D2R
    u = np.array([np.cos(lat)*np.cos(th0 + dlon), np.cos(lat)*np.sin(th0 + dlon), np.sin(lat)])
    E = np.array([-np.sin(th0 + dlon), np.cos(th0 + dlon), 0.0]); N = np.cross(u, E)
    v = np.sqrt(MU_E*(2/rmag - 1/a)); horiz = np.cos(head)*N + np.sin(head)*E
    return rmag*u, v*(np.cos(gam)*horiz + np.sin(gam)*u)
for off in (0.0, 1.0, 2.0, 5.0, 10.0, 15.0):
    rA, vA = truth_off(off)
    ob_ = observe([-60.0, 0.0, 60.0], rA, vA)
    L_ = [o["u"] for o in ob_]
    D0_ = L_[0] @ np.cross(L_[1], L_[2])
    # distance of the station from the orbital plane [km]
    hvec = np.cross(rA, vA); hvec /= np.linalg.norm(hvec)
    el_max = max(np.degrees(o["el"]) for o in observe(np.arange(-120, 121, 10.0), rA, vA))
    print(f"      offset {off:5.1f} deg: max el (in +-120 s) {el_max:6.2f}  D0 {D0_:11.4e}   station-to-plane distance {abs(station(0.0)[0] @ hvec):8.1f} km")

# =====================================================================================================  least squares
print("\n== Least squares (2 arcsec)")
LS = lsq_experiment()
demo = LS["demo"]
tsl = demo["ts"]; hist = demo["hist"]; xl = demo["x"]; Pl = demo["P"]; B_ = demo["B"]
show("number of observations; arc [s]", (len(tsl), tsl[-1] - tsl[0]))
x0 = demo["start"]
show("start: three obs at (-10, 0, +10 s): |dr| [km], |dv| [m/s]", (np.linalg.norm(x0[:3] - r0), 1e3*np.linalg.norm(x0[3:] - v0)))
for k, (xh, rms) in enumerate(hist):
    print(f"      iteration {k}:  RMS residual {rms/ARCSEC:12.4f}\"   |dr| {np.linalg.norm(xh[:3] - r0):10.4f} km   |dv| {1e3*np.linalg.norm(xh[3:] - v0):10.4f} m/s")
show("final |dr| [km], |dv| [m/s]", (np.linalg.norm(xl[:3] - r0), 1e3*np.linalg.norm(xl[3:] - v0)))
Pr = B_ @ Pl[:3, :3] @ B_.T
sg = np.sqrt(np.diag(Pr))
show("formal sigma along LOS, cross 1, cross 2 [km]", sg)
show("formal sigma_v (rms of 3) [m/s]", 1e3*np.sqrt(np.trace(Pl[3:, 3:])/3))
show("formal sigma_r (sqrt trace) [km]", np.sqrt(np.trace(Pl[:3, :3])))
show("actual error in the LOS frame [km]", B_ @ (xl[:3] - r0))
print("   -- Monte Carlo, 200 trials, seed 32")
show("IOD (first, middle, last obs): median |dr| [km], median |dv| [m/s]", (np.median(LS["iod"]), 1e3*np.median(LS["iod_v"])))
show("LSQ (all 49): median |dr| [km], median |dv| [m/s]", (np.median(LS["lsq"]), 1e3*np.median(LS["lsq_v"])))
show("sample std of the LSQ error along LOS, cross1, cross2 [km]", LS["los"].std(axis=0))
show("formal sigma (same frame) [km]", sg)
show("RMS residual / sigma (final iterate of the illustrative run)", hist[-1][1]/(2*ARCSEC))
show("expected RMS/sigma for 98 residuals, 6 parameters", np.sqrt((98 - 6)/98))

# =====================================================================================================  J2, zenith pass, Ceres
print("\n== The two-body model and a zenith pass")
from scipy.integrate import solve_ivp
J2, RE_ = 1.08262668e-3, 6378.137
def f_j2(t, y):
    r = y[:3]; x, yy, z = r; rn = np.linalg.norm(r)
    k = 1.5*J2*MU_E*RE_**2/rn**5
    a = -MU_E*r/rn**3 + k*np.array([x*(5*z*z/rn**2 - 1), yy*(5*z*z/rn**2 - 1), z*(5*z*z/rn**2 - 3)])
    return np.concatenate([y[3:], a])
j2err = {}
for T in (-480, -240, 240, 480):
    sol = solve_ivp(f_j2, [0, T], np.concatenate([r0, v0]), rtol=1e-12, atol=1e-12)
    j2err[T] = 1e3*np.linalg.norm(sol.y[:3, -1] - propagate(r0, v0, T)[0])
    show(f"J2 minus Kepler over {T} s: |dr| [m]", j2err[T])
check("J2 displacement over +-240 s (mean of the two ends) [m]", 0.5*(j2err[-240] + j2err[240]), 210, 1)
check("J2 displacement over +-480 s (mean of the two ends) [m]", 0.5*(j2err[-480] + j2err[480]), 840, 5)
# the zenith pass
rz, vz = truth_state(0.0)
obz = observe([-60.0, 0.0, 60.0], rz, vz)
Lz = [o["u"] for o in obz]; Rz = [o["R"] for o in obz]
rzg, vzg, dzg = gauss(Lz, Rz, [-60.0, 0.0, 60.0], details=True)
show("zenith pass (offset 0): D0, exact-data iterated Gauss error [km]", (dzg["D0"], np.linalg.norm(rzg - rz)))
rng = np.random.default_rng(77)
ez, ek = [], []
rk, vk = truth_state()
obk = observe([-60.0, 0.0, 60.0], rk, vk)
for _ in range(300):
    ra_, dec_ = noisy_radec(obz, 2*ARCSEC, rng)
    try:
        rg_, _ = gauss([unit(ra_[i], dec_[i]) for i in range(3)], Rz, [-60.0, 0.0, 60.0]); ez.append(np.linalg.norm(rg_ - rz))
    except Exception:
        ez.append(np.inf)
    ra_, dec_ = noisy_radec(obk, 2*ARCSEC, rng)
    rg_, _ = gauss([unit(ra_[i], dec_[i]) for i in range(3)], [o["R"] for o in obk], [-60.0, 0.0, 60.0]); ek.append(np.linalg.norm(rg_ - rk))
show("zenith pass: median error with 2 arcsec noise [km] (300 trials), same for the 60 deg pass", (np.median(ez), np.median(ek)))
show("zenith pass: 84th percentile [km]", np.percentile(ez, 84))
check("zenith pass: exact-data error [km]", np.linalg.norm(rzg - rz), 4e-9, 2e-9)
check("zenith pass: median noisy error [km]", np.median(ez), 123, 0.5)
check("60 deg pass: median noisy error, table of the noise section [km]", NA[3]["gauss"][1], 2.0, 0.1)
check("ratio of the zenith pass to the table value", np.median(ez)/NA[3]["gauss"][1], 63, 1.0)
# the first terms of F, G: radial velocity term
check("v_r / r at t = 0 [1/s] (quoted 5e-5)", (r0 @ v0)/np.linalg.norm(r0)**2, 5e-5, 5e-6)
# exact vs series determinant (exercise 9)
tau9 = 60.0
F1_, G1_, *_ = fg(r0, v0, -tau9); F3_, G3_, *_ = fg(r0, v0, tau9)
check("(F1 G3 - F3 G1)/tau exact", (F1_*G3_ - F3_*G1_)/(2*tau9), 0.99782, 5e-6)
check("   series 1 - u tau^2/6, tau = 120 s", 1 - MU_E/np.linalg.norm(r0)**3*(2*tau9)**2/6, 0.99782, 5e-6)
check("u = mu/r^3 [1/s^2]", MU_E/np.linalg.norm(r0)**3, 9.08e-7, 5e-10)
check("2 arcsec across 2410 km [m]", 2*ARCSEC*2410.785e3, 23.4, 0.05)

# =====================================================================================================  numbers quoted on the page
print("\n== Numbers quoted on the page")
# scenario
check("a [km]", el0[0], 7800, 1e-6); check("e", el0[1], 0.05826, 5e-6); check("i [deg]", el0[2], 43.081, 5e-4)
check("Omega [deg]", el0[3], 290.224, 5e-4); check("omega [deg]", el0[4], 319.269, 5e-4); check("f(0) [deg]", el0[5], 66.929, 5e-4)
check("period [min]", 2*np.pi/n_mean/60, 114.3, 0.05)
check("perigee radius", el0[0]*(1 - el0[1]), 7345.5, 0.05); check("apogee radius", el0[0]*(1 + el0[1]), 8254.5, 0.05)
check("theta_G(0) [deg]", np.degrees(theta_g(0.0)) % 360, 233.131, 5e-4)
for a_, b_, c_ in zip(r0, (4656.838, -5551.747, 2291.717), range(3)):
    check(f"r(0) component {c_}", a_, b_, 5e-4)
for a_, b_, c_ in zip(v0, (3.621537, 4.413025, 4.604554), range(3)):
    check(f"v(0) component {c_}", a_, b_, 5e-7)
check("max elevation [deg]", elv.max(), 60.6, 0.05); check("time of max [s]", tt[np.argmax(elv)], -2, 0.5)
check("above 10 deg: duration [s]", up[-1] - up[0], 927, 0.5)
tab = {-300: (2269.593, 245.333, 20.887, 249.4397, -17.1113), -60: (1394.200, 288.152, 56.399, 278.7186, 21.4097),
       0: (1370.344, 320.697, 60.603, 291.0129, 35.1436), 60: (1449.817, 351.649, 56.185, 306.7986, 47.0505),
       300: (2410.785, 30.931, 25.355, 18.0587, 58.7162)}
for o in OB:
    st = tab[int(o["t"])]
    got = (o["range"], np.degrees(o["az"]), np.degrees(o["el"]), np.degrees(o["ra"]), np.degrees(o["dec"]))
    for lab, g, q, tol in zip(("range", "az", "el", "alpha", "delta"), got, st, (5e-4, 5e-4, 5e-4, 5e-5, 5e-5)):
        check(f"table t={int(o['t'])}: {lab}", g, q, tol)
# Gibbs, Example 1
ang = [np.degrees(np.arccos(rr[i] @ rr[j]/np.linalg.norm(rr[i])/np.linalg.norm(rr[j]))) for i, j in ((0, 1), (1, 2), (0, 2))]
for g, q in zip(ang, (16.81, 16.31, 33.12)):
    check("Gibbs example: angle [deg]", g, q, 5e-3)
check("Gibbs example: coplanarity measure", info["coplanarity"], -1.3e-7, 5e-9)
check("coplanarity angle [deg]", np.degrees(info["coplanarity"]), -7e-6, 5e-7)
for g, q in zip(info["N"], (-6.66000e9, -2.45350e9, 7.58961e9)): check("N component [km^3]", g, q, 5e3)
for g, q in zip(info["D"], (-856755.6, -315622.4, 976342.4)): check("D component [km^2]", g, q, 0.1)
for g, q in zip(info["S"], (58016.6, -32787.0, 40311.4)): check("S component [km^2]", g, q, 0.1)
check("|N| [km^3]", np.linalg.norm(info["N"]), 1.03912e10, 5e4)
check("|D| [km^2]", np.linalg.norm(info["D"]), 1.33675e6, 5)
check("|S| [km^2]", np.linalg.norm(info["S"]), 7.7884e4, 0.5)
check("p [km]", info["p"], 7773.515, 5e-4); check("p true [km]", pp, 7773.521, 5e-4)
check("e from S x D / D^2", np.linalg.norm(e_rec), 0.058264, 5e-7)
for g, q in zip(v2r, (3.621536, 4.413022, 4.604552)): check("Gibbs v2 component [km/s]", g, q, 5e-7)
check("Gibbs example: |dv| [m/s]", 1e3*np.linalg.norm(v2r - v0), 0.0030, 5e-5)
for g, q in zip(el_r, (7799.994, 0.05826, 43.0811, 290.2235, 319.2679, 66.9301)):
    check("Gibbs example: element", g, q, 6e-4 if q > 100 else 6e-5)
check("Kepler consistency, back [m]", 1e3*np.linalg.norm(propagate(rrr[1], v2r, -300.0)[0] - rrr[0]), 1.2, 0.05)
check("Kepler consistency, forward [m]", 1e3*np.linalg.norm(propagate(rrr[1], v2r, 300.0)[0] - rrr[2]), 1.5, 0.05)
check("cos(angle D, h)", (info["D"] @ hh)/np.linalg.norm(info["D"])/np.linalg.norm(hh), 1.0, 1e-9)
# Herrick-Gibbs, Example 2
tau = 10.0
rp10 = [round_to(observe([t])[0]["r"], 1e-3) for t in (-tau, 0.0, tau)]
vg10, vh10 = gibbs(*rp10), herrick_gibbs(*rp10, -tau, 0.0, tau)
for g, q in zip(vg10, (3.620416, 4.411655, 4.603080)): check("Gibbs v2 at +-10 s [km/s]", g, q, 5e-7)
for g, q in zip(vh10, (3.621544, 4.413030, 4.604514)): check("HG v2 at +-10 s [km/s]", g, q, 5e-7)
check("Gibbs error at +-10 s [m/s]", 1e3*np.linalg.norm(vg10 - v0), 2.30, 0.005)
check("HG error at +-10 s [m/s]", 1e3*np.linalg.norm(vh10 - v0), 0.040, 5e-4)
check("angle r1-r3 at +-10 s [deg]", np.degrees(np.arccos(rp10[0] @ rp10[2]/np.linalg.norm(rp10[0])/np.linalg.norm(rp10[2]))), 1.10, 0.005)
rpx = [observe([t])[0]["r"] for t in (-tau, 0.0, tau)]
Dx = np.cross(rpx[0], rpx[1]) + np.cross(rpx[1], rpx[2]) + np.cross(rpx[2], rpx[0])
check("|D|/|r1 x r2| at +-10 s", np.linalg.norm(Dx)/np.linalg.norm(np.cross(rpx[0], rpx[1])), 9e-5, 1e-6)
check("psi^2 for the same (psi = half the separation)", np.radians(0.5521702971)**2, 9e-5, 5e-6)
check("noise-free crossing [deg]", cross(XO[:, 1], np.maximum(XO[:, 2], 1e-16), np.maximum(XO[:, 3], 1e-16)), 0.74, 0.005)
check("noisy crossing [deg]", cross(XO[:, 1], XO[:, 4], XO[:, 5]), 21, 0.5)
check("HG best noisy error [m/s]", 1e3*XO[kmin, 5], 0.09, 0.005); check("   at separation [deg]", XO[kmin, 1], 13, 0.5)
check("Gibbs noisy error at 33 deg [m/s]", 1e3*XO[list(XO[:, 0]).index(300.0), 4], 0.10, 0.005)
check("Gibbs noisy error at 99 deg [m/s]", 1e3*XO[list(XO[:, 0]).index(900.0), 4], 0.02, 0.005)
# Lambert, Example 3
check("Lambert: transfer angle [deg]", np.degrees(out["dth"]), 33.117, 5e-4); check("Lambert: z", out["z"], 0.3184, 5e-5)
check("Lambert dv1 [m/s]", 1e3*np.linalg.norm(va_r - va_t), 0.0011, 5e-5); check("Lambert dv3 [m/s]", 1e3*np.linalg.norm(vb_r - vb_t), 0.0010, 5e-5)
check("Lambert -> t=0 dr [m]", 1e3*np.linalg.norm(r_mid - r0), 0.33, 0.005); check("Lambert -> t=0 dv [m/s]", 1e3*np.linalg.norm(v_mid - v0), 0.0011, 5e-5)
for g, q in zip(va_r, (4.748842, 2.704460, 5.041449)): check("Lambert v1 component", g, q, 5e-7)
for g, q in zip(va_t, (4.748841, 2.704460, 5.041448)): check("true v1 component", g, q, 5e-7)
check("Lambert noisy median [m/s]", 1e3*eLm, 0.037, 5e-4); check("Gibbs noisy median [m/s]", 1e3*eGm, 0.11, 5e-3)
check("time error 1 ms along track [m]", np.linalg.norm(v0)*1e-3*1e3, 7.3, 0.05)
# Laplace, Example 4
for g, q in zip(ra_w, (278.718649, 291.012940, 306.798619)): check("RA [deg]", g, q, 5e-7)
for g, q in zip(de_w, (21.409699, 35.143647, 47.050460)): check("Dec [deg]", g, q, 5e-7)
for i_, qq in enumerate(((4235.280, -4524.291, 1502.908), (4255.034, -4505.718, 1502.908), (4274.707, -4487.058, 1502.908))):
    for g, q in zip(Rw[i_], qq): check(f"R_{i_ + 1} component", g, q, 5e-4)
for g, q in zip(obw[1]["Rd"], (0.328562, 0.310282, 0.0)): check("Rdot component", g, q, 5e-7)
for g, q in zip(obw[1]["Rdd"], (-2.2626e-5, 2.3959e-5, 0.0)): check("Rddot component", g, q, 5e-10)
for i_, qq in enumerate(((0.141122, -0.920236, 0.365034), (0.293214, -0.763333, 0.575628), (0.408134, -0.545591, 0.731954))):
    for g, q in zip(Lw[i_], qq): check(f"L_{i_ + 1} component", g, q, 5e-7)
for g, q in zip(dl["Ld"], (2.2251e-3, 3.1220e-3, 3.0577e-3)): check("Ldot component", g, q, 5e-8)
for g, q in zip(dl["Ldd"], (-1.0325e-5, 1.6900e-5, -1.5075e-5)): check("Lddot component", g, q, 5e-10)
check("Laplace D", dl["D"], 9.7466e-9, 5e-13); check("D1", dl["D1"], 1.0268e-7, 5e-11); check("D2", dl["D2"], -15.381, 5e-4)
check("A [km]", dl["A"], -10.535, 5e-4); check("B [km^4] / 1e14", dl["B"]/1e14, 6.2903, 5e-5); check("C [km]", dl["C"], 5552.115, 5e-4)
check("c6 / 1e7", dl["c6"]/1e7, -4.0549, 5e-5); check("c3 / 1e18", dl["c3"]/1e18, -6.9717, 5e-5); check("c0 / 1e29", dl["c0"]/1e29, -3.9568, 5e-5)
check("number of positive roots (Laplace example)", len(dl["roots"]), 1, 0); check("Laplace root [km]", dl["r"], 7631.402, 5e-4)
check("number of real negative roots", int(np.sum((np.abs(np.imag(np.roots([1, 0, dl["c6"], 0, 0, dl["c3"], 0, 0, dl["c0"]]))) < 1e-6) & (np.real(np.roots([1, 0, dl["c6"], 0, 0, dl["c3"], 0, 0, dl["c0"]])) < 0))), 1, 0)
check("rho [km]", dl["rho"], 1404.806, 5e-4); check("rhodot [km/s]", dl["rhod"], 0.45780, 5e-6)
for g, q in zip(r2l, (4666.943, -5578.052, 2311.554)): check("Laplace r2 component", g, q, 5e-4)
for g, q in zip(v2l, (3.58862, 4.34669, 4.55894)): check("Laplace v2 component", g, q, 5e-6)
check("Laplace |dr| [km]", np.linalg.norm(r2l - r0), 34.5, 0.05); check("Laplace |dv| [m/s]", 1e3*np.linalg.norm(v2l - v0), 87, 0.5)
check("Laplace elements a", el_l[0], 7679, 0.5); check("   e", el_l[1], 0.0553, 5e-5)
check("true rhodot", (o0["rho"] @ (o0["v"] - o0["Rd"]))/o0["range"], 0.48430, 5e-6)
check("angle between L1 and L3 [deg]", np.degrees(np.arccos(Lw[0] @ Lw[2])), 34, 0.5)
check("Laplace at +-10 s: |dr| [km]", np.linalg.norm(laplace([o["u"] for o in observe([-10, 0, 10])], [o["R"] for o in observe([-10, 0, 10])], o0["Rd"], o0["Rdd"], [-10, 0, 10])[0] - r0), 1.0, 0.02)
r_t = 7600.0
check("geometry rho at r = 7600 [km]", -dl["C"] + np.sqrt(r_t**2 - Rn2 + dl["C"]**2), 1370.3, 0.05)
check("dynamics rho at r = 7600 [km]", dl["A"] + dl["B"]/r_t**3, 1422.4, 0.05)
# Gauss, Example 5
check("D0", dg["D0"], 2.1053e-3, 5e-8)
for i_ in range(3):
    for j_, q in enumerate(((-900.350, -1767.223, -936.454), (-904.805, -1775.746, -940.934), (-909.240, -1784.226, -945.392))[i_]):
        check(f"D_{i_ + 1}{j_ + 1}", dg["Dm"][i_, j_], q, 5e-4)
check("A [km]", dg["A"], -10.114, 5e-4); check("B [1e9 km s^2]", dg["B"]/1e9, 1.5182, 5e-5)
check("c6 / 1e7", dg["c6"]/1e7, -4.0553, 5e-5); check("c3 / 1e18", dg["c3"]/1e18, -6.7077, 5e-5); check("c0 / 1e29", dg["c0"]/1e29, -3.6623, 5e-5)
check("number of positive roots (Gauss example)", len(dg["roots"]), 1, 0); check("Gauss root [km]", dg["roots"][-1], 7598.869, 5e-4)
check("first pass c1", f0["c1"], 0.5008176, 5e-8); check("first pass c3", f0["c3"], 0.5008176, 5e-8)
for g, q in zip(f0["rho"], (1392.943, 1369.103, 1448.501)): check("first pass rho", g, q, 5e-4)
for g, q in zip(f0["r2"], (4656.474, -5550.799, 2291.002)): check("first pass r2 component", g, q, 5e-4)
for g, q in zip(f0["v2"], (3.61893, 4.40890, 4.60054)): check("first pass v2 component", g, q, 5e-6)
check("first pass |dr| [km]", np.linalg.norm(f0["r2"] - r0), 1.24, 0.005); check("first pass |dv| [m/s]", 1e3*np.linalg.norm(f0["v2"] - v0), 6.3, 0.05)
for k_, (rho_q, dr_q, dv_q) in {0: (1369.1027, 1.2416, 6.315), 1: (1370.9636, 0.6193, 3.156), 2: (1370.0353, 0.3090, 1.575), 4: (1370.2674, 0.0770, 0.392),
                                 8: (1370.3395, 0.0049, 0.025), 12: (None, None, None)}.items():
    if rho_q is None:
        continue
    check(f"iteration {k_}: rho2", errs[k_][2], rho_q, 5e-5); check(f"iteration {k_}: |dr|", errs[k_][0], dr_q, 5e-5); check(f"iteration {k_}: |dv|", errs[k_][1], dv_q, 5e-4)
check("iterations", dg["iterations"], 32, 0)
check("final |dr| [m]", 1e3*np.linalg.norm(r2g - r0), 0.12, 0.005); check("final |dv| [mm/s]", 1e6*np.linalg.norm(v2g_ - v0), 0.7, 0.05)
for g, q in zip(el_g, (7799.998, 0.05830, 43.0811, 290.2235, 319.2685, 66.9295)): check("Gauss elements", g, q, 6e-4 if q > 100 else 6e-5)
check("series c1 at true r", tw3/tauw*(1 + u_/6*(tauw**2 - tw3**2)), 0.5008172, 5e-8)
check("exact c1", c1e, 0.5008196, 5e-8); check("exact c3", c3e, 0.5008171, 5e-8)
# noise table
for row, (D0q, devq, lq, g1q, gq) in zip(NA, ((1.0e-5, 21, 77, 77, 77), (8.3e-5, 85, 16, 16, 16), (6.5e-4, 342, 16, 4.2, 4.2), (2.1e-3, 772, 34, 2.1, 2.0),
                                              (1.4e-2, 3164, 128, 4.9, 0.50), (6.4e-2, 14016, 408, 20, 0.13), (0.19, 92769, 1012, 81, 0.031))):
    h = row["h"]
    check(f"table h={h}: D0", row["D0"], D0q, 0.06*D0q); check(f"   out of plane [arcsec]", row["dev"], devq, 0.6 + 0.001*devq)
    check(f"   Laplace median", row["laplace"][1], lq, 0.6 + 0.005*lq); check(f"   Gauss first pass", row["gauss1"][1], g1q, 0.06 + 0.005*g1q)
    check(f"   Gauss iterated", row["gauss"][1], gq, 0.06*gq + 0.0006)
check("2 arcsec at 1370 km [m]", 2*ARCSEC*obw[1]["range"]*1e3, 13, 0.5)
check("noise-free Laplace error at 10, 60, 120, 240 s (10)", np.linalg.norm(laplace([o["u"] for o in observe([-10, 0, 10])], [o["R"] for o in observe([-10, 0, 10])], o0["Rd"], o0["Rdd"], [-10, 0, 10])[0] - r0), 1.0, 0.02)
for hq, qq in ((60, 34), (120, 128), (240, 408)):
    obq = observe([-hq, 0, hq])
    check(f"noise-free Laplace error at {hq} s", np.linalg.norm(laplace([o["u"] for o in obq], [o["R"] for o in obq], o0["Rd"], o0["Rdd"], [-hq, 0, hq])[0] - r0), qq, 0.6)
for hq, (g1q, itq) in ((10, (None, 24)), (60, (None, 32)), (120, (None, 34)), (240, (None, 40))):
    obq = observe([-hq, 0, hq])
    _, _, dq = gauss([o["u"] for o in obq], [o["R"] for o in obq], [-hq, 0, hq], details=True)
    check(f"iterations at {hq} s", dq["iterations"], itq, 0)
# least squares
check("LSQ start |dr| [km]", np.linalg.norm(x0[:3] - r0), 153, 0.5); check("LSQ start |dv| [m/s]", 1e3*np.linalg.norm(x0[3:] - v0), 784, 0.5)
for k_, (rmsq, drq, dvq) in enumerate(((263.5, 152.9, 784), (28.7, 18.8, 96), (1.76, 0.169, 0.87), (1.71, 0.123, 0.63))):
    check(f"LSQ iteration {k_}: RMS [arcsec]", hist[k_][1]/ARCSEC, rmsq, 0.05 + 0.001*rmsq)
    check(f"   |dr| [km]", np.linalg.norm(hist[k_][0][:3] - r0), drq, 0.06 + 0.001*drq)
    check(f"   |dv| [m/s]", 1e3*np.linalg.norm(hist[k_][0][3:] - v0), dvq, 0.6)
check("LSQ iterations", len(hist) - 1, 5, 2)
check("expected RMS/sigma", np.sqrt(92/98)*2, 1.94, 0.005)
check("sigma along LOS [m]", 1e3*sg[0], 253, 0.5); check("sigma across 1 [m]", 1e3*sg[1], 2.8, 0.05); check("sigma across 2 [m]", 1e3*sg[2], 2.9, 0.05)
dx_ = B_ @ (xl[:3] - r0)*1e3
check("actual error along [m]", dx_[0], 123, 0.5); check("actual across 1 [m]", dx_[1], -1.9, 0.05); check("actual across 2 [m]", dx_[2], 1.0, 0.05)
sd_ = LS["los"].std(axis=0)*1e3
check("MC scatter along [m]", sd_[0], 247, 0.5); check("MC across 1 [m]", sd_[1], 2.7, 0.05); check("MC across 2 [m]", sd_[2], 3.0, 0.05)
check("IOD median |dr| [km]", np.median(LS["iod"]), 0.55, 0.005); check("IOD median |dv| [m/s]", 1e3*np.median(LS["iod_v"]), 3.0, 0.05)
check("LSQ median |dr| [km]", np.median(LS["lsq"]), 0.16, 0.005); check("LSQ median |dv| [m/s]", 1e3*np.median(LS["lsq_v"]), 0.85, 0.005)
check("ratio along/across", sg[0]/np.sqrt(0.5*(sg[1]**2 + sg[2]**2)), 90, 1.0)
check("effective number of independent measurements across", (2*ARCSEC*obw[1]["range"]/np.sqrt(0.5*(sg[1]**2 + sg[2]**2)))**2, 22, 1.0)
# pass geometry table
for off, (elq, D0q, dq) in ((0.0, (89.5, 3.2e-5, 7)), (1.0, (83.4, 6.0e-4, 119)), (2.0, (77.4, 1.1e-3, 230)), (5.0, (60.6, 2.1e-3, 566)),
                            (10.0, (39.5, 2.1e-3, 1125)), (15.0, (25.6, 1.5e-3, 1682))):
    rA, vA = truth_state(off)
    ob_ = observe([-60.0, 0.0, 60.0], rA, vA)
    L_ = [o["u"] for o in ob_]
    hv = np.cross(rA, vA); hv /= np.linalg.norm(hv)
    check(f"offset {off}: peak elevation", max(np.degrees(o["el"]) for o in observe(np.arange(-120, 121, 10.0), rA, vA)), elq, 0.06)
    check(f"   D0", L_[0] @ np.cross(L_[1], L_[2]), D0q, 0.06*D0q); check(f"   distance to the plane [km]", abs(station(0.0)[0] @ hv), dq, 0.6)
check("D0 ratio between the 60 deg pass and the zenith pass", 2.1053e-3/3.2171e-5, 65, 0.5)

print("\n==", sum(RESULTS), "of", len(RESULTS), "checks passed")
sys.exit(0 if all(RESULTS) else 1)