"""Verify every formula of the f-and-g-series deck (fg-series-deck.qmd).

Run from sfm-slides/:   python3 codes/fg-series-verify.py

1. The derivative rules  u' = -3up,  p' = q - 2p^2,  q' = -p(u + 2q)  (symbolic, from r'' = -mu r/r^3).
2. The recurrence  F_{n+1} = F_n' - u G_n,  G_{n+1} = F_n + G_n'  and the coefficients through tau^8.
3. Circular orbit (p = q = 0): F -> cos(n tau), G -> sin(n tau)/n.
4. The Wronskian F G' - F' G = 1 holds order by order.
5. Numbers: the truncated series against the exact Lagrange coefficients (_uv.fg) for the deck's example.
"""
import sys
import numpy as np
import sympy as sp

sys.path.insert(0, "codes")
from _uv import fg, MU_E                          # exact F, G, Fdot, Gdot (universal variables)

# ---------------------------------------------------------------- 1. derivative rules
t, mu = sp.symbols("t mu", positive=True)
x, y, z = [sp.Function(c)(t) for c in "xyz"]
rv = sp.Matrix([x, y, z])
r = sp.sqrt(rv.dot(rv))
acc = {sp.Derivative(c, (t, 2)): -mu * c / r**3 for c in (x, y, z)}
vv = rv.diff(t)
u_ = mu / r**3
p_ = rv.dot(vv) / r**2
q_ = vv.dot(vv) / r**2 - u_


def d(e):
    return sp.simplify(e.diff(t).subs(acc))


assert sp.simplify(d(u_) - (-3 * u_ * p_)) == 0
assert sp.simplify(d(p_) - (q_ - 2 * p_**2)) == 0
assert sp.simplify(d(q_) - (-p_ * (u_ + 2 * q_))) == 0
print("1. u' = -3up,  p' = q - 2p^2,  q' = -p(u+2q)        OK")

# ---------------------------------------------------------------- 2. recurrence
u, p, q, tau = sp.symbols("u p q tau")
rules = {u: -3 * u * p, p: q - 2 * p**2, q: -p * (u + 2 * q)}


def dot(e):
    return sp.expand(sum(sp.diff(e, s) * rules[s] for s in (u, p, q)))


N = 8
Fn, Gn = [sp.Integer(1)], [sp.Integer(0)]
for n in range(N):
    Fn.append(sp.expand(dot(Fn[n]) - u * Gn[n]))
    Gn.append(sp.expand(Fn[n] + dot(Gn[n])))

print("2. Coefficients  F_n, G_n  (F = sum F_n tau^n/n!, G = sum G_n tau^n/n!):")
for n in range(N + 1):
    print(f"   n={n}:  F_{n} = {sp.factor(Fn[n])!s:40s}  G_{n} = {sp.factor(Gn[n])}")

F = sum(Fn[n] * tau**n / sp.factorial(n) for n in range(N + 1))
G = sum(Gn[n] * tau**n / sp.factorial(n) for n in range(N + 1))
print("   F =", sp.collect(sp.expand(F), tau))
print("   G =", sp.collect(sp.expand(G), tau))

# the values quoted in the deck
assert sp.expand(Fn[2] + u) == 0 and sp.expand(Fn[3] - 3 * u * p) == 0
assert sp.expand(Fn[4] - (u**2 + 3 * u * q - 15 * u * p**2)) == 0
assert sp.expand(Fn[5] - 15 * u * p * (7 * p**2 - 3 * q - u)) == 0
assert sp.expand(Gn[3] + u) == 0 and sp.expand(Gn[4] - 6 * u * p) == 0
assert sp.expand(Gn[5] - (u**2 + 9 * u * q - 45 * u * p**2)) == 0
print("   deck's F_2..F_4, G_3..G_5                          OK")
print("   F_5 =", sp.factor(Fn[5]), "    G_6 =", sp.factor(Gn[6]))

# ---------------------------------------------------------------- 3. circular orbit
nn = sp.symbols("n", positive=True)
Fc = sp.expand(F.subs({p: 0, q: 0, u: nn**2}))
Gc = sp.expand(G.subs({p: 0, q: 0, u: nn**2}))
assert sp.expand(Fc - sp.series(sp.cos(nn * tau), tau, 0, N + 1).removeO()) == 0
assert sp.expand(Gc - sp.series(sp.sin(nn * tau) / nn, tau, 0, N + 1).removeO()) == 0
print("3. circular orbit: F = cos(n tau), G = sin(n tau)/n  through tau^8   OK")

# ---------------------------------------------------------------- 4. Wronskian
W = sp.expand(F * sp.diff(G, tau) - sp.diff(F, tau) * G)
low = sum(W.coeff(tau, k) * tau**k for k in range(N))       # orders the truncation gets right
assert sp.simplify(low - 1) == 0
print(f"4. F G' - F' G = 1 + O(tau^{N})                       OK")

# ---------------------------------------------------------------- 5. numbers
r0 = np.array([7000.0, 0.0, 0.0])                            # km
v0 = np.array([1.0, 7.8, 0.0])                               # km/s
R0, V0 = np.linalg.norm(r0), np.linalg.norm(v0)
uu = MU_E / R0**3
pp = r0 @ v0 / R0**2
qq = V0**2 / R0**2 - uu
a = 1 / (2 / R0 - V0**2 / MU_E)
T = 2 * np.pi * np.sqrt(a**3 / MU_E)
print(f"5. Example: r0 = {r0} km, v0 = {v0} km/s")
print(f"   u = {uu:.6e} 1/s^2, p = {pp:.6e} 1/s, q = {qq:.6e} 1/s^2")
print(f"   sqrt(u) = {np.sqrt(uu):.6e} 1/s (1/sqrt(u) = {1/np.sqrt(uu):.1f} s), a = {a:.1f} km, T = {T/60:.2f} min")

Ff = sp.lambdify((u, p, q, tau), [Fn[n] * tau**n / sp.factorial(n) for n in range(N + 1)])
Gf = sp.lambdify((u, p, q, tau), [Gn[n] * tau**n / sp.factorial(n) for n in range(N + 1)])
print("   tau [s] | order | F series        G series [s]     | pos. error [m]")
for dt in (10.0, 60.0, 300.0):
    Fe, Ge, Fde, Gde, *_ = fg(r0, v0, dt)
    rex = Fe * r0 + Ge * v0
    fterms = np.array(Ff(uu, pp, qq, dt), dtype=float)
    gterms = np.array(Gf(uu, pp, qq, dt), dtype=float)
    for k in (2, 3, 4, 6):
        Fs, Gs = fterms[: k + 1].sum(), gterms[: k + 1].sum()
        err = np.linalg.norm(Fs * r0 + Gs * v0 - rex) * 1e3
        Fds = sum(n * fterms[n] / dt for n in range(1, k + 1))       # d/dtau of the truncated series
        Gds = sum(n * gterms[n] / dt for n in range(1, k + 1))
        verr = np.linalg.norm(Fds * r0 + Gds * v0 - (Fde * r0 + Gde * v0)) * 1e3
        print(f"   {dt:6.0f}  | {k:5d} | {Fs:.12f}  {Gs:.9f}  | {err:.3e}   vel. error {verr:.3e} m/s")
    print(f"   {dt:6.0f}  | exact | {Fe:.12f}  {Ge:.9f}  |")
print("All checks passed.")
