"""Figures for the f-and-g-series note and deck (fg-series.qmd).

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

  fig-fg-series   position error of the truncated series against the step tau,
                  for truncation after tau^2 ... tau^6 (the LEO example)
  fig-fg-deriv    static version of the "every derivative lives in the plane" widget: the jerk
                  r^(3) = F_3 r + G_3 v drawn as a parallelogram at eight points of the orbit (for the PDF)
"""
import sys
import numpy as np
import sympy as sp

sys.path.insert(0, "codes")
import _figfont                                   # all figure text at one scale
import matplotlib.pyplot as plt
from _manstyle import INK, MAROON, BLUE, GREEN, AMBER, PURPLE, despine
from _uv import fg, MU_E

# coefficients F_n, G_n from the recurrence (see fg-series-verify.py)
u, p, q = sp.symbols("u p q")
rules = {u: -3 * u * p, p: q - 2 * p**2, q: -p * (u + 2 * q)}
dot = lambda e: sp.expand(sum(sp.diff(e, s) * rules[s] for s in (u, p, q)))
N = 6
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])))
Fc = sp.lambdify((u, p, q), Fn)
Gc = sp.lambdify((u, p, q), Gn)

r0 = np.array([7000.0, 0.0, 0.0])
v0 = np.array([1.0, 7.8, 0.0])
R0, V0 = np.linalg.norm(r0), np.linalg.norm(v0)
uu, pp = MU_E / R0**3, r0 @ v0 / R0**2
qq = V0**2 / R0**2 - uu
fc = np.array(Fc(uu, pp, qq), float)
gc = np.array(Gc(uu, pp, qq), float)
fact = np.array([float(sp.factorial(n)) for n in range(N + 1)])

taus = np.logspace(0, np.log10(1500), 200)
exact = np.array([(lambda F, G, *_: F * r0 + G * v0)(*fg(r0, v0, t)) for t in taus])

fig, ax = plt.subplots(figsize=(8.4, 5.2))
colours = [MAROON, AMBER, GREEN, BLUE, PURPLE]
for k, c in zip(range(2, N + 1), colours):
    pw = taus[:, None] ** np.arange(k + 1) / fact[: k + 1]
    rs = (pw @ fc[: k + 1])[:, None] * r0 + (pw @ gc[: k + 1])[:, None] * v0
    err = np.linalg.norm(rs - exact, axis=1) * 1e3          # m
    ax.loglog(taus, err, color=c, lw=2, label=rf"through $\tau^{k}$")
    i = np.searchsorted(taus, 30)
    ax.text(taus[i] * 0.93, err[i], rf"$\propto\tau^{{{k+1}}}$", color=c, ha="right", va="bottom", fontsize=12)
ax.axvline(1 / np.sqrt(uu), color=INK, lw=1, ls="--")
ax.text(1 / np.sqrt(uu) * 0.9, 2e-6, r"$\tau=1/\sqrt{u}\approx 928$ s", color=INK, fontsize=12, ha="right")
ax.axhline(1.0, color=INK, lw=0.6, ls=":")
ax.text(300, 1.4, "1 m", color=INK, fontsize=11)
ax.set_xlim(1, 1500)
ax.set_ylim(1e-7, 1e7)                          # below ~1e-8 m: double-precision round-off
ax.set_xlabel(r"step $\tau$  [s]")
ax.set_ylabel("position error  [m]")
ax.legend(loc="upper left", frameon=False)
despine(ax)
fig.savefig("img/fig-fg-series.png", dpi=150, bbox_inches="tight")
print("wrote img/fig-fg-series.png")

# ---- static version of the derivative widget: r^(3) = F_3 r + G_3 v at eight points of the orbit
from _uv import propagate
a_ = 1 / (2 / R0 - V0**2 / MU_E)
T_ = 2 * np.pi * np.sqrt(a_**3 / MU_E)
F3 = sp.lambdify((u, p, q), Fn[3])
G3 = sp.lambdify((u, p, q), Gn[3])
path = np.array([propagate(r0, v0, t)[0] for t in np.linspace(0, T_, 361)])
pts = [propagate(r0, v0, t) for t in np.linspace(0, T_, 9)[:-1]]


def comps(r, v):
    R, V = np.linalg.norm(r), np.linalg.norm(v)
    uu_, pp_ = MU_E / R**3, r @ v / R**2
    qq_ = V**2 / R**2 - uu_
    return F3(uu_, pp_, qq_) * r, G3(uu_, pp_, qq_) * v


scale = 2600 / max(max(np.linalg.norm(A), np.linalg.norm(B), np.linalg.norm(A + B))
                   for A, B in (comps(r, v) for r, v in pts))
fig, ax = plt.subplots(figsize=(7.2, 6.6))
ax.add_patch(plt.Circle((0, 0), 6378, color="#cfd8e3", zorder=0))
ax.plot(path[:, 0], path[:, 1], color=INK, lw=1, alpha=0.6)
kw = dict(angles="xy", scale_units="xy", scale=1, width=0.006, zorder=3)
for r, v in pts:
    A, B = comps(r, v)
    A, B = A * scale, B * scale
    ax.plot([r[0] + A[0], r[0] + A[0] + B[0]], [r[1] + A[1], r[1] + A[1] + B[1]], color=BLUE, lw=0.8, ls="--")
    ax.plot([r[0] + B[0], r[0] + A[0] + B[0]], [r[1] + B[1], r[1] + A[1] + B[1]], color=MAROON, lw=0.8, ls="--")
    ax.quiver(r[0], r[1], A[0], A[1], color=MAROON, **kw)
    ax.quiver(r[0], r[1], B[0], B[1], color=BLUE, **kw)
    ax.quiver(r[0], r[1], A[0] + B[0], A[1] + B[1], color=PURPLE, **kw)
    ax.plot(r[0], r[1], "o", color=INK, ms=4, zorder=4)
ax.plot([], [], color=MAROON, lw=2, label=r"$F_3\,\mathbf{r}$")
ax.plot([], [], color=BLUE, lw=2, label=r"$G_3\,\mathbf{v}$")
ax.plot([], [], color=PURPLE, lw=2, label=r"$\mathbf{r}^{(3)}$ (the jerk)")
ax.legend(loc="upper left", frameon=False)
ax.set_aspect("equal")
ax.axis("off")
fig.savefig("img/fig-fg-deriv.png", dpi=150, bbox_inches="tight")
print("wrote img/fig-fg-deriv.png")
