"""Checks the numbers quoted in topocentric.qmd, using the helper module _topo.py and, for the key results,
an independent route (explicit rotation matrices, finite differences, an elements-from-state computation).

Run from the sfm-slides directory:   python3 codes/topocentric-verify.py
"""
import sys
import numpy as np
from scipy.optimize import brentq, minimize_scalar
sys.path.insert(0, "codes")
from _topo import (A_WGS, F_WGS, E2, MU, WE, C, R3, R1, prime_vertical, site_ecef, geocentric_latitude, sez_matrix, azelrange,
                   sez_from_azelrange, observe, state_from_observation, radec_from_direction, circular_state)

RESULTS = []


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


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


D2R = np.pi / 180
phi, lam, hsite = 12.97 * D2R, 77.59 * D2R, 0.92

print("The ellipsoid and the site")
check("e^2", E2, 0.006694380, 5e-10)
check("b = a sqrt(1 - e^2) [km]", A_WGS * np.sqrt(1 - E2), 6356.752, 5e-4)
ph = np.linspace(0, np.pi / 2, 200001)
dd = ph - np.arctan((1 - E2) * np.tan(ph))
check("max (phi - phi_gc) [deg]", np.degrees(dd.max()), 0.192, 5e-4)
check("  at phi [deg]", np.degrees(ph[np.argmax(dd)]), 45.1, 0.05)
check("  in arcmin", np.degrees(dd.max()) * 60, 11.5, 0.05)
check("  along the ground [km]", dd.max() * 6371, 21, 0.5)
check("  at 1000 km range [km]", dd.max() * 1000, 3.4, 0.05)
check("tan(phi_gc) = (1 - e^2) tan(phi) at 45 deg: phi - phi_gc [deg]", np.degrees(np.pi / 4 - np.arctan((1 - E2))), 0.192, 5e-4)
check("N at Bengaluru [km]", prime_vertical(phi), 6379.2127, 5e-4)
rs = site_ecef(phi, lam, hsite)
check("site x [km]", rs[0], 1336.147, 5e-4); check("site y [km]", rs[1], 6072.091, 5e-4); check("site z [km]", rs[2], 1422.378, 5e-4)
check("|r_site| [km]", np.linalg.norm(rs), 6377.99, 0.005)
check("geocentric latitude [deg]", np.degrees(geocentric_latitude(phi, hsite)), 12.886, 5e-4)
check("  difference [arcmin]", np.degrees(phi - geocentric_latitude(phi, hsite)) * 60, 5.03, 0.005)
# the ellipse equation and the normal direction at a surface point (independent check of eq. site)
for pd in (10, 45, 80):
    p_ = pd * D2R; r0 = site_ecef(p_, 0.0, 0.0)
    check(f"on the ellipsoid at phi = {pd}: rho^2/a^2 + z^2/b^2", (r0[0] / A_WGS) ** 2 + (r0[2] / (A_WGS * np.sqrt(1 - E2))) ** 2, 1.0, 1e-12)
    n_ = np.array([r0[0] / A_WGS ** 2, 0, r0[2] / (A_WGS ** 2 * (1 - E2))]); n_ /= np.linalg.norm(n_)
    check(f"  normal direction is (cos phi, sin phi)", np.hypot(n_[0] - np.cos(p_), n_[2] - np.sin(p_)), 0, 1e-12)
check("equator site [km]", site_ecef(0.0, 0.0, 0.0)[0], 6378.137, 5e-4)
check("pole site z [km]", site_ecef(np.pi / 2, 0.0, 0.0)[2], 6356.752, 5e-4)
check("N at the pole = a^2/b [km]", prime_vertical(np.pi / 2), A_WGS ** 2 / (A_WGS * np.sqrt(1 - E2)), 1e-9)

print("The SEZ frame")
M = sez_matrix(phi, lam)
S_, E_, Z_ = M
truth("S x E = Z", np.allclose(np.cross(S_, E_), Z_))
truth("M M^T = 1 and det M = +1", np.allclose(M @ M.T, np.eye(3)) and abs(np.linalg.det(M) - 1) < 1e-12)
Ry = lambda t: np.array([[np.cos(t), 0, -np.sin(t)], [0, 1, 0], [np.sin(t), 0, np.cos(t)]])
Rz = lambda t: np.array([[np.cos(t), np.sin(t), 0], [-np.sin(t), np.cos(t), 0], [0, 0, 1]])
truth("M = Ry(90 deg - phi) Rz(lambda) (passive rotations)", np.allclose(M, Ry(np.pi / 2 - phi) @ Rz(lam)))
ref = np.array([[0.04823, 0.21920, -0.97449], [-0.97663, 0.21491, 0.0], [0.20942, 0.95172, 0.22444]])
check("M for Bengaluru: max entry error", np.abs(M - ref).max(), 0, 6e-6)
M0 = sez_matrix(0.0, 0.0)
truth("at phi = lambda = 0: S = -K, E = J, Z = I", np.allclose(M0, [[0, 0, -1], [0, 1, 0], [1, 0, 0]]))
ENU = np.array([M[1], -M[0], M[2]])
check("ENU first row at lambda = 0 is (0, 1, 0)", np.abs(sez_matrix(phi, 0.0)[1] - [0, 1, 0]).max(), 0, 1e-12)

print("The worked observation")
a = 6378.137 + 420; inc = 51.64 * D2R; raan = 166.0 * D2R; tg0 = 100.0 * D2R
t_aos0 = 24.00892501398482
t = t_aos0 + 150
r, v = circular_state(a, inc, raan, 0.0, t); tg = tg0 + WE * t
check("t [s]", t, 174.009, 5e-4)
check("theta_G [deg]", np.degrees(tg) % 360, 100.727, 5e-4)
for k, (val, st) in enumerate(zip(r, (-6668.678, 815.911, 1038.120))):
    check(f"r[{k}] [km]", val, st, 5e-4)
for k, (val, st) in enumerate(zip(v, (0.3193, -4.8834, 5.8893))):
    check(f"v[{k}] [km/s]", val, st, 5e-5)
az, el, rng, daz, dele, drng = observe(r, v, tg, phi, lam, hsite)
check("range [km]", rng, 868.82, 0.005)
check("elevation [deg]", np.degrees(el), 25.51, 0.005)
check("azimuth [deg]", np.degrees(az), 232.21, 0.005)
check("azimuth - 180 [deg] (west of south)", np.degrees(az) - 180, 52.2, 0.05)
check("rate of azimuth [deg/s]", np.degrees(daz), 0.1417, 5e-5)
check("rate of elevation [deg/s]", np.degrees(dele), 0.2494, 5e-5)
check("range rate [km/s]", drng, -6.008, 5e-4)
check("one-way Doppler at 2.2 GHz [kHz]", -2.2e9 * drng / C / 1e3, 44.09, 0.005)
# independent: explicit matrices and finite differences
rho_e = R3(tg).T @ r - rs
rho_s = Ry(np.pi / 2 - phi) @ Rz(lam) @ rho_e
truth("range, az, el from the explicit rotation product agree", np.allclose([np.linalg.norm(rho_s), np.degrees(np.arctan2(rho_s[1], -rho_s[0])) % 360, np.degrees(np.arcsin(rho_s[2] / np.linalg.norm(rho_s)))],
                                                                           [rng, np.degrees(az), np.degrees(el)]))
dt = 1e-3
o2 = observe(*circular_state(a, inc, raan, 0.0, t + dt), tg0 + WE * (t + dt), phi, lam, hsite)
o1 = observe(*circular_state(a, inc, raan, 0.0, t - dt), tg0 + WE * (t - dt), phi, lam, hsite)
check("finite difference: d(az)/dt [deg/s]", np.degrees((o2[0] - o1[0]) / (2 * dt)), np.degrees(daz), 1e-6)
check("finite difference: d(el)/dt [deg/s]", np.degrees((o2[1] - o1[1]) / (2 * dt)), np.degrees(dele), 1e-6)
check("finite difference: d(range)/dt [km/s]", (o2[2] - o1[2]) / (2 * dt), drng, 1e-7)
# without the rotation term the range rate would differ
vwo = np.dot(rho_e, R3(tg).T @ v) / rng
check("range rate with the omega x r term omitted differs by [km/s]", abs(vwo - drng), 0.0, 0.47)
check("omega_E R_E [km/s]", WE * A_WGS, 0.465, 5e-4)
# the inverse
r2, v2 = state_from_observation(az, el, rng, daz, dele, drng, tg, phi, lam, hsite)
check("inverse: max |r - r'| [km]", np.abs(r2 - r).max(), 0, 1e-9)
check("inverse: max |v - v'| [km/s]", np.abs(v2 - v).max(), 0, 1e-12)
# elements from the state
rn, vn = np.linalg.norm(r2), np.linalg.norm(v2); hv = np.cross(r2, v2); hn = np.linalg.norm(hv)
eps = vn ** 2 / 2 - MU / rn; a_el = -MU / (2 * eps); ev = np.cross(v2, hv) / MU - r2 / rn
nv = np.cross([0, 0, 1], hv)
check("elements: a [km]", a_el, 6798.137, 5e-4)
check("  e", np.linalg.norm(ev), 0.0, 1e-12)
check("  i [deg]", np.degrees(np.arccos(hv[2] / hn)), 51.64, 5e-5)
check("  Omega [deg]", np.degrees(np.arctan2(nv[1], nv[0])) % 360, 166.0, 5e-5)
u_el = np.degrees(np.arctan2(np.dot(np.cross(nv, r2), hv) / hn, np.dot(nv, r2))) % 360
check("  argument of latitude [deg]", u_el, 11.23, 5e-3)
check("  = n t [deg]", np.degrees(np.sqrt(MU / a ** 3) * t), 11.23, 5e-3)

print("Right ascension and declination")
rho_i = r - R3(tg) @ rs
alpha, dec = radec_from_direction(rho_i)
check("topocentric RA [deg]", np.degrees(alpha), 125.64, 0.005)
check("topocentric Dec [deg]", np.degrees(dec), -26.25, 0.005)
LST = (tg + lam) % (2 * np.pi)
check("local sidereal time [deg]", np.degrees(LST), 178.317, 5e-4)
check("  in hours", np.degrees(LST) / 15, 11 + 53.3 / 60, 0.002)
Hh = (LST - alpha) % (2 * np.pi)
check("hour angle [deg]", np.degrees(Hh), 52.68, 0.005)
sd = np.sin(phi) * np.sin(el) + np.cos(phi) * np.cos(el) * np.cos(az)
check("sin(dec) from the triangle", sd, -0.4423, 5e-5)
check("sin(dec) from the rotation", np.sin(dec), sd, 1e-12)
check("cos(dec) sin(H) = -cos(el) sin(A)", np.cos(dec) * np.sin(Hh), -np.cos(el) * np.sin(az), 1e-12)
check("cos(dec) cos(H) = cos(phi) sin(el) - sin(phi) cos(el) cos(A)", np.cos(dec) * np.cos(Hh), np.cos(phi) * np.sin(el) - np.sin(phi) * np.cos(el) * np.cos(az), 1e-12)
ag, dg = radec_from_direction(r)
check("geocentric RA [deg]", np.degrees(ag), 173.02, 0.005)
check("geocentric Dec [deg]", np.degrees(dg), 8.78, 0.005)
check("angle between topocentric and geocentric directions [deg]", np.degrees(np.arccos(rho_i @ r / np.linalg.norm(rho_i) / np.linalg.norm(r))), 57.8, 0.05)
check("  nadir angle formula (spherical) [deg]", np.degrees(np.arcsin(np.linalg.norm(rs) / rn * np.cos(el))), 57.9, 0.05)

print("A pass")


def obs(tt):
    return observe(*circular_state(a, inc, raan, 0.0, tt), tg0 + WE * tt, phi, lam, hsite)


tm = minimize_scalar(lambda tt: -obs(tt)[1], bounds=(-300, 600), method="bounded", options={"xatol": 1e-6}).x
mask = 5 * D2R
t1 = brentq(lambda tt: obs(tt)[1] - mask, tm - 600, tm); t2 = brentq(lambda tt: obs(tt)[1] - mask, tm, tm + 600)
check("AOS [s]", t1, 24.01, 0.005)
check("duration [s]", t2 - t1, 507.9, 0.05)
check("maximum elevation [deg]", np.degrees(obs(tm)[1]), 63.8, 0.05)
check("AOS azimuth [deg]", np.degrees(obs(t1)[0]), 223.2, 0.05)
check("LOS azimuth [deg]", np.degrees(obs(t2)[0]), 31.4, 0.05)
check("azimuth at the maximum [deg]", np.degrees(obs(tm)[0]), 307.2, 0.05)
check("range at AOS [km]", obs(t1)[2], 1856.5, 0.05)
check("range at LOS [km]", obs(t2)[2], 1867.9, 0.05)
rmin = minimize_scalar(lambda tt: obs(tt)[2], bounds=(tm - 200, tm + 200), method="bounded").x
check("closest approach [km]", obs(rmin)[2], 465.1, 0.05)
check("range rate at AOS [km/s]", obs(t1)[5], -6.834, 5e-4)
check("range rate at LOS [km/s]", obs(t2)[5], 6.829, 5e-4)
check("Doppler at AOS, 2.2 GHz, one-way [kHz]", -2.2e9 * obs(t1)[5] / C / 1e3, 50.1, 0.05)
rd = lambda tt: obs(tt)[5]
slope = (rd(rmin + 0.5) - rd(rmin - 0.5)) / 1.0
check("range acceleration at closest approach [km/s^2]", slope, 0.109, 5e-4)
check("  Doppler rate [Hz/s]", -2.2e9 * slope / C, -800, 1)
check("  straight-line estimate v^2 / rho_min, v = 7 km/s [km/s^2]", 49 / 465.1, 0.105, 5e-4)
check("  estimated Doppler rate [Hz/s]", -2.2e9 * (49 / 465.1) / C, -770, 5)

print("Visibility")
n_iss = np.sqrt(MU / a ** 3)
eta = np.arcsin(A_WGS / a * np.cos(5 * D2R)); psi0 = np.pi / 2 - 5 * D2R - eta
check("eta [deg]", np.degrees(eta), 69.17, 0.005)
check("psi_0 [deg]", np.degrees(psi0), 15.83, 0.005)
check("overhead pass duration 2 psi_0 / n [s]", 2 * psi0 / n_iss, 490.5, 0.05)
check("  in minutes", 2 * psi0 / n_iss / 60, 8.2, 0.05)
check("period [min]", 2 * np.pi / n_iss / 60, 92.97, 0.005)
check("parallax exercise: eta = asin(6378/7000 cos 10 deg) [deg]", np.degrees(np.arcsin(6378 / 7000 * np.cos(10 * D2R))), 63.8, 0.05)

print("Accuracy")
bennett = lambda e: 1 / np.tan(np.radians(e + 7.31 / (e + 4.4)))
check("refraction at 0 deg [arcmin]", bennett(0), 34.5, 0.05)
check("refraction at 5 deg [arcmin]", bennett(5), 9.9, 0.05)
check("refraction at 45 deg [arcmin]", bennett(45), 1.0, 0.05)
check("  at 5 deg [deg]", bennett(5) / 60, 0.16, 0.005)
check("  at 5 deg, 1000 km [km]", bennett(5) / 60 * D2R * 1000, 2.9, 0.05)
e1 = brentq(lambda e: bennett(e) - 1.0, 20, 80)
check("elevation at which refraction is 1 arcmin [deg]", e1, 45, 1.0)
check("0.01 deg at 1000 km [m]", 1000 * 0.01 * D2R * 1e3, 175, 0.5)
check("10 ns of round trip as a range [m]", 0.5 * C * 10e-9 * 1e3, 1.5, 0.01)
check("0.1 Hz at 2.2 GHz [mm/s]", C * 0.1 / 2.2e9 * 1e6, 13.6, 0.05)
check("light time at 1860 km [ms]", 1860 / C * 1e3, 6.2, 0.05)
check("  satellite motion in it [m]", 7.66 * 1860 / C * 1e3, 48, 0.5)

print(f"\n{sum(RESULTS)} PASS, {len(RESULTS) - sum(RESULTS)} FAIL")