#!/usr/bin/env python3
"""Measure the craft's TURN rates, and whether they depend on speed.

The unit definition carries `AA_Yaw_{Max,Min}`, `AA_PitchPlus_{Max,Min}`,
`AA_Roll_{Max,Min}` — pairs whose obvious reading is "rate at maximum speed" and
"rate at minimum speed", i.e. agility falling as the craft goes faster (for the
player: yaw 65°/s vs 120°/s, roll 475°/s vs 750°/s). That is a prediction, so
measure it: hold a throttle to pin the speed regime, hold a stick axis, and
differentiate the craft's own forward vector.

Same discipline as speed_law.py: lock onto the player object once, sample at
20 Hz, and differentiate over 1-second windows (the guest updates slower than the
sample rate).

Usage: turn_law.py <out.csv> [hold_s]
"""
import json, math, os, struct, subprocess, sys, time
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import gworld, entities2, speed_law

def fwd_at(fd, off, cfg):
    """The craft's forward row out of its rotation matrix (entities2 `self` bound
    rot_delta / rot_stride / fwd_row and validated the row against motion)."""
    at = off + cfg["rot_delta"] + cfg["fwd_row"] * cfg["rot_stride"]
    v = struct.unpack(">3f", os.pread(fd, 12, at))
    n = math.sqrt(sum(c * c for c in v)) or 1.0
    s = cfg.get("fwd_sign", 1)
    return tuple(s * c / n for c in v)

def pad(*a):
    subprocess.run(["vgamepad", *a], capture_output=True)

def rate(seq, lo, hi):
    """Degrees per second between the first sample at/after `lo` and the last at/before `hi`."""
    a = [s for s in seq if s[0] >= lo][0]
    b = [s for s in seq if s[0] <= hi][-1]
    dot = max(-1.0, min(1.0, sum(a[1 + i] * b[1 + i] for i in range(3))))
    return math.degrees(math.acos(dot)) / (b[0] - a[0])

def main():
    out_csv = sys.argv[1]
    hold = float(sys.argv[2]) if len(sys.argv) > 2 else 6.0
    cfg = json.load(open("/tmp/nav-live.json"))
    w, off, nm = speed_law.find_player()
    if not w:
        sys.exit("player entity not found after retries")
    print(f"# locked on {nm}")
    rows = []
    # (label, throttle, axis, value) — throttle pins the speed regime, so the
    # Max/Min pair in the definition can be told apart.
    # LX is ROLL (the forward vector barely moves under it) and the right stick
    # does not steer at all — measured. LY is pitch, and `vgamepad` documents
    # `LY: -1 = up`, so LY=+1 is nose DOWN = the `PitchMinus` family.
    phases = [
        ("slow_down", ("LT", 1.0), ("LY", 1.0)),   # AV_PitchMinus_Min  75 deg/s
        ("fast_down", ("RT", 1.0), ("LY", 1.0)),   # AV_PitchMinus_Max  40
        ("slow_up",   ("LT", 1.0), ("LY", -1.0)),  # AV_PitchPlus_Min  150
        ("fast_up",   ("RT", 1.0), ("LY", -1.0)),  # AV_PitchPlus_Max   70
    ]
    for label, (trig, tv), (axis, av) in phases:
        pad("trig", "RT", "0.0"); pad("trig", "LT", "0.0")
        pad("axis", "LX", "0.0"); pad("axis", "LY", "0.0"); pad("axis", "RX", "0.0")
        pad("trig", trig, str(tv))
        time.sleep(1.5)                      # let the speed settle before turning
        pad("axis", axis, str(av))
        seq, t0 = [], time.time()
        while time.time() - t0 < hold:
            f = fwd_at(w.fd, off, cfg)
            seq.append((round(time.time() - t0, 3), *f))
            time.sleep(0.05)
        pad("axis", axis, "0.0")
        for s in seq:
            rows.append((label, *s))
        try:
            r = [rate(seq, t, t + 1.0) for t in (1.0, 2.0, 3.0) if seq[-1][0] > t + 1.0]
            print(f"# {label:<11} turn rate per 1 s window: " + " ".join(f"{x:6.1f}" for x in r) + " deg/s")
        except Exception as e:
            print(f"# {label:<11} rate failed: {e}")
    pad("trig", "RT", "0.0"); pad("trig", "LT", "0.0"); pad("reset")
    with open(out_csv, "w") as f:
        f.write("phase,t,fx,fy,fz\n")
        for r in rows:
            f.write(",".join(str(x) for x in r) + "\n")
    print(f"# wrote {out_csv} ({len(rows)} samples)")

if __name__ == "__main__":
    main()