#!/usr/bin/env python3
"""test_diag_joint_hold.py — diag_joint_hold.py (依頼 57) の確かめ.

  - 動いている途中の遅れと, 止まったあとのずれを分ける (rpc が返ったあとの実測だけを使う)
  - 指令が変わらないのに動いた区間を「外れ」として数える. HOME は数えない
  - 爪の rpc で区間を切る
  - log.jsonl だけでも補正の 1 刻みとチャンクの区切りのずれを出す
  - KIOXIA があれば, 今日の記録で依頼文の粗い値 (j4 の最大) と同じになるか・入れる 3 本に外れがないか
  - 依頼 97: ゲームパッドの補正 (send_angle / stop の rpc 行を pad の行と組にする) と, 送ったらすぐ返る daemon の区間
"""
import csv
import json
import sys
import tempfile
import unittest
from pathlib import Path
from unittest import mock

import numpy as np

_HERE = Path(__file__).resolve().parent
sys.path.insert(0, str(_HERE))

import diag_joint_hold as d  # noqa: E402

COLS = ["ts", "event_type", "stage", "j0", "j1", "j2", "j3", "j4", "j5", "speed", "gripper_state",
        "ts_send", "ts_arrival", "attempt", "arrived", "note"]


class Rec:
    def __init__(self):
        self.rows = []

    def send(self, t, q, stage="model", rpc=1.5):
        self.rows.append({"ts": t, "event_type": "send_angles", "stage": stage, **_j(q), "note": f"rpc_sec={rpc}"})

    def rpc(self, t, cmd, stage="correct", rpc=0.03):
        self.rows.append({"ts": t, "event_type": "rpc", "stage": stage, "note": f"cmd={cmd} rpc_sec={rpc}"})

    def grip(self, t, stage="model", rpc=1.5):
        self.rows.append({"ts": t, "event_type": "rpc", "stage": stage, "note": f"cmd=set_gripper_value rpc_sec={rpc}"})

    def actual(self, t0, t1, q, stage="model", dt=0.1):
        t = t0
        while t < t1 - 1e-9:
            qq = q(t) if callable(q) else q
            self.rows.append({"ts": round(t, 3), "event_type": "actual", "stage": stage, **_j(qq)})
            t += dt

    def write(self, run_dir, log=()):
        rec = run_dir / "recording"
        rec.mkdir(parents=True)
        with (rec / "states.csv").open("w", newline="") as f:
            w = csv.DictWriter(f, fieldnames=COLS)
            w.writeheader()
            for r in self.rows:
                w.writerow({k: r.get(k, "") for k in COLS})
        (run_dir / "log.jsonl").write_text("".join(json.dumps(x) + "\n" for x in log))
        (run_dir / f"{run_dir.name}.json").write_text(json.dumps({"outcome": "success", "corrections": 1}))


def _j(q):
    return {f"j{i}": f"{v:.3f}" for i, v in enumerate(q)}


def pose(j4, j1=10.0):
    return [0.0, j1, 0.0, 0.0, j4, -30.0]


class TestSegments(unittest.TestCase):
    def build(self, rec, log=()):
        td = tempfile.mkdtemp()
        run = Path(td) / "pi05_x"
        rec.write(run, log)
        return d.summarize_run(run)

    def test_hold_vs_lag_and_events(self):
        r = Rec()
        # 1. 送って rpc が返ったあと, 指令から 1° ずれて止まる (ふつうの止まり)
        r.send(0.0, pose(-50))
        r.actual(1.5, 4.0, pose(-49))
        # 2. 送ったあと 2 秒かけて -60 → -55 へ動き (指令は -60), そのまま止まる (外れ: 指令なしに +5°)
        r.send(4.0, pose(-60))
        r.actual(5.5, 7.5, lambda t: pose(-60 + 5 * min(1.0, (t - 5.5) / 1.0)))
        r.actual(7.5, 9.0, pose(-55))
        # 3. HOME で戻らない (数えない)
        r.send(9.0, pose(0), stage="home")
        r.actual(10.5, 12.0, pose(40), stage="home")
        res, segs = self.build(r)
        self.assertEqual(len(segs), 3)
        self.assertAlmostEqual(res["hold"]["j4"]["max"], 5.0, places=1)       # 止まったあとのずれ (2.)
        self.assertAlmostEqual(res["lag_max"]["j4"], 5.0, places=1)           # 動いている途中 (2. の頭は 0° ずれ… 最後の動きで 5°)
        self.assertEqual(res["hold_by_context"]["model"]["n"], 2)
        ev = res["events"]
        self.assertEqual([(e["sec"], e["joint"]) for e in ev], [(4.0, "j4")])  # HOME は数えない
        self.assertAlmostEqual(ev[0]["hold_err"], 5.0, places=1)
        self.assertAlmostEqual(res["home"][0]["err_last"][4], 40.0, places=1)
        self.assertAlmostEqual(res["coarse_max"]["j4"], 40.0, places=1)       # 粗い値は HOME も含む
        self.assertAlmostEqual(res["hold_signed_median"]["j1"], 0.0, places=2)

    def test_no_samples_during_rpc_and_still_threshold(self):
        r = Rec()
        r.send(0.0, pose(-50), rpc=1.6)
        r.actual(0.0, 1.5, pose(-30))            # rpc の途中の値 (使わない)
        r.actual(1.6, 3.0, pose(-50.2))
        res, segs = self.build(r)
        self.assertEqual(segs[0]["n"], 14)
        self.assertEqual(res["events"], [])
        self.assertAlmostEqual(res["hold"]["j4"]["max"], 0.2, places=2)
        self.assertEqual(res["lag_max"]["j4"], 0.0)

    def test_grip_splits_segment(self):
        r = Rec()
        r.send(0.0, pose(-50))
        r.actual(1.5, 2.5, pose(-50))
        r.grip(2.5)
        r.actual(4.0, 5.0, pose(-46))             # 爪のあとで 4° 動いていた (外れの手前)
        res, segs = self.build(r)
        self.assertEqual([s["kind"] for s in segs], ["send", "grip_after"])
        self.assertEqual(res["grip"][0]["delta"][4], 4.0)
        self.assertAlmostEqual(res["hold_by_context"]["grip_after"]["j4_max"], 4.0, places=2)
        self.assertEqual(res["events"], [])        # 4.0 は外れの目安 (4° を超える) に入らない

    def test_drift_while_still(self):
        r = Rec()
        r.send(0.0, pose(-50))
        r.actual(1.5, 6.5, lambda t: pose(-50 + 0.2 * (t - 1.5)))   # 0.2°/秒 でじわじわ (動いているとはしない)
        res, _ = self.build(r)
        self.assertAlmostEqual(res["drift_max"]["j4"], 1.0, delta=0.05)
        self.assertEqual(res["lag_max"]["j4"], 0.0)

    def test_joint_wise_still(self):
        """codex R1 必須: j1 が動き続けていても, 止まった j4 の 5° のずれを見落とさない."""
        r = Rec()
        r.send(0.0, pose(-60, j1=10))
        r.actual(1.5, 4.5, lambda t: pose(-55, j1=10 + 2.0 * (t - 1.5)))   # j1 は 2°/秒 で動き続ける. j4 は -55 で止まり
        res, segs = self.build(r)
        self.assertTrue(np.isnan(segs[0]["hold_err"][1]))                    # j1 は一度も止まっていない
        self.assertAlmostEqual(segs[0]["hold_err"][4], 5.0, places=2)
        self.assertEqual([e["joint"] for e in res["events"]], ["j1", "j4"])
        self.assertEqual(res["event_segments"], 1)                           # 区間の数は 1
        self.assertIsNone(res["hold"]["j1"])

    def test_grip_after_not_still_is_none(self):
        """codex R2 必須: 爪のあとに止まらなかった関節のずれは None (最初の実測のずれで埋めない)."""
        r = Rec()
        r.send(0.0, pose(-50, j1=10))
        r.actual(1.5, 2.5, pose(-50, j1=10))
        r.grip(2.5)
        r.actual(4.0, 6.0, lambda t: pose(-50, j1=10 + 2.0 * (t - 4.0)))
        res, _ = self.build(r)
        g = res["grip"][0]
        self.assertIsNone(g["err_after"][1])
        self.assertEqual(g["err_after"][4], 0.0)
        self.assertEqual(g["err_first"][1], 0.0)

    def test_event_segments_by_index(self):
        """codex R2 推奨: 秒が近い別の区間も別に数える."""
        r = Rec()
        r.send(0.0, pose(-60), rpc=0.01)
        r.actual(0.01, 0.03, pose(-50))
        r.send(0.04, pose(-60), rpc=0.01)
        r.actual(0.05, 1.0, pose(-50))
        res, _ = self.build(r)
        self.assertEqual(res["event_segments"], 2)

    def test_home_only(self):
        """codex R1 必須: HOME の区間しかない記録でも止まらない."""
        r = Rec()
        r.send(0.0, pose(0), stage="home")
        r.actual(1.5, 3.0, pose(1), stage="home")
        res, _ = self.build(r)
        self.assertEqual(res["work_segments"], 0)
        self.assertEqual(res["events"], [])
        self.assertEqual(res["lag_max"]["j4"], 0.0)
        self.assertIsNone(res["hold"]["j4"])
        self.assertAlmostEqual(res["home"][0]["err_last"][4], 1.0, places=2)

    def test_drift_uses_last_still_run(self):
        """codex R1 推奨: 止まり中の変化は最後の止まりが続いたところだけ (途中で動いた分を入れない)."""
        r = Rec()
        r.send(0.0, pose(-50))
        r.actual(1.5, 3.0, pose(-50))
        r.actual(3.0, 3.5, lambda t: pose(-50 + 10 * (t - 3.0)))           # 途中で 5° 動く
        r.actual(3.5, 6.0, pose(-45))
        res, segs = self.build(r)
        self.assertAlmostEqual(segs[0]["drift"][4], 0.0, places=2)
        self.assertGreater(segs[0]["still_sec"][4], 1.5)
        self.assertLess(segs[0]["still_sec"][4], 2.5)

    def test_json_has_no_nan(self):
        r = Rec()
        r.send(0.0, pose(-60, j1=10))
        r.actual(1.5, 4.5, lambda t: pose(-55, j1=10 + 2.0 * (t - 1.5)))
        td = Path(tempfile.mkdtemp())
        r.write(td / "pi05_y")
        out = td / "o.json"
        with mock.patch("sys.argv", ["x", "--runs-root", str(td), "--run", "pi05_y", "--timeline", "pi05_y",
                                     "--json", str(out)]), mock.patch("sys.stdout"):
            d.main()
        self.assertNotIn("NaN", out.read_text())
        json.loads(out.read_text())
        with mock.patch("sys.argv", ["x", "--runs-root", str(td), "--run", "pi05_y", "--json", str(out)]), \
                mock.patch("sys.stdout"), self.assertRaises(SystemExit):
            d.main()                                                          # 既にあるファイルには書かない

    def test_log_hold_errors(self):
        log = [
            {"chunk": 0, "t": 1.0, "executed": True, "state_source": "measured",
             "measured_state": pose(-1) + [0.0], "sent_targets": [pose(-40)]},
            {"chunk": 1, "t": 2.0, "executed": False, "state_source": "target",
             "measured_state": pose(-37) + [0.0], "sent_targets": [pose(-45)]},
            {"type": "correction", "action": "move", "sent": True, "t": 3.0,
             "after": pose(-45), "measured_after": pose(-30)},
            {"type": "rewind", "t": 4.0},
            {"chunk": 2, "t": 5.0, "executed": True, "state_source": "measured", "measured_state": pose(9) + [0.0]},
        ]
        out = d.log_hold_errors(log)
        self.assertEqual([round(float(e[4]), 2) for _, e in out["chunk"]], [3.0])     # 実行していないチャンクの目標は使わない. 巻き戻しのあとは比べない
        self.assertEqual([round(float(e[4]), 2) for _, e in out["correction"]], [15.0])

    def test_old_record_without_send_rows(self):
        r = Rec()
        r.actual(0.0, 2.0, pose(-50))
        res, segs = self.build(r)
        self.assertFalse(res["has_send_rows"])
        self.assertEqual(segs, [])


def pad_move(t, command, joint, hold=False, resends=(), cid=1):
    r = {"type": "pad", "correction_id": cid, "kind": "move", "sent": True, "t_send": t, "command": command,
         "joint": joint, "resends": list(resends)}
    if hold:
        r["no_check"] = True
    return r


def pad_halt(t, q, joint, cid=1):
    return {"type": "pad", "correction_id": cid, "kind": "halt", "t_halt": t, "measured_after_halt": q,
            "halt_joint": joint, "halt": "stop"}


class TestPadAndFast(unittest.TestCase):
    """依頼 97: ゲームパッドの補正 (send_angle / stop の rpc 行) と, 送ったらすぐ返る daemon の区間."""

    def build(self, rec, log=()):
        td = tempfile.mkdtemp()
        run = Path(td) / "pi05_p"
        rec.write(run, log)
        return d.summarize_run(run)

    def test_fast_approach_is_not_lag(self):
        """rpc がすぐ返ると, 腕は返ったあとに指令へ向かって動く. 途中で一度止まってまた向かっても外れにしない."""
        r = Rec()
        r.send(0.0, pose(-60), rpc=0.03)
        r.actual(0.03, 0.6, pose(-50))                                         # 動き出す前 (前の姿勢のまま)
        r.actual(0.6, 1.1, lambda t: pose(-50 - 10 * (t - 0.6)))               # -55 まで
        r.actual(1.1, 1.8, pose(-55))                                          # 一度止まる
        r.actual(1.8, 2.3, lambda t: pose(-55 - 9 * (t - 1.8)))                # また向かう
        r.actual(2.3, 3.5, pose(-59.5))
        res, segs = self.build(r)
        self.assertEqual(res["fast_segments"], 1)
        self.assertEqual(res["events"], [])
        self.assertEqual(res["lag_max"]["j4"], 0.0)
        self.assertAlmostEqual(segs[0]["hold_err"][4], 0.5, places=2)
        self.assertLess(res["away_max"]["j4"], 0.2)

    def test_fast_hold_ignores_still_before_moving(self):
        """すぐ返った区間で, 動き出す前の止まり (前の姿勢) を止まったあとのずれにしない (動き続けたら None)."""
        r = Rec()
        r.send(0.0, pose(-60), rpc=0.03)
        r.actual(0.03, 0.8, pose(-50))
        r.actual(0.8, 2.0, lambda t: pose(-50 - 8 * (t - 0.8)))
        res, segs = self.build(r)
        self.assertTrue(np.isnan(segs[0]["hold_err"][4]))
        self.assertEqual(res["events"], [])

    def test_fast_sag_after_arrival_is_event(self):
        """すぐ返った区間でも, 着いたあとに指令なしで 6° 垂れたら外れ (away と止まりのずれ)."""
        r = Rec()
        r.send(0.0, pose(-60), rpc=0.03)
        r.actual(0.03, 1.0, pose(-60))
        r.actual(1.0, 1.6, lambda t: pose(-60 + 10 * (t - 1.0)))
        r.actual(1.6, 3.0, pose(-54))
        res, segs = self.build(r)
        self.assertAlmostEqual(segs[0]["away_max"][4], 6.0, delta=0.05)
        self.assertEqual([e["joint"] for e in res["events"]], ["j4"])
        self.assertAlmostEqual(res["events"][0]["away_max"], 6.0, delta=0.05)

    def test_pad_step_hold_stop(self):
        """1 刻み: 指令は pad の command. 長押し: 押した関節は見ない. stop: 止めた関節だけ止めたあとの角度."""
        start = pose(-60, j1=40)
        step = pose(-60, j1=40); step[0] = 1.0
        far = pose(-60, j1=25); far[0] = 1.0                                   # j1 を 15° 先へ (ほかは直前の指令)
        r = Rec()
        r.send(0.0, start, stage="model", rpc=0.03)
        r.actual(0.03, 1.0, start, stage="model")
        r.rpc(2.0, "send_angle")                                               # 1 刻み (j0 +1)
        r.actual(2.03, 3.0, step, stage="correct")
        r.rpc(3.0, "send_angle")                                               # 長押し (j1 → 25)
        r.actual(3.03, 3.5, lambda t: [1.0, 40 - 20 * (t - 3.03), 0, 0, -60, -30], stage="correct")
        r.rpc(3.5, "stop")
        r.actual(3.55, 5.0, [1.0, 30.6, 0, 0, -60, -30], stage="correct")
        r.rpc(5.0, "send_angle")                                               # 組にできない (pad の行がない… 送り直しの数を越える)
        r.actual(5.03, 5.5, [1.0, 30.6, 0, 0, -60, -30], stage="correct")
        log = [pad_move(2.0, step, 0), pad_move(3.0, far, 1, hold=True), pad_halt(3.5, [1.0, 30.6, 0, 0, -60.2, -30], 1)]
        res, segs = self.build(r, log)
        self.assertEqual([s["kind"] for s in segs], ["send", "pad_step", "pad_hold", "pad_stop", "pad_unknown"])
        self.assertEqual(res["pad_segments"], {"pad_step": 1, "pad_hold": 1, "pad_stop": 1, "pad_unknown": 1})
        np.testing.assert_allclose(segs[1]["cmd"], step)
        self.assertTrue(np.isnan(segs[2]["hold_err"][1]))                     # 長押しの関節は見ない
        self.assertEqual(segs[2]["lag_max"][1], 0.0)
        self.assertEqual(segs[2]["skip"], 1)
        np.testing.assert_allclose(segs[3]["cmd"], [1.0, 30.6, 0, 0, -60, -30])   # j4 は止めたあとの -60.2 にしない
        self.assertTrue(np.isnan(segs[4]["cmd"]).all())
        self.assertEqual(res["events"], [])

    def test_pad_resend_and_uncommanded_wrist(self):
        """送り直し (resends) は同じ pad の行に組む. 長押しの間に手首が指令なしで 6° 動いたら stop の区間で外れ."""
        start = pose(-60, j1=50)
        far = pose(-60, j1=35)
        r = Rec()
        r.send(0.0, start, rpc=0.03)
        r.actual(0.03, 1.0, start)
        r.rpc(2.0, "send_angle")
        r.rpc(2.3, "send_angle")                                               # 送り直し
        r.actual(2.33, 3.0, lambda t: [0, 50 - 15 * (t - 2.33), 0, 0, -60 + 9 * (t - 2.33), -30], stage="correct")
        r.rpc(3.0, "stop")
        r.actual(3.05, 4.5, [0, 40, 0, 0, -54, -30], stage="correct")
        log = [pad_move(2.0, far, 1, hold=True, resends=[0.3]), pad_halt(3.0, [0, 40, 0, 0, -54, -30], 1)]
        res, segs = self.build(r, log)
        self.assertEqual([s["kind"] for s in segs], ["send", "pad_hold", "pad_hold", "pad_stop"])
        self.assertEqual(res["pad_segments"]["pad_unknown"], 0)
        self.assertEqual([(e["kind"], e["joint"]) for e in res["events"]], [("pad_hold", "j4"), ("pad_stop", "j4")])
        self.assertAlmostEqual(segs[3]["hold_err"][4], 6.0, places=2)

    def test_pad_send_not_matched_to_next(self):
        """codex R1 必須: rpc の時刻に近い送信と組にする (0.15 秒あとの次の送信を拾わない)."""
        a_cmd = pose(-60, j1=40); a_cmd[0] = 1.0
        b_cmd = pose(-60, j1=40); b_cmd[0] = 2.0
        r = Rec()
        r.send(0.0, pose(-60, j1=40), rpc=0.03)
        r.actual(0.03, 0.9, pose(-60, j1=40))
        r.rpc(1.0, "send_angle")
        r.actual(1.03, 1.14, a_cmd, stage="correct")
        r.rpc(1.16, "send_angle")
        r.actual(1.19, 2.0, b_cmd, stage="correct")
        log = [pad_move(1.0, a_cmd, 0), pad_move(1.15, b_cmd, 0)]
        _, segs = self.build(r, log)
        np.testing.assert_allclose(segs[1]["cmd"], a_cmd)
        np.testing.assert_allclose(segs[2]["cmd"], b_cmd)

    def test_away_carries_across_fast_segments(self):
        """codex R1 必須: j4 の指令が同じまま, ほかの関節の送信で区間が切れても, 0→3° と 3→6° の垂れを 6° と見る."""
        r = Rec()
        r.send(0.0, pose(-60, j1=10), rpc=0.03)
        r.actual(0.03, 1.0, lambda t: pose(-60 + 3 * t, j1=10))
        r.send(1.0, pose(-60, j1=11), rpc=0.03)          # j1 だけ変わる (j4 の指令は同じ)
        r.actual(1.03, 2.0, lambda t: pose(-60 + 3 * t, j1=11))
        res, segs = self.build(r)
        self.assertLess(segs[0]["away_max"][4], 3.1)
        self.assertGreater(segs[1]["away_max"][4], 5.5)
        self.assertEqual([e["joint"] for e in res["events"]], ["j4"])
        r2 = Rec()                                          # j4 の指令が変われば引き継がない
        r2.send(0.0, pose(-60), rpc=0.03)
        r2.actual(0.03, 1.0, lambda t: pose(-60 + 3 * t))
        r2.send(1.0, pose(-57), rpc=0.03)
        r2.actual(1.03, 2.0, lambda t: pose(-60 + 3 * t))
        res2, _ = self.build(r2)
        self.assertEqual(res2["events"], [])

    def test_carry_cut_by_segments_without_samples(self):
        """codex R2 必須: 実測のない区間 (指令が変わる・すぐ返らない・長押し・組にできない) を挟めば引き継がない.
        指令 0°・実測 0→3° → (実測なしの区間) → 指令 0°・実測 3→6° で, 最後の away は 3° (6° にしない)."""
        def run(between, log=()):
            r = Rec()
            r.send(0.0, pose(-60), rpc=0.03)
            r.actual(0.03, 1.0, lambda t: pose(-60 + 3 * t))
            between(r)
            r.send(1.2, pose(-60), rpc=0.03)
            r.actual(1.23, 2.2, lambda t: pose(-60 + 3 * (t - 0.2)))
            _, segs = self.build(r, log)
            return segs[-1]["away_max"][4]
        self.assertLess(run(lambda r: r.send(1.0, pose(-50), rpc=0.03)), 3.1)              # 指令が変わる
        self.assertLess(run(lambda r: r.send(1.0, pose(-60), rpc=1.5)), 3.1)               # すぐ返らない
        self.assertLess(run(lambda r: r.rpc(1.0, "send_angle", stage="model")), 3.1)       # 組にできない
        far = pose(-45)
        self.assertLess(run(lambda r: r.rpc(1.0, "send_angle", stage="model"),
                            [pad_move(1.0, far, 4, hold=True)]), 3.1)                        # j4 の長押し
        self.assertGreater(run(lambda r: r.grip(1.0, rpc=0.01)), 5.5)                      # 爪だけ: 引き継ぐ
        self.assertLess(run(lambda r: r.rpc(1.0, "send_angle", stage="model"),
                            [pad_move(1.0, pose(-60), 4, hold=True)]), 3.1)                  # 同じ指令のまま j4 の長押し (codex R3)

    def test_halt_one_to_one(self):
        """codex R1 推奨: stop はいちばん近い・まだ組にしていない halt と組む (同じ halt を 2 回使わない)."""
        r = Rec()
        r.send(0.0, pose(-60, j1=40), rpc=0.03)
        r.actual(0.03, 0.9, pose(-60, j1=40))
        r.rpc(1.0, "stop", stage="correct")
        r.actual(1.03, 1.1, pose(-60, j1=35), stage="correct")
        r.rpc(1.15, "stop", stage="correct")
        r.actual(1.18, 2.0, pose(-60, j1=30), stage="correct")
        log = [pad_halt(1.0, pose(-60, j1=35), 1), pad_halt(1.15, pose(-60, j1=30), 1)]
        _, segs = self.build(r, log)
        self.assertEqual([s["kind"] for s in segs], ["send", "pad_stop", "pad_stop"])
        self.assertAlmostEqual(segs[1]["cmd"][1], 35.0)
        self.assertAlmostEqual(segs[2]["cmd"][1], 30.0)

    def test_halt_not_reused(self):
        """codex R3 推奨: halt が 1 件で stop が 2 件なら, 2 件目は pad_unknown (同じ halt を使わない)."""
        r = Rec()
        r.send(0.0, pose(-60, j1=40), rpc=0.03)
        r.actual(0.03, 0.9, pose(-60, j1=40))
        r.rpc(1.0, "stop", stage="correct")
        r.actual(1.03, 1.08, pose(-60, j1=35), stage="correct")
        r.rpc(1.1, "stop", stage="correct")
        r.actual(1.13, 2.0, pose(-60, j1=35), stage="correct")
        _, segs = self.build(r, [pad_halt(1.05, pose(-60, j1=35), 1)])
        self.assertEqual([s["kind"] for s in segs], ["send", "pad_stop", "pad_unknown"])


RUNS = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy")


@unittest.skipUnless(RUNS.is_dir(), "KIOXIA がない")
class TestToday(unittest.TestCase):
    """依頼文の粗い値 (j4 の「直前に送った指令と実測の差」の最大) と同じになり, 入れる 3 本には外れがない."""
    COARSE = {"190347": 3.5, "190854": 2.5, "192101": 1.8, "192418": 10.3, "192758": 19.5,
              "193041": 18.4, "193411": 8.0, "194042": 59.5}

    def test_coarse_and_events(self):
        for stem, v in self.COARSE.items():
            res, _ = d.summarize_run(RUNS / f"pi05_20261101_{stem}")
            self.assertAlmostEqual(res["coarse_max"]["j4"], v, delta=0.051, msg=stem)
            n_ev = len([e for e in res["events"] if e["joint"] == "j4"])
            if stem in ("190347", "190854", "192101"):
                self.assertEqual(n_ev, 0, stem)
            else:
                self.assertGreater(n_ev, 0, stem)

    def test_20261106_gamepad(self):
        """依頼 97: 11-06 の成功 5 本. rpc 行はすべて pad の行と組になり, 外れは 185554 (長押しの間に j4 +6.8°) だけ."""
        for stem in ("183926", "184452", "184650", "184906", "185554"):
            p = RUNS / f"pi05_20261106_{stem}"
            if not p.is_dir():
                self.skipTest("11-06 の記録がない")
            res, _ = d.summarize_run(p)
            self.assertEqual(res["pad_segments"]["pad_unknown"], 0, stem)
            self.assertGreater(res["fast_segments"], 0, stem)
            if stem == "185554":
                self.assertEqual({e["joint"] for e in res["events"]}, {"j4"})
                self.assertAlmostEqual(res["hold"]["j4"]["max"], 6.77, delta=0.051)
            else:
                self.assertEqual(res["events"], [], stem)


if __name__ == "__main__":
    unittest.main()
