#!/usr/bin/env python3
"""Two measurements the first flight-model pass left open, in ONE flight.

1. **The `LT` half of the throttle curve** — `RT` was walked 0→1 and gave
   `target = CruisingVelocity + RT·(MaximumVelocity − CruisingVelocity)`; `LT`
   should mirror it down to `MinimumVelocity`.
2. **Roll rate** vs `AV_Roll_{Min,Max}` (200 / 125 °/s). Roll turns the craft about
   its own forward axis, so the forward vector barely moves (3–5 °/s residual, which
   is how `LX` was identified as roll at all) — measure a DIFFERENT matrix row.

Binding happens first and fast: the entity scan searches *changing* position
triples, so it only sees the craft while it still has speed. A mission left idling
drops out of the scan entirely.

Usage: flight_law2.py <out-prefix> [dwell_s]
"""
import json, math, os, struct, subprocess, sys, time
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import speed_law

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

def row_at(fd, off, cfg, row):
    at = off + cfg["rot_delta"] + 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
    return tuple(c / n for c in v)

def ang(a, b):
    d = max(-1.0, min(1.0, sum(a[i] * b[i] for i in range(3))))
    return math.degrees(math.acos(d))

def main():
    prefix = sys.argv[1]
    dwell = float(sys.argv[2]) if len(sys.argv) > 2 else 4.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}")

    # ── 1. LT curve (speed from position) ─────────────────────────────────────
    lt_rows = []
    for v in (0.0, 0.25, 0.5, 0.75, 1.0):
        pad("trig", "RT", "0.0"); pad("trig", "LT", str(v))
        time.sleep(2.0)
        seq, t0 = [], time.time()
        while time.time() - t0 < dwell:
            seq.append((round(time.time() - t0, 3), *speed_law.pos_at(w.fd, off)))
            time.sleep(0.05)
        lt_rows += [(v, *s) for s in seq]
        a, b = seq[0], seq[-1]
        print(f"# LT={v:<5} speed {math.dist(a[1:], b[1:]) / (b[0] - a[0]):7.0f}/s")

    # ── 2. Roll rate, measured on a non-forward row ───────────────────────────
    roll_rows = []
    other = 1 if cfg.get("fwd_row", 0) != 1 else 2
    for label, trig in (("slow_roll", "LT"), ("fast_roll", "RT")):
        pad("trig", "RT", "0.0"); pad("trig", "LT", "0.0")
        pad("trig", trig, "1.0")
        time.sleep(2.0)
        pad("axis", "LX", "1.0")
        seq, t0 = [], time.time()
        while time.time() - t0 < dwell:
            seq.append((round(time.time() - t0, 3), *row_at(w.fd, off, cfg, other)))
            time.sleep(0.05)
        pad("axis", "LX", "0.0")
        roll_rows += [(label, *s) for s in seq]
        rates = []
        for t in range(int(dwell) - 1):
            a = [s for s in seq if s[0] >= t][0]
            b = [s for s in seq if s[0] <= t + 1][-1]
            rates.append(ang(a[1:], b[1:]) / (b[0] - a[0]))
        print(f"# {label:<10} row {other} turn rate: " + " ".join(f"{r:6.1f}" for r in rates) + " deg/s")
    pad("trig", "RT", "0.0"); pad("trig", "LT", "0.0"); pad("reset")

    with open(prefix + "-lt.csv", "w") as f:
        f.write("lt,t,x,y,z\n")
        for r in lt_rows:
            f.write(",".join(str(x) for x in r) + "\n")
    with open(prefix + "-roll.csv", "w") as f:
        f.write("phase,t,ux,uy,uz\n")
        for r in roll_rows:
            f.write(",".join(str(x) for x in r) + "\n")
    print(f"# wrote {prefix}-lt.csv and {prefix}-roll.csv")

if __name__ == "__main__":
    main()