#!/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 frozen  # noqa: E402
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()

    # There is more than one orthonormal 3x3 block near the player object -- a
    # scan finds them at pos-0x70 and pos-0x30, and `entities2.py self` has
    # picked each on different runs. Watch BOTH, because "the one the config
    # names did not move" is a much weaker statement than "neither of the two
    # candidates moved".
    deltas = [W.rot_delta]
    if "--deltas" in sys.argv:
        deltas = [int(x, 0) for x in sys.argv[sys.argv.index("--deltas") + 1].split(",")]

    def read_block(off, delta):
        b = os.pread(W.fd, W.rot_stride * 3 + 12, off + delta)
        rows = []
        for r in range(3):
            row = np.frombuffer(b[W.rot_stride * r:W.rot_stride * r + 12],
                                dtype=">f4").astype(np.float64)
            rows.append(row)
        return np.array(rows)

    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, None
        mats = {}
        for d in deltas:
            try:
                M = read_block(me[0], d)
            except Exception:
                continue
            if np.all(np.isfinite(M)):
                mats[d] = M
        M = W.rot(me[0])
        if M is None:
            return None, None, mats
        return M[W.fwd_row] * W.fwd_sign, me[2], mats

    def phase(name, lx):
        # BRACKET the phase with a liveness check at BOTH ends. The first run of
        # this probe reported the forward vector pinned and 0.00 deg/s under
        # every stick, which reads as "the attitude matrix is dead" -- and the
        # guest had simply FROZEN partway through. A dead world holds every
        # matrix still, so a phase that ends frozen proves nothing and must be
        # thrown away rather than reported.
        pad.axis("LX", lx)
        f0, p0, m0 = fwd_now()
        t0 = time.time()
        time.sleep(secs)
        f1, p1, m1 = fwd_now()
        pad.axis("LX", 0.0)
        froze = frozen.frozen(3.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")
        blocks = "  ".join(
            f"pos{d:+#06x} d{float(np.abs(m1[d]-m0[d]).max()):.4f}"
            for d in deltas if d in m0 and d in m1)
        print(f"{name:>12}: |turn| {turn:6.2f} deg/s   fwd {f0.round(3)} -> "
              f"{f1.round(3)}   moved {moved:8.1f}   [{blocks}]"
              f"{'   *** GUEST FROZE - DISCARD ***' if froze else ''}")

    if frozen.frozen(3.0)[0]:
        print("# GUEST IS ALREADY FROZEN — nothing to measure")
        return 3
    print("# phases: neutral, then full LEFT stick, then full RIGHT stick")
    print("# (each phase is followed by a liveness check; a frozen tail voids it)")
    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())