"""Numeric checks behind the F01 worked examples and solutions. Run from frontier_code/."""
import math
from solutions import control_lab as lab
G = lab.G
checks = []
def check(name, value, expected, tol):
ok = abs(value - expected) <= tol
checks.append((name, value, expected, ok))
m = 1.5
check("F01.02 P-only error (m)", m * G / 5.0, 2.943, 5e-4)
check("F01.02 kp for 5 cm (N/m)", m * G / 0.05, 294.3, 0.05)
check("F01.03 payload error (m)", 0.3 * G / 4.0, 0.7358, 5e-4)
# F01.04 related rates
h, hdot, d = 30.0, 3.0, 40.0
cos_t = d / math.hypot(d, h)
thetadot = hdot / d * cos_t ** 2
check("F01.04 theta' (rad/s)", thetadot, 0.048, 1e-9)
check("F01.04 theta' (deg/s)", math.degrees(thetadot), 2.750, 5e-4)
# F01.05 noise
check("F01.05 100 Hz velocity noise", math.sqrt(2) * 0.02 / 0.01, 2.828, 5e-4)
check("F01.05 10 Hz velocity noise", math.sqrt(2) * 0.02 / 0.1, 0.2828, 5e-5)
# F01.06 bicycle
R = 2.9 / math.tan(math.radians(5))
check("F01.06 radius (m)", R, 33.15, 5e-3)
check("F01.06 yaw rate (rad/s)", 10.0 / R, 0.3017, 5e-5)
# F01.07 pure pursuit
kappa = 2 * 1.0 / 65.0
delta = math.atan(2.9 * kappa)
direct = lab.pure_pursuit_steer(lab.CarState(0, 0, 0, 0), (8.0, 1.0), 2.9)
check("F01.07 kappa (1/m)", kappa, 0.030769, 5e-7)
check("F01.07 delta (deg)", math.degrees(delta), 5.10, 5e-3)
check("F01.07 lab agrees (rad)", direct, delta, 1e-12)
# F01.09 critical damping
c, kp = 0.3, 4.0
kd_crit = 2 * math.sqrt(m * kp) - c
check("F01.09 kd critical", kd_crit, 4.599, 5e-4)
zeta = (c + 3.0) / (2 * math.sqrt(m * kp))
os_pred = 100 * math.exp(-math.pi * zeta / math.sqrt(1 - zeta ** 2))
check("F01.09 zeta at kd=3", zeta, 0.6736, 5e-5)
check("F01.09 predicted overshoot at kd=3 (%)", os_pred, 5.71, 5e-3)
sim_os3 = lab.step_metrics(lab.simulate_hover(4.0, 0.0, 3.0, mass=m, feedforward_mass=m, duration=15.0), 10.0).overshoot_pct
sim_osc = lab.step_metrics(lab.simulate_hover(4.0, 0.0, kd_crit, mass=m, feedforward_mass=m, duration=15.0), 10.0).overshoot_pct
checks.append(("F01.09 simulated overshoot kd=3 (%)", sim_os3, None, True))
checks.append(("F01.09 simulated overshoot kd=crit (%)", sim_osc, None, sim_osc < 1.0))
# F01.10 tuning example
met = lab.step_metrics(lab.simulate_hover(4.0, 0.5, 4.0, mass=m, feedforward_mass=m, payload=0.3, duration=20.0), 10.0)
checks.append(("F01.10 overshoot (%)", met.overshoot_pct, None, met.overshoot_pct < 10))
checks.append(("F01.10 settling (s)", met.settling_time, None, met.settling_time is not None and met.settling_time < 6))
checks.append(("F01.10 sse (m)", met.steady_state_error, None, abs(met.steady_state_error) < 0.02))
# F01.12 windup numbers
kw = dict(mass=m, feedforward_mass=m, setpoint=50.0, duration=25.0)
on = lab.step_metrics(lab.simulate_hover(6.0, 2.0, 4.0, anti_windup=True, **kw), 50.0)
off = lab.step_metrics(lab.simulate_hover(6.0, 2.0, 4.0, anti_windup=False, **kw), 50.0)
checks.append(("F01.12 overshoot with anti-windup (%)", on.overshoot_pct, None, True))
checks.append(("F01.12 overshoot without (%)", off.overshoot_pct, None, True))
bad = 0
for name, value, expected, ok in checks:
print(f"{'PASS' if ok else 'FAIL'} {name}: {value:.6g}" + (f" (expected {expected})" if expected is not None else ""))
bad += not ok
raise SystemExit(1 if bad else 0)