#!/usr/bin/env python3
"""Is the pilot's attitude binding live, and is its yaw stick the right way round?

`pilot.py` never fires: `fire=1` in 0 of 13 521 samples over a 900 s run. The log
says why the gate never opens — `|aim yaw|` is **exactly 90.0°** in all 3 004
samples that had a target, which is the `ez < 0` branch of `sticks()`, i.e. *the
target is behind us*, always — and the range to the committed target grows
22 km → 33 km → 49 km and stays there. The craft flies away from what it is
chasing and the turn never completes.

Three things could do that and they need separating by measurement, not argument:

1. the attitude matrix the pilot reads is **stale**, so the ship turns and the
   pilot cannot see it;
2. the matrix is live but `fwd_sign` / `fwd_row` name the wrong axis, so
   "forward" points backwards;
3. both are fine and the **yaw stick sign** is inverted, so the controller turns
   away from the error it is trying to null.

This probe separates them: watch the forward vector with no input at all, then
under a hard left stick, then a hard right one.

Usage: aim_probe.py <nav-live.json> [seconds_per_phase]
"""
import json
import math
import os
import sys
import time

import numpy as np

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import navigator  # noqa: E402
from flight_probe import Pad  # noqa: E402


def main():
    cfg = json.load(open(sys.argv[1]))
    secs = float(sys.argv[2]) if len(sys.argv) > 2 else 4.0
    W = navigator.World(cfg)
    W.scan()
    pad = Pad()

    def fwd_now():
        ents = W.sample(time.time())
        me = next((e for e in ents if "Player" in e[1]), None)
        if me is None:
            return None, None
        M = W.rot(me[0])
        if M is None:
            return None, None
        return M[W.fwd_row] * W.fwd_sign, me[2]

    def phase(name, lx):
        pad.axis("LX", lx)
        f0, p0 = fwd_now()
        t0 = time.time()
        time.sleep(secs)
        f1, p1 = fwd_now()
        pad.axis("LX", 0.0)
        if f0 is None or f1 is None:
            print(f"{name:>12}: NO PLAYER/ORIENTATION")
            return
        dot = float(np.clip(np.dot(f0, f1), -1, 1))
        turn = math.degrees(math.acos(dot)) / max(time.time() - t0, 1e-3)
        cross = np.cross(f0, f1)
        moved = float(np.linalg.norm(p1 - p0)) if p0 is not None else float("nan")
        print(f"{name:>12}: |turn| {turn:6.2f} deg/s   fwd {f0.round(3)} -> "
              f"{f1.round(3)}   cross {cross.round(3)}   moved {moved:8.1f}")

    print("# phases: neutral, then full LEFT stick, then full RIGHT stick")
    phase("neutral", 0.0)
    phase("LX=-1", -1.0)
    phase("neutral", 0.0)
    phase("LX=+1", +1.0)
    pad.reset()
    return 0


if __name__ == "__main__":
    raise SystemExit(main())