#!/usr/bin/env python3
"""Does the angular rate RAMP after the stick goes over?

Burst measurements came out a strikingly flat 0.60-0.64x of the roll cap and
1.07-1.21x of the pitch cap. A clock error cannot do that (it would move both the
same way), so something per-axis is going on, and the cheapest candidate is that a
short burst never reaches the steady rate — an angular-acceleration ramp would make
a 1-second burst under-read.

Rather than compare separate bursts (which differ in speed, attitude and starting
conditions), measure INSIDE one hold: successive 0.25 s windows of a single 3 s
press. A ramp shows up as the first windows reading low and the later ones
plateauing, with everything else held constant by construction.

Windows, never per-read deltas: polling outruns the guest's update of these fields.

Usage: ramp_probe.py <roll|pitch> <out.csv> [hold_s] [repeats]
"""
import json
import math
import os
import struct
import sys
import time

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import speed_law  # noqa: E402
from axis_probe import alive, pad_state, pin_rows, rows_at, dot  # noqa: E402

DRIVER = {"roll": {"lx": 32767}, "pitch": {"ly": 32767}}
WIN = 0.25


def pos_at(fd, off):
    return struct.unpack(">3f", os.pread(fd, 12, off))


def main():
    axis, out_csv = sys.argv[1], sys.argv[2]
    hold = float(sys.argv[3]) if len(sys.argv) > 3 else 3.0
    reps = int(sys.argv[4]) if len(sys.argv) > 4 else 2
    cfg = json.load(open("/tmp/nav-live.json"))
    w, off, nm = speed_law.find_player()
    if not w:
        sys.exit("player entity not found")
    f_i = cfg["fwd_row"]
    if not alive(w, off, cfg):
        sys.exit("craft is not moving")
    u_i, w_i = pin_rows(w, off, cfg)
    print(f"# locked on {nm}, {axis} ramp over {hold}s in {WIN}s windows")

    rows = []
    for rep in range(reps):
        pad_state()                       # cruise: least speed bleed of the three
        time.sleep(6.0)
        prev_r, prev_p = rows_at(w.fd, off, cfg), pos_at(w.fd, off)
        pad_state(**DRIVER[axis])
        t0 = wt0 = time.time()
        path = swept = 0.0
        seq = []
        while time.time() - t0 < hold:
            time.sleep(0.02)
            now = time.time()
            cur_r, cur_p = rows_at(w.fd, off, cfg), pos_at(w.fd, off)
            path += math.dist(cur_p, prev_p)
            if axis == "roll":
                d = math.atan2(dot(cur_r[u_i], prev_r[w_i]), dot(cur_r[u_i], prev_r[u_i]))
            else:
                d = math.atan2(dot(cur_r[f_i], prev_r[u_i]), dot(cur_r[f_i], prev_r[f_i]))
            swept += abs(math.degrees(d))
            prev_r, prev_p = cur_r, cur_p
            if now - wt0 >= WIN:
                span = now - wt0
                seq.append((round(now - t0, 2), round(path / span, 1), round(swept / span, 1)))
                wt0, path, swept = now, 0.0, 0.0
        pad_state()
        for t, sp, rt in seq:
            rows.append((rep, t, sp, rt))
        print(f"#  rep{rep}: " + "  ".join(f"{rt:.0f}" for _, _, rt in seq))
        print(f"#         speed " + "  ".join(f"{sp:.0f}" for _, sp, _ in seq))
        if not alive(w, off, cfg):
            print("#  CRAFT STOPPED MOVING — aborting")
            break
        time.sleep(2.0)

    with open(out_csv, "w") as f:
        f.write("rep,t,speed,rate_deg_per_wall_s\n")
        for r in rows:
            f.write(",".join(str(x) for x in r) + "\n")
    print(f"# wrote {out_csv}")


if __name__ == "__main__":
    raise SystemExit(main())