"""Checks every number and identity quoted in plane-changes.qmd. Prints FAIL on a mismatch.
Run from anywhere:   python3 codes/plane-changes-verify.py"""
import numpy as np
from scipy.optimize import minimize_scalar

fails = 0


def ok(name, x, y, tol):
    global fails
    good = abs(x - y) <= tol
    fails += not good
    print("ok  " if good else "FAIL", name, round(float(x), 5), y)


mu, r1, r2 = 398600.4418, 6678.137, 42164.0
Re, J2 = 6378.137, 1.08263e-3
at = (r1 + r2)/2
vc1, vc2 = np.sqrt(mu/r1), np.sqrt(mu/r2)
vt1, vt2 = np.sqrt(mu*(2/r1 - 1/at)), np.sqrt(mu*(2/r2 - 1/at))
ve = 9.80665e-3*320                            # exhaust speed for I_sp = 320 s, km/s
d = np.radians
pc = lambda v, a: 2*v*np.sin(a/2)
tri = lambda u, w, a: np.sqrt(u*u + w*w - 2*u*w*np.cos(a))
left = lambda dv: np.exp(-dv/ve)
DI = d(19.2)

# speeds used throughout
ok("a_t", at, 24421, 0.5); ok("v_c1", vc1, 7.726, 5e-4); ok("v_t1", vt1, 10.151, 5e-4)
ok("v_t2", vt2, 1.608, 5e-4); ok("v_c2", vc2, 3.075, 5e-4)

# the isosceles triangle: law of cosines = half-angle form, and the direction of the burn
for a in d(np.array([1.0, 10, 28.5, 60, 120, 180])):
    ok("law of cosines = half-angle form", tri(1, 1, a), pc(1, a), 1e-12)
    v2 = np.array((np.cos(a), np.sin(a))); dv = v2 - np.array((1.0, 0.0))
    ok("angle between dv and v1 = 90 + di/2", np.degrees(np.arccos(dv[0]/np.linalg.norm(dv))), 90 + np.degrees(a)/2, 1e-6)

# a turn on an eccentric orbit: only the transverse component turns
rng = np.random.default_rng(1)
for _ in range(5):
    vr, vth, a = rng.uniform(0.1, 3), rng.uniform(1, 9), rng.uniform(0.1, 2.5)
    v1 = np.array((vr, vth, 0.0)); v2 = np.array((vr, vth*np.cos(a), vth*np.sin(a)))
    ok("eccentric: |dv| = 2 v_theta sin(a/2)", np.linalg.norm(v2 - v1), 2*vth*np.sin(a/2), 1e-12)

# where to make the turn
ok("LEO", pc(vc1, DI), 2.577, 5e-4); ok("GEO", pc(vc2, DI), 1.026, 5e-4); ok("GTO apoapsis", pc(vt2, DI), 0.536, 5e-4)
ok("best against worst", pc(vc1, DI)/pc(vt2, DI), 4.8, 0.05)
ok("60 deg in LEO, mass left", left(vc1), 0.085, 5e-4)
ok("5 deg LEO", pc(vc1, d(5)), 0.674, 5e-4); ok("5 deg GEO", pc(vc2, d(5)), 0.268, 5e-4)

# the strategies
H1, H2 = vt1 - vc1, vc2 - vt2
ok("first burn", H1, 2.42573, 5e-6); ok("second burn", H2, 1.46682, 5e-6); ok("turn at GEO", pc(vc2, DI), 1.02552, 5e-6)
A = H1 + H2 + pc(vc2, DI); b1 = tri(vc1, vt1, DI); B = b1 + H2; c2 = tri(vt2, vc2, DI); C = H1 + c2
ok("(a)", A, 4.91807, 5e-6); ok("(b) burn", b1, 3.82218, 5e-6); ok("(b)", B, 5.28900, 5e-6)
ok("(c) burn", c2, 1.64363, 5e-6); ok("(c)", C, 4.06936, 5e-6)
for t_, v_ in ((A, 4.918), (B, 5.289), (C, 4.069)):
    ok("table total", t_, v_, 5e-4)
ok("the five-decimal sums add up as printed", 2.42573 + 1.46682 + 1.02552, 4.91807, 1e-9)
ok("the five-decimal sums add up as printed", 3.82218 + 1.46682, 5.28900, 1e-9)
ok("the five-decimal sums add up as printed", 2.42573 + 1.64363, 4.06936, 1e-9)
ok("saving", A - C, 0.849, 5e-4); ok("saving, per cent", 100*(A - C)/A, 17, 0.5)
ok("mass (a)", left(A), 0.209, 5e-4); ok("mass (b)", left(B), 0.185, 5e-4); ok("mass (c)", left(C), 0.273, 5e-4)
ok("(c) over (a)", left(C)/left(A), 1.31, 5e-3)

# limits of the combined formula
ok("no turn", tri(vt2, vc2, 0.0), H2, 1e-12); ok("no change of speed", tri(vc2, vc2, DI), pc(vc2, DI), 1e-12)

# launch sites, with the whole turn at GEO
for inc, burn, tot, m in ((7, 1.492, 3.917, 0.287), (19.2, 1.644, 4.069, 0.273), (28.5, 1.830, 4.256, 0.258)):
    t = H1 + tri(vt2, vc2, d(inc))
    ok(f"i={inc} burn", tri(vt2, vc2, d(inc)), burn, 5e-4); ok(f"i={inc} total", t, tot, 5e-4); ok(f"i={inc} mass", left(t), m, 5e-4)
ok("Sriharikota over the Cape", left(C)/left(H1 + tri(vt2, vc2, d(28.5))), 1.06, 5e-3)
ok("Kourou over Sriharikota", left(H1 + tri(vt2, vc2, d(7)))/left(C), 1.05, 5e-3)


def best(a):
    r = minimize_scalar(lambda x: tri(vc1, vt1, x) + tri(vt2, vc2, a - x), bounds=(0, a), method="bounded",
                        options={"xatol": 1e-10})
    return r.x, r.fun


# the optimum split
x, f = best(DI)
s1, s2 = tri(vc1, vt1, x), tri(vt2, vc2, DI - x)
ok("delta*", np.degrees(x), 1.64, 5e-3); ok("rest", 19.2 - np.degrees(x), 17.56, 5e-3)
ok("burn 1", s1, 2.43901, 5e-5); ok("burn 2", s2, 1.61619, 5e-5); ok("total", f, 4.05520, 5e-5); ok("(c) total", C, 4.06936, 5e-5)
ok("both sides", vc1*vt1*np.sin(x)/s1, 0.92263, 5e-5); ok("both sides", vt2*vc2*np.sin(DI - x)/s2, 0.92263, 5e-5)
ok("saved m/s", (C - f)*1e3, 14.2, 0.05); ok("burn 1 grows m/s", (s1 - H1)*1e3, 13, 0.5); ok("burn 2 falls m/s", (c2 - s2)*1e3, 27, 0.5)
ok("final mass gain, per cent", 100*(left(f)/left(C) - 1), 0.5, 0.05)
ok("slope at delta=0 is negative", float(-vt2*vc2*np.sin(DI)/c2 < 0), 1, 0)
ok("(b) combined at LEO beats a separate turn at LEO", float(b1 < H1 + pc(vc1, DI)), 1, 0)
ok("(b) still costs more than (a)", float(B > A), 1, 0)

# the small-turn result
Ak, Bk = vc1*vt1/H1, vt2*vc2/H2
ok("A", Ak, 32.33, 5e-3); ok("B", Bk, 3.370, 5e-4); ok("A/B", Ak/Bk, 9.6, 0.05); ok("B/(A+B)", Bk/(Ak + Bk), 0.0944, 5e-5)
ok("products", vc1*vt1, 78.4, 0.05); ok("products", vt2*vc2, 4.94, 5e-3)
ok("small-turn prediction at 19.2", Bk/(Ak + Bk)*19.2, 1.81, 5e-3); ok("exact share", 100*x/DI, 8.6, 0.05)
for a in (0.5, 1.0, 2.0):
    ok(f"small-turn formula at {a} deg", np.degrees(best(d(a))[0]), Bk/(Ak + Bk)*a, 2e-3)
for D, ds, sv in ((60, 2.90, 40), (61, 2.90, 40)):
    y, g = best(d(D))
    ok(f"at {D}: delta*", np.degrees(y), ds, 5e-3); ok(f"at {D}: saving", (H1 + tri(vt2, vc2, d(D)) - g)*1e3, sv, 0.5)
grid = np.arange(1, 90.5, 0.5)
ok("largest delta* near", grid[np.argmax([best(d(g))[0] for g in grid])], 61, 1.5)

# the node
r, i, DO = 6778.0, d(51.6), d(10)
ca = np.cos(i)**2 + np.sin(i)**2*np.cos(DO)
al = np.arccos(ca); v = np.sqrt(mu/r); n = np.sqrt(mu/r**3)
ok("cos alpha", ca, 0.99067, 5e-6); ok("alpha", np.degrees(al), 7.833, 5e-4); ok("v", v, 7.669, 5e-4)
ok("dv", pc(v, al), 1.048, 5e-4); ok("mass used", 1 - left(pc(v, al)), 0.28, 5e-3)
dot = np.degrees(-1.5*J2*(Re/r)**2*n*np.cos(i)*86400)
ok("node rate", dot, -5.00, 5e-3); ok("10 deg west, days", 10/abs(dot), 2.0, 0.05); ok("10 deg east, days", 350/abs(dot), 70, 0.5)
ok("polar orbit", np.degrees(np.arccos(np.cos(d(90))**2 + np.sin(d(90))**2*np.cos(DO))), 10, 1e-9)
ok("dot-product identity", np.dot((np.sin(i)*np.sin(.3), -np.sin(i)*np.cos(.3), np.cos(i)),
                                  (np.sin(i)*np.sin(.9), -np.sin(i)*np.cos(.9), np.cos(i))),
   np.cos(i)**2 + np.sin(i)**2*np.cos(.6), 1e-12)

# the exercise
y, g = best(d(13.7))
ok("Sriharikota delta*", np.degrees(y), 1.23, 5e-3); ok("total", g, 3.977, 5e-4)
ok("whole turn at GEO", H1 + tri(vt2, vc2, d(13.7)), 3.985, 5e-4); ok("saved m/s", (H1 + tri(vt2, vc2, d(13.7)) - g)*1e3, 8.0, 0.05)
ok("the dog-leg, m/s", (f - g)*1e3, 78, 0.5); ok("19.2 total, 3 places", f, 4.055, 5e-4)

# why the inclination is not the latitude
phi = d(13.7)
inc = lambda A_: np.degrees(np.arccos(np.cos(phi)*np.sin(d(A_))))
ok("due east gives the latitude", inc(90), 13.7, 1e-9)
ok("GSLV-F12, formula", inc(104), 19.49, 5e-3); ok("LVM3-M5, formula", inc(107), 21.71, 5e-3)
ok("sin A for 19.2", np.cos(d(19.2))/np.cos(phi), 0.9720, 5e-5)
ok("heading south of east", 180 - np.degrees(np.arcsin(np.cos(d(19.2))/np.cos(phi))), 103.6, 0.05)
ok("heading north of east", np.degrees(np.arcsin(np.cos(d(19.2))/np.cos(phi))), 76.4, 0.05)
ok("cos i = cos phi sin A from the vectors", np.dot((0.0, -np.cos(d(104)), np.sin(d(104))), (np.sin(phi), 0.0, np.cos(phi))), np.cos(phi)*np.sin(d(104)), 1e-12)
ok("rotation speed lost at 104 deg, m/s", 465.1*np.cos(phi)*(1 - np.sin(d(104))), 13, 0.5)

print("\nfailures:", fails)
raise SystemExit(1 if fails else 0)