#!/usr/bin/env python3
"""Roll rate measured ABOUT THE FORWARD AXIS, so pitch cannot leak into it.

The previous attempt watched a non-forward row of the rotation matrix and got
numbers within a few per cent of the pitch run — because pitch moves that row as
much as roll does. The fix is to measure the rotation *in the plane perpendicular
to forward*: express the new up-vector in the OLD (up, right) basis and take
`atan2(u_new·w_old, u_new·u_old)`. Any component along forward — which is what
pitch produces — is dropped by construction.

Each phase is bracketed by HUD screenshots so the mission clock converts wall
seconds to game seconds within this run (the ratio has measured 1.260, 1.311 and
1.383 in three flights, so it cannot be assumed).

Usage: roll_axis.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

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

def shot(n):
    subprocess.run(["screenshot", f"/sylph-home/re/shots/{n}.png"], capture_output=True)

def rows_at(fd, off, cfg):
    base = off + cfg["rot_delta"]
    out = []
    for r in range(3):
        v = struct.unpack(">3f", os.pread(fd, 12, base + r * cfg["rot_stride"]))
        n = math.sqrt(sum(c * c for c in v)) or 1.0
        out.append(tuple(c / n for c in v))
    return out

def dot(a, b):
    return 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 8.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}")
    f_i = cfg.get("fwd_row", 0)
    u_i, w_i = [r for r in (0, 1, 2) if r != f_i]
    rows = []
    for label, (trig, tv) in (("slow", ("LT", 1.0)), ("fast", ("RT", 1.0))):
        pad("trig", "RT", "0.0"); pad("trig", "LT", "0.0"); pad("axis", "LX", "0.0")
        pad("trig", trig, str(tv))
        time.sleep(5.0)
        shot(f"rollax_{label}_a")
        pad("axis", "LX", "1.0")
        t0, prev, swept = time.time(), rows_at(w.fd, off, cfg), 0.0
        seq = []
        while time.time() - t0 < dwell:
            time.sleep(0.05)
            cur = rows_at(w.fd, off, cfg)
            # roll = rotation of `up` within the OLD (up, right) plane
            d = math.degrees(math.atan2(dot(cur[u_i], prev[w_i]), dot(cur[u_i], prev[u_i])))
            swept += abs(d)
            seq.append((round(time.time() - t0, 3), round(d, 4)))
            prev = cur
        pad("axis", "LX", "0.0")
        wall = seq[-1][0]
        shot(f"rollax_{label}_b")
        rows += [(label, *s) for s in seq]
        print(f"# {label:<5} wall {wall:5.2f}s   roll swept {swept:7.1f}deg   "
              f"rate {swept / wall:6.1f} deg/wall-s")
    pad("trig", "RT", "0.0"); pad("trig", "LT", "0.0"); pad("reset")
    with open(out_csv, "w") as f:
        f.write("phase,t,droll_deg\n")
        for r in rows:
            f.write(",".join(str(x) for x in r) + "\n")
    print("# read HUD TIME off rollax_slow_a/b and rollax_fast_a/b for this run's clock ratio")

if __name__ == "__main__":
    main()