#!/usr/bin/env python3
"""Is the `_Min`/`_Max` pair a LERP on speed, and is roll really speed-independent?

Pitch measured 87 deg/s at minimum speed and 54 at maximum, against
`AV_PitchMinus_{Min,Max}` 75/40 — consistent with either a two-state switch or an
interpolation. A THIRD point settles it: at half throttle the craft sits midway
between cruise and maximum, so an interpolation must put the pitch rate between the
two, while a switch cannot.

Roll measured ~144 and ~150 in the two regimes, but only 2 s apart — not enough
settle for a 126 -> 1342 speed change. Here every phase settles 5 s.

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

SETTLE = 5.0

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):
    return math.degrees(math.acos(max(-1.0, min(1.0, sum(a[i] * b[i] for i in range(3))))))

def main():
    out_csv = 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}")
    fwd, other = cfg.get("fwd_row", 0), (1 if cfg.get("fwd_row", 0) != 1 else 2)
    rows = []
    phases = [
        # label,        throttle,        stick,        matrix row to watch
        ("pitch_slow",  ("LT", 1.0),  ("LY", 1.0),  fwd),
        ("pitch_mid",   ("RT", 0.5),  ("LY", 1.0),  fwd),
        ("pitch_fast",  ("RT", 1.0),  ("LY", 1.0),  fwd),
        ("roll_slow",   ("LT", 1.0),  ("LX", 1.0),  other),
        ("roll_fast",   ("RT", 1.0),  ("LX", 1.0),  other),
    ]
    for label, (trig, tv), (axis, av), row in phases:
        pad("trig", "RT", "0.0"); pad("trig", "LT", "0.0")
        pad("axis", "LX", "0.0"); pad("axis", "LY", "0.0")
        pad("trig", trig, str(tv))
        time.sleep(SETTLE)                      # 5 s: a full 126 -> 1342 change needs ~2 s
        # speed at the moment of the turn, so the regime is recorded not assumed
        p0 = speed_law.pos_at(w.fd, off); time.sleep(0.6); p1 = speed_law.pos_at(w.fd, off)
        v_at_turn = math.dist(p0, p1) / 0.6
        pad("axis", axis, str(av))
        seq, t0 = [], time.time()
        while time.time() - t0 < dwell:
            seq.append((round(time.time() - t0, 3), *row_at(w.fd, off, cfg, row)))
            time.sleep(0.05)
        pad("axis", axis, "0.0")
        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]))
        mean = sum(rates) / len(rates)
        print(f"# {label:<11} speed {v_at_turn:6.0f}/s   rate {mean:6.1f} deg/s   " +
              " ".join(f"{r:5.0f}" for r in rates))
    pad("trig", "RT", "0.0"); pad("trig", "LT", "0.0"); pad("reset")
    with open(out_csv, "w") as f:
        f.write("phase,t,ax,ay,az\n")
        for r in rows:
            f.write(",".join(str(x) for x in r) + "\n")
    print(f"# wrote {out_csv}")

if __name__ == "__main__":
    main()