#!/usr/bin/env python3
"""test_gamepad_teleop.py — gamepad_teleop.py の確かめ (アームなし・コントローラーなし. 依頼 68).

  python3 tools/test_gamepad_teleop.py -v

中身:
  - ボタン番号と操作の対応 (依頼 68 の表) / 十字キー上下の向き / △ で j1 → j5 → j1
  - 守り: j1 は 90.87 を超えない / j0 ±15°・j1〜j5 ±10° / J4_SAFE / HOME のあと基準が HOME
  - 停止: L2・R2 の両方 > 0.0 で止まる・片方では止まらない / 停止中は受け付けない / OPTIONS で戻る / 接続が切れたら停止
  - HOME: 1 秒に満たない長押しでは送らない / 1 秒続けたら 1 回だけ送る
  - 動いている間の押しは受け付けない (ためない)
  - daemon への送り方: 停止が立っていれば送らない / 届かなくても送り直さない
  - ドライランで main() がひととおり動く (偽のコントローラー・ダミーの画面・一時フォルダ)
  - codex R1 の指摘: 範囲外の出発点 / 目標と読んだ角度のずれ / 長押しの持ち越し / 停止と送信の順番 /
    再開で古い仕事が生き返らない / 読み取り失敗 / 記録の失敗 / ボタンより先に L2+R2 を見る
KIOXIA にもアームにも触らない.
"""
import json
import os
import sys
import tempfile
import threading
import unittest
from pathlib import Path
from unittest import mock

os.environ.setdefault("SDL_VIDEODRIVER", "dummy")
os.environ.setdefault("SDL_AUDIODRIVER", "dummy")

import numpy as np  # noqa: E402

sys.path.insert(0, str(Path(__file__).resolve().parent))
import gamepad_teleop as gt  # noqa: E402

HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0]


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

    def write(self, rec):
        json.dumps(rec, ensure_ascii=False)      # JSON にできること
        self.rows.append(rec)


class FakeArm:
    execute = False

    def __init__(self, start=HOME, reach=True):
        self.q = np.asarray(start, dtype=float).copy()
        self.sent = []
        self.grips = []
        self.reach = reach

    def read(self):
        return self.q.copy()

    def send_and_check(self, target, gate, quitting):
        if not gate.run(lambda: self.sent.append([float(v) for v in target])):
            return {"sent": False, "why": "halted_before_send"}
        if self.reach:
            self.q = np.asarray(target, dtype=float).copy()
        return {"sent": True, "reached": self.reach, "reached_angles": gt._r(self.q), "reach_err": 0.0}

    def gripper(self, state, gate):
        return gate.run(lambda: self.grips.append(state))


def make(start=HOME, reach=True, sync=True):
    arm = FakeArm(start, reach)
    log = ListLog()
    spawned = []
    spawn = (lambda fn: fn()) if sync else spawned.append
    tp = gt.Teleop(arm, start, log, out=lambda *a: None, spawn=spawn)
    return tp, arm, log, spawned


def press(tp, b, now=0.0):
    tp.press(b, now)
    tp.release(b, now)
    tp.poll()


class TestButtonMap(unittest.TestCase):
    def test_numbers(self):
        self.assertEqual((gt.BTN_CROSS, gt.BTN_CIRCLE, gt.BTN_SQUARE, gt.BTN_TRIANGLE), (0, 1, 2, 3))
        self.assertEqual((gt.BTN_SHARE, gt.BTN_HOME, gt.BTN_OPTIONS, gt.BTN_L3, gt.BTN_R3), (4, 5, 6, 7, 8))
        self.assertEqual((gt.BTN_L1, gt.BTN_R1), (9, 10))
        self.assertEqual((gt.BTN_UP, gt.BTN_DOWN, gt.BTN_LEFT, gt.BTN_RIGHT, gt.BTN_TOUCHPAD), (11, 12, 13, 14, 15))
        self.assertEqual((gt.AXIS_L2, gt.AXIS_R2), (4, 5))

    def test_dpad_left_right_always_j0(self):
        for sel in range(5):
            tp, arm, log, _ = make()
            tp.sel = sel
            press(tp, gt.BTN_RIGHT)
            self.assertEqual(log.rows[-1]["joint"], 0)
            self.assertAlmostEqual(arm.sent[-1][0], 0.5)
            press(tp, gt.BTN_LEFT)
            press(tp, gt.BTN_LEFT)
            self.assertAlmostEqual(arm.sent[-1][0], -0.5)
            self.assertEqual(arm.sent[-1][1:], HOME[1:])

    def test_up_minus_down_plus_starts_j1(self):
        tp, arm, log, _ = make()
        self.assertEqual(tp.joint, 1)
        press(tp, gt.BTN_DOWN)
        self.assertAlmostEqual(arm.sent[-1][1], 0.5)
        press(tp, gt.BTN_UP)
        press(tp, gt.BTN_UP)
        self.assertAlmostEqual(arm.sent[-1][1], -0.5)

    def test_triangle_cycles(self):
        tp, arm, log, _ = make()
        seen = [tp.joint]
        for _ in range(5):
            press(tp, gt.BTN_TRIANGLE)
            seen.append(tp.joint)
        self.assertEqual(seen, [1, 2, 3, 4, 5, 1])
        self.assertEqual(arm.sent, [])                      # 切り替えだけでは送らない
        tp.sel = 3                                           # j4
        press(tp, gt.BTN_DOWN)
        self.assertAlmostEqual(arm.sent[-1][4], 0.5)

    def test_r1_coarse_step(self):
        tp, arm, log, _ = make()
        tp.press(gt.BTN_R1, 0.0)
        press(tp, gt.BTN_RIGHT)
        self.assertAlmostEqual(arm.sent[-1][0], 2.0)
        press(tp, gt.BTN_DOWN)
        self.assertAlmostEqual(arm.sent[-1][1], 2.0)
        tp.release(gt.BTN_R1, 0.0)
        press(tp, gt.BTN_RIGHT)
        self.assertAlmostEqual(arm.sent[-1][0], 2.5)

    def test_gripper(self):
        tp, arm, log, _ = make()
        press(tp, gt.BTN_CROSS)
        self.assertEqual(arm.grips, [1])
        self.assertEqual(tp.grip, "closed")
        press(tp, gt.BTN_CIRCLE)
        self.assertEqual(arm.grips, [1, 0])
        self.assertEqual(tp.grip, "open")

    def test_unassigned_do_nothing(self):
        tp, arm, log, _ = make()
        for b in (gt.BTN_SQUARE, gt.BTN_SHARE, gt.BTN_L1, gt.BTN_L3, gt.BTN_R3, gt.BTN_TOUCHPAD):
            press(tp, b)
        self.assertEqual(arm.sent, [])
        self.assertEqual(arm.grips, [])
        self.assertTrue(all(r["kind"] == "ignored" for r in log.rows))
        self.assertEqual(tp.joint, 1)


class TestGuards(unittest.TestCase):
    def test_j1_never_above_9087(self):
        start = [0.0, 85.0, 0.0, 0.0, 0.0, -30.0]
        tp, arm, log, _ = make(start)
        tp.r1 = True
        for _ in range(6):
            press(tp, gt.BTN_DOWN)
        self.assertTrue(all(s[1] <= 90.87 + 1e-9 for s in arm.sent))
        self.assertAlmostEqual(arm.sent[-1][1], 90.87)
        clipped = [r for r in log.rows if r.get("clip")]
        self.assertEqual(clipped[0]["clip"][0]["limit"], "J1_MAX")
        refused = [r for r in log.rows if r.get("refused")]
        self.assertTrue(refused and refused[-1]["refused"][0]["why"] == "at_limit")
        self.assertIn("J1_MAX", tp.last_limit)

    def test_j0_15_and_others_10(self):
        tp, arm, log, _ = make()
        tp.r1 = True
        for _ in range(12):
            press(tp, gt.BTN_RIGHT)
        self.assertAlmostEqual(max(s[0] for s in arm.sent), 15.0)
        for _ in range(20):
            press(tp, gt.BTN_LEFT)
        self.assertAlmostEqual(min(s[0] for s in arm.sent), -15.0)
        tp.sel = 1                                          # j2
        for _ in range(10):
            press(tp, gt.BTN_UP)
        self.assertAlmostEqual(min(s[2] for s in arm.sent), -10.0)
        tp.sel = 4                                          # j5 (基準 -30)
        for _ in range(10):
            press(tp, gt.BTN_DOWN)
        self.assertAlmostEqual(max(s[5] for s in arm.sent), -20.0)

    def test_j4_safe(self):
        start = [0.0, 0.0, 0.0, 0.0, 55.0, -30.0]
        tp, arm, log, _ = make(start)
        tp.sel = 3
        tp.r1 = True
        for _ in range(5):
            press(tp, gt.BTN_DOWN)
        self.assertAlmostEqual(max(s[4] for s in arm.sent), 60.0)
        self.assertEqual([r for r in log.rows if r.get("clip")][0]["clip"][0]["limit"], "J4_SAFE")

    def test_window_function(self):
        lo, hi, lo_src, hi_src = gt.joint_window([0, 85, 0, 0, 55, -30])
        self.assertAlmostEqual(hi[1], 90.87)
        self.assertEqual(hi_src[1], "J1_MAX")
        self.assertAlmostEqual(hi[4], 60.0)
        self.assertEqual(hi_src[4], "J4_SAFE")
        self.assertAlmostEqual(lo[0], -15.0)
        self.assertAlmostEqual(hi[2], 10.0)

    def test_no_compensation_other_joints(self):
        start = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0]
        p = gt.plan_step(start, 1, 20.0, start)
        self.assertAlmostEqual(p["target"][1], 10.0)
        self.assertEqual(list(p["target"][[0, 2, 3, 4, 5]]), [0.0, 0.0, 0.0, 0.0, -30.0])

    def test_out_of_window_start_refuses(self):
        # 起動したときに j1 が 91 (守りの外): j1 を下げる (−) 押しも, 他の関節の押しも送らない
        start = [0.0, 91.0, 0.0, 0.0, 0.0, -30.0]
        p = gt.plan_step(start, 0, 0.5, start)
        self.assertIsNone(p["target"])
        self.assertEqual(p["refused"][0]["joint"], 1)

    def test_home_rebases(self):
        start = [10.0, 5.0, 0.0, 0.0, 0.0, -30.0]
        tp, arm, log, _ = make(start)
        tp.press(gt.BTN_HOME, 0.0)
        tp.tick(1.01)
        tp.poll()
        self.assertEqual(arm.sent[-1], HOME)
        self.assertEqual(list(tp.base), HOME)
        tp.r1 = True
        for _ in range(10):
            press(tp, gt.BTN_LEFT)
        self.assertAlmostEqual(arm.sent[-1][0], -15.0)       # 基準が HOME (j0=0) になった

    def test_home_bypasses_range(self):
        # 範囲の外 (j1=95) にいても HOME は送れる
        start = [0.0, 95.0, 0.0, 0.0, 0.0, -30.0]
        tp, arm, log, _ = make(start)
        press(tp, gt.BTN_DOWN)
        self.assertEqual(arm.sent, [])
        tp.press(gt.BTN_HOME, 0.0)
        tp.tick(1.0)
        tp.poll()
        self.assertEqual(arm.sent, [HOME])

    def test_start_from_measured_when_not_reached(self):
        tp, arm, log, _ = make(reach=False)
        press(tp, gt.BTN_RIGHT)                    # 届かない (仮の姿勢は HOME のまま)
        self.assertFalse(tp.cmd_reached)
        press(tp, gt.BTN_RIGHT)                    # 次は読んだ角度 (0) から
        self.assertAlmostEqual(arm.sent[-1][0], 0.5)
        self.assertEqual(log.rows[-1]["start_source"], "measured")


class TestCodexR1(unittest.TestCase):
    def test_selected_joint_out_of_window_not_clamped(self):
        # 基準 j1=100 (守りの外) から −0.5 を押しても 90.87 へ 1 回で動かさない
        start = [0.0, 100.0, 0.0, 0.0, 0.0, -30.0]
        p = gt.plan_step(start, 1, -0.5, start)
        self.assertIsNone(p["target"])
        self.assertEqual(p["refused"][0]["why"], "start_out_of_window")
        tp, arm, log, _ = make(start)
        press(tp, gt.BTN_UP)
        self.assertEqual(arm.sent, [])

    def test_physical_limit_ends(self):
        base = [0.0, 0.0, 170.0, 0.0, 0.0, -30.0]       # j2 の物理可動域は ±177
        p = gt.plan_step(base, 2, 20.0, base)
        self.assertAlmostEqual(p["target"][2], 177.0)
        self.assertEqual(p["clip"][0]["limit"], "PHYS")

    def test_gap_between_command_and_measured(self):
        tp, arm, log, _ = make()
        press(tp, gt.BTN_RIGHT)                         # 目標 j0=0.5 に届いた
        arm.q[0] = 10.0                                 # 手で動かされた
        press(tp, gt.BTN_RIGHT)
        self.assertAlmostEqual(arm.sent[-1][0], 10.5)   # 今読んだ角度から 0.5
        self.assertEqual(log.rows[-1]["start_source"], "measured")
        arm.q[0] = -20.0                                # 基準から ±15 の外 → 送らない
        press(tp, gt.BTN_RIGHT)
        self.assertEqual(len(arm.sent), 2)

    def test_home_hold_across_busy_is_dropped(self):
        tp, arm, log, spawned = make(sync=False)
        press(tp, gt.BTN_RIGHT)                         # 動いている
        tp.press(gt.BTN_HOME, 0.0)                      # busy 中に押し始め
        spawned.pop()()
        tp.poll()                                       # 0.5 秒で空く
        tp.tick(1.5)
        self.assertEqual(spawned, [])
        self.assertIn(("home", False, "busy"), [(r["kind"], r.get("accepted"), r.get("reason")) for r in log.rows])

    def test_home_hold_dropped_when_other_op_starts(self):
        tp, arm, log, spawned = make(sync=False)
        tp.press(gt.BTN_HOME, 0.0)
        press(tp, gt.BTN_RIGHT)                         # 長押しの途中で j0 を動かした
        tp.tick(0.2)
        spawned.pop()()
        tp.poll()
        tp.tick(1.5)
        self.assertEqual(spawned, [])
        self.assertIn("hold_cancelled", [r.get("reason") for r in log.rows])

    def test_home_hold_dropped_by_stop_and_disconnect(self):
        tp, arm, log, _ = make()
        tp.press(gt.BTN_HOME, 0.0)
        tp.set_triggers(1, 1)
        tp.set_triggers(-1, -1)
        press(tp, gt.BTN_OPTIONS)
        tp.tick(1.5)
        self.assertEqual(arm.sent, [])
        tp.r1 = True
        tp.press(gt.BTN_HOME, 2.0)
        tp.controller_lost("controller_disconnected")
        self.assertFalse(tp.r1)
        press(tp, gt.BTN_OPTIONS)
        tp.tick(5.0)
        self.assertEqual(arm.sent, [])

    def test_resume_does_not_revive_old_job(self):
        tp, arm, log, spawned = make(sync=False)
        press(tp, gt.BTN_RIGHT)
        tp.set_triggers(1, 1)
        tp.set_triggers(-1, -1)
        press(tp, gt.BTN_OPTIONS)                       # すぐ再開
        self.assertFalse(tp.stopped)
        spawned.pop()()                                 # 止める前の仕事が走る
        tp.poll()
        self.assertEqual(arm.sent, [])

    def test_home_read_failure_does_not_send(self):
        tp, arm, log, _ = make([5.0, 0, 0, 0, 0, -30])
        reads = iter([None, np.array(HOME)])
        arm.read = lambda: next(reads)
        tp.press(gt.BTN_HOME, 0.0)
        tp.tick(1.0)
        tp.poll()
        self.assertEqual(arm.sent, [])
        self.assertTrue(tp.stopped)

    def test_log_failure_blocks_resume(self):
        tp, arm, log, _ = make()

        def bad(rec):
            raise OSError("disk gone")
        log.write = bad
        press(tp, gt.BTN_TRIANGLE)
        self.assertTrue(tp.stopped)
        press(tp, gt.BTN_OPTIONS)
        self.assertTrue(tp.stopped)
        press(tp, gt.BTN_RIGHT)
        self.assertEqual(arm.sent, [])

    def test_loop_reads_triggers_before_buttons(self):
        import pygame
        pygame.init()
        screen = pygame.display.set_mode((200, 100))
        fonts = {k: pygame.font.SysFont("menlo", 12) for k in ("big", "mono", "small")}
        js = FakeJoystick(0)
        tp, arm, log, _ = make()
        frames = {
            0: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)),
            2: lambda: (js.axes.__setitem__(slice(4, 6), [1.0, 1.0]),
                        pygame.event.post(pygame.event.Event(pygame.JOYBUTTONDOWN, instance_id=7, button=gt.BTN_RIGHT, joy=0))),
            4: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)),
        }
        with mock.patch.object(gt.pygame.joystick, "Joystick", lambda i: js):
            gt.run_loop(tp, screen, fonts, "TEST", on_frame=lambda n, t: frames.get(n, lambda: None)())
        self.assertEqual(arm.sent, [])
        self.assertTrue(tp.stopped)
        # 読み取りに失敗したコマのボタンも処理しない
        js2 = FakeJoystick(0)
        js2.get_axis = lambda a: (_ for _ in ()).throw(pygame.error("gone"))
        tp2, arm2, log2, _ = make()
        frames2 = {
            0: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)),
            2: lambda: pygame.event.post(pygame.event.Event(pygame.JOYBUTTONDOWN, instance_id=7, button=gt.BTN_RIGHT, joy=0)),
            4: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)),
        }
        with mock.patch.object(gt.pygame.joystick, "Joystick", lambda i: js2):
            gt.run_loop(tp2, screen, fonts, "TEST", on_frame=lambda n, t: frames2.get(n, lambda: None)())
        self.assertEqual(arm2.sent, [])
        self.assertTrue(tp2.stopped)
        pygame.quit()


class TestShutdown(unittest.TestCase):
    def test_uncommitted_job_is_not_sent_at_exit(self):
        tp, arm, log, spawned = make(sync=False)
        press(tp, gt.BTN_RIGHT)
        tp.shutdown()                                # 仕事はまだ走っていない
        spawned.pop()()                              # 終了のあとに走っても送らない
        self.assertEqual(arm.sent, [])
        last = [r for r in log.rows if r["kind"] == "move"][0]
        self.assertIs(last["send_committed_before_exit"], False)
        self.assertTrue(last["interrupted_at_exit"])

    def test_committed_send_is_waited_and_recorded(self):
        tp, arm, log, _ = make(sync=False)
        release = threading.Event()
        entered = threading.Event()
        orig = arm.send_and_check

        def slow(target, gate, quitting):
            def fn():
                entered.set()
                release.wait(2.0)
                arm.sent.append([float(v) for v in target])
            if not gate.run(fn):
                return {"sent": False, "why": "halted_before_send"}
            return {"sent": True, "reached": None, "interrupted": True}
        arm.send_and_check = slow
        tp.spawn = tp._spawn_thread
        press(tp, gt.BTN_RIGHT)
        entered.wait(1.0)
        threading.Timer(0.2, release.set).start()
        done = tp.shutdown()                          # 送ると決めた送信が戻るまで待つ
        self.assertTrue(done)
        self.assertEqual(len(arm.sent), 1)
        move = [r for r in log.rows if r["kind"] == "move"][0]
        self.assertIs(move["send_committed_before_exit"], True)

    def test_worker_not_done_returns_false(self):
        tp, arm, log, _ = make(sync=False)
        entered = threading.Event()

        def stuck(target, gate, quitting):
            gate.run(lambda: (entered.set(), threading.Event().wait(1.0)))
            return {"sent": True}
        arm.send_and_check = stuck
        tp.spawn = tp._spawn_thread
        press(tp, gt.BTN_RIGHT)
        entered.wait(1.0)
        with mock.patch.object(gt, "WORKER_JOIN_SEC", 0.1):
            self.assertFalse(tp.shutdown())           # 戻らない → main はソケットを閉じない


class TestStop(unittest.TestCase):
    def test_both_triggers(self):
        tp, arm, log, _ = make()
        tp.set_triggers(0.5, -1.0)
        self.assertFalse(tp.stopped)
        tp.set_triggers(-1.0, 0.9)
        self.assertFalse(tp.stopped)
        tp.set_triggers(0.0, 0.0)                  # ちょうど 0.0 は超えていない
        self.assertFalse(tp.stopped)
        tp.set_triggers(0.01, 0.01)
        self.assertTrue(tp.stopped)
        self.assertEqual(tp.state, gt.STATE_STOPPED)

    def test_stopped_rejects_and_options_resumes(self):
        tp, arm, log, _ = make()
        tp.set_triggers(1.0, 1.0)
        for b in (gt.BTN_RIGHT, gt.BTN_DOWN, gt.BTN_CROSS, gt.BTN_CIRCLE):
            press(tp, b)
        tp.press(gt.BTN_HOME, 0.0)
        tp.tick(2.0)
        tp.release(gt.BTN_HOME, 2.0)
        self.assertEqual(arm.sent, [])
        self.assertEqual(arm.grips, [])
        self.assertTrue(all(r.get("reason") == "stopped" for r in log.rows
                            if r["kind"] in ("move", "grip_close", "grip_open", "home")))
        press(tp, gt.BTN_OPTIONS)                  # まだ L2+R2 を押している → 戻らない
        self.assertTrue(tp.stopped)
        tp.set_triggers(-1.0, -1.0)
        press(tp, gt.BTN_OPTIONS)
        self.assertFalse(tp.stopped)
        press(tp, gt.BTN_RIGHT)
        self.assertEqual(len(arm.sent), 1)

    def test_disconnect_and_read_error(self):
        tp, arm, log, _ = make()
        tp.controller_lost("controller_disconnected")
        self.assertTrue(tp.stopped)
        self.assertEqual(log.rows[-1]["kind"], "controller_lost")
        tp2, arm2, log2, _ = make()
        tp2.controller_read_failed("x")
        tp2.controller_read_failed("x")
        self.assertTrue(tp2.stopped)
        self.assertEqual(sum(r["kind"] == "read_error" for r in log2.rows), 1)
        press(tp2, gt.BTN_OPTIONS)                 # 読めないうちは戻らない
        self.assertTrue(tp2.stopped)
        tp2.controller_read_ok()
        press(tp2, gt.BTN_OPTIONS)
        self.assertFalse(tp2.stopped)

    def test_stop_while_moving_blocks_send(self):
        tp, arm, log, spawned = make(sync=False)
        press(tp, gt.BTN_RIGHT)                    # 仕事を作ったが, まだ走らせていない
        self.assertTrue(tp.busy)
        tp.set_triggers(1.0, 1.0)                  # 動いている間でも停止は受け付ける
        self.assertTrue(tp.stopped)
        spawned.pop()()                            # 仕事が走る: 送る直前に停止を見て送らない
        tp.poll()
        self.assertEqual(arm.sent, [])
        self.assertFalse(tp.busy)
        move = [r for r in log.rows if r["kind"] == "move"][0]
        self.assertEqual(move["why"], "halted_before_send")


class TestHomeHold(unittest.TestCase):
    def test_short_press_does_not_send(self):
        tp, arm, log, _ = make()
        tp.press(gt.BTN_HOME, 10.0)
        tp.tick(10.5)
        tp.tick(10.99)
        tp.release(gt.BTN_HOME, 10.99)
        tp.poll()
        self.assertEqual(arm.sent, [])
        self.assertEqual(log.rows[-1]["kind"], "home_short")

    def test_hold_sends_once(self):
        tp, arm, log, _ = make([5.0, 0, 0, 0, 0, -30])
        tp.press(gt.BTN_HOME, 0.0)
        for t in (0.5, 1.0, 1.5, 3.0, 5.0):
            tp.tick(t)
            tp.poll()
        tp.release(gt.BTN_HOME, 5.0)
        self.assertEqual(arm.sent, [HOME])
        self.assertEqual(sum(r["kind"] == "home" for r in log.rows), 1)
        self.assertFalse(any(r["kind"] == "home_short" for r in log.rows))
        tp.press(gt.BTN_HOME, 6.0)                 # 押し直せば, また 1 回
        tp.tick(7.0)
        tp.poll()
        self.assertEqual(len(arm.sent), 2)


class TestBusy(unittest.TestCase):
    def test_presses_while_moving_are_dropped(self):
        tp, arm, log, spawned = make(sync=False)
        press(tp, gt.BTN_RIGHT)
        for b in (gt.BTN_RIGHT, gt.BTN_DOWN, gt.BTN_CROSS):
            press(tp, b)
        tp.press(gt.BTN_HOME, 0.0)
        tp.tick(1.5)
        self.assertEqual(len(spawned), 1)           # 仕事は 1 つだけ
        spawned.pop()()
        tp.poll()
        tp.tick(3.0)                                # 長押しのまま空いても, ためた HOME は出ない
        self.assertEqual(spawned, [])
        self.assertEqual(arm.sent, [[0.5, 0, 0, 0, 0, -30]])
        busy = [r for r in log.rows if r.get("reason") == "busy"]
        self.assertEqual(len(busy), 4)


class TestDaemonArm(unittest.TestCase):
    def _arm(self, reads):
        it = iter(reads)
        last = [None]

        def read():
            try:
                last[0] = next(it)
            except StopIteration:
                pass
            return None if last[0] is None else np.asarray(last[0], dtype=float)
        return read

    def test_halt_blocks_send(self):
        sends = []
        with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \
                mock.patch.object(gt, "daemon_send_angles", lambda *a, **k: sends.append(a)):
            gate, quit_ = gt.SendGate(), threading.Event()
            gate.cancel_now()
            r = gt.DaemonArm().send_and_check(np.array(HOME), gate, quit_)
        self.assertEqual(sends, [])
        self.assertFalse(r["sent"])

    def test_single_send_no_retry(self):
        sends = []
        with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \
                mock.patch.object(gt, "daemon_send_angles", lambda a, **k: sends.append((a, k))), \
                mock.patch.object(gt, "REACH_TIMEOUT_SEC", 0.3), \
                mock.patch.object(gt.wr, "_compute_arrival_timeout", lambda d: 0.1), \
                mock.patch.object(gt, "POLL_SLEEP", 0.05):
            r = gt.DaemonArm().send_and_check(np.array([5.0, 0, 0, 0, 0, -30]),
                                              gt.SendGate(), threading.Event())
        self.assertEqual(len(sends), 1)
        self.assertEqual(sends[0][1]["speed"], 20)
        self.assertFalse(r["reached"])
        self.assertTrue(r["sent"])

    def test_reached(self):
        target = [1.0, 0, 0, 0, 0, -30]
        with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME, [0.5, 0, 0, 0, 0, -30]])), \
                mock.patch.object(gt, "daemon_send_angles", lambda a, **k: None):
            r = gt.DaemonArm().send_and_check(np.array(target), gt.SendGate(), threading.Event())
        self.assertTrue(r["reached"])
        self.assertEqual(r["reach_err"], 0.5)

    def test_reach_uses_unrounded_error(self):
        target = [3.004, 0, 0, 0, 0, -30]
        with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \
                mock.patch.object(gt, "daemon_send_angles", lambda a, **k: None), \
                mock.patch.object(gt, "REACH_TIMEOUT_SEC", 0.2), \
                mock.patch.object(gt.wr, "_compute_arrival_timeout", lambda d: 0.1), \
                mock.patch.object(gt, "POLL_SLEEP", 0.05):
            r = gt.DaemonArm().send_and_check(np.array(target), gt.SendGate(), threading.Event())
        self.assertFalse(r["reached"])

    def test_read_none_during_check_is_error(self):
        with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME, None])), \
                mock.patch.object(gt, "daemon_send_angles", lambda a, **k: None):
            r = gt.DaemonArm().send_and_check(np.array([5.0, 0, 0, 0, 0, -30]), gt.SendGate(), threading.Event())
        self.assertTrue(r["sent"])
        self.assertEqual(r["error"], "read_failed_during_check")
        self.assertIsNone(r["reached"])

    def test_stop_does_not_wait_and_reports_commit(self):
        # 送ると決めて送信 (通信) の最中に停止が来ても, 停止は待たない. 決めていたことを返す.
        # 停止のあとに「送ると決める」ことはない
        gate = gt.SendGate()
        entered, release = threading.Event(), threading.Event()

        def slow_send():
            entered.set()
            release.wait(2.0)
        t = threading.Thread(target=lambda: gate.run(slow_send))
        t.start()
        entered.wait(1.0)
        got = []
        stopper = threading.Thread(target=lambda: got.append(gate.cancel_now()))
        stopper.start()
        stopper.join(0.5)
        self.assertFalse(stopper.is_alive())         # 待たされない
        self.assertEqual(got, [True])                # 停止より前に送ると決めていた
        release.set()
        t.join(2.0)
        self.assertFalse(gate.run(lambda: self.fail("取り消しのあとに送った")))
        g2 = gt.SendGate()
        self.assertFalse(g2.cancel_now())            # まだ決めていない仕事は送らない
        self.assertFalse(g2.run(lambda: self.fail("取り消しのあとに送った")))

    def test_send_uses_rpc_timeout(self):
        seen = []
        with mock.patch.object(gt.rpc, "read_angles", self._arm([HOME])), \
                mock.patch.object(gt, "daemon_send_angles", lambda a, **k: seen.append(k)):
            gt.DaemonArm().send_and_check(np.array(HOME), gt.SendGate(), threading.Event())
        self.assertEqual(seen[0]["timeout_override"], gt.SEND_RPC_TIMEOUT_SEC)

    def test_stop_log_says_whether_send_was_committed(self):
        tp, arm, log, spawned = make(sync=False)
        press(tp, gt.BTN_RIGHT)
        tp.set_triggers(1, 1)                       # まだ送ると決めていない
        stop = [r for r in log.rows if r["kind"] == "stop"][0]
        self.assertIs(stop["send_committed_before_stop"], False)

    def test_gripper_gate(self):
        calls = []
        with mock.patch.object(gt, "daemon_set_gripper_value", lambda v: calls.append(("close", v))), \
                mock.patch.object(gt, "daemon_set_gripper", lambda s, wait: calls.append(("open", s, wait))), \
                mock.patch.object(gt.rpc, "GRIPPER_SLEEP", 0.0):
            arm = gt.DaemonArm()
            self.assertTrue(arm.gripper(1, gt.SendGate()))
            self.assertTrue(arm.gripper(0, gt.SendGate()))
            g = gt.SendGate()
            g.cancel_now()
            self.assertFalse(arm.gripper(1, g))
        self.assertEqual(calls, [("close", 20), ("open", 0, gt.rpc.GRIP_OPEN_DAEMON_WAIT)])


class FakeJoystick:
    def __init__(self, idx):
        self.axes = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0]

    def get_instance_id(self):
        return 7

    def get_name(self):
        return "PS4 Controller"

    def get_guid(self):
        return "fake-guid"

    def get_numaxes(self):
        return 6

    def get_numbuttons(self):
        return 16

    def get_numhats(self):
        return 0

    def get_axis(self, a):
        return self.axes[a]


class TestDryRunMain(unittest.TestCase):
    def test_flow(self):
        import pygame
        js_holder = {}

        def make_js(idx):
            js_holder["js"] = FakeJoystick(idx)
            return js_holder["js"]

        def btn(b, down=True):
            t = pygame.JOYBUTTONDOWN if down else pygame.JOYBUTTONUP
            pygame.event.post(pygame.event.Event(t, instance_id=7, button=b, joy=0))

        # フレームごとの台本 (ドライランは 1 回 0.3 秒動いている扱い = 60fps で 18 コマ程度)
        script = {
            1: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)),
            3: lambda: (btn(gt.BTN_RIGHT), btn(gt.BTN_RIGHT, False)),
            5: lambda: (btn(gt.BTN_RIGHT), btn(gt.BTN_RIGHT, False)),       # 動いている間 → 受け付けない
            40: lambda: btn(gt.BTN_TRIANGLE),
            42: lambda: (btn(gt.BTN_DOWN), btn(gt.BTN_DOWN, False)),
            80: lambda: btn(gt.BTN_CROSS),
            82: lambda: js_holder["js"].axes.__setitem__(slice(4, 6), [0.5, 0.5]),   # 停止
            85: lambda: (btn(gt.BTN_LEFT), btn(gt.BTN_LEFT, False)),
            86: lambda: js_holder["js"].axes.__setitem__(slice(4, 6), [-1.0, -1.0]),
            88: lambda: btn(gt.BTN_OPTIONS),
            150: lambda: btn(gt.BTN_HOME),
            230: lambda: btn(gt.BTN_HOME, False),
            300: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)),
        }

        def on_frame(n, tp):
            if n in script:
                script[n]()

        with tempfile.TemporaryDirectory() as td, \
                mock.patch.object(gt, "LOG_ROOT", Path(td)), \
                mock.patch.object(gt, "ensure_ssd_mounted", lambda: None), \
                mock.patch.object(gt.pygame.joystick, "Joystick", make_js), \
                mock.patch.object(gt.rpc, "GRIPPER_SLEEP", 0.2), \
                mock.patch.object(gt, "get_daemon_socket", side_effect=AssertionError("daemon に触った")), \
                mock.patch.object(gt, "daemon_send_angles", side_effect=AssertionError("daemon に送った")):
            gt.main([], on_frame=on_frame)
            runs = list(Path(td).iterdir())
            self.assertEqual(len(runs), 1)
            run = runs[0]
            self.assertTrue(run.name.endswith("_dry"))
            rows = [json.loads(x) for x in (run / "log.jsonl").read_text().splitlines()]
            meta = json.loads((run / "meta.json").read_text())
            self.assertTrue((run / "gamepad_teleop.py").exists())
        kinds = [(r["kind"], r.get("accepted"), r.get("reason")) for r in rows]
        # 受け付けた 1 回目の行は届いてから書くので, 動いている間に断った行 (busy) が先に来る
        self.assertEqual(kinds[0], ("move", False, "busy"))
        self.assertEqual(kinds[1][:2], ("move", True))
        self.assertIn(("move", False, "busy"), kinds)
        self.assertIn(("select_joint", True, None), kinds)
        self.assertIn(("stop", True, "L2+R2"), kinds)
        self.assertIn(("move", False, "stopped"), kinds)
        self.assertIn(("resume", True, None), kinds)
        home = [r for r in rows if r["kind"] == "home"]
        self.assertEqual(len(home), 1)
        self.assertEqual(home[0]["command"], HOME)
        moves = [r for r in rows if r["kind"] == "move" and r.get("sent") is True]
        self.assertEqual(moves[0]["command"], [0.5, 0, 0, 0, 0, -30])
        self.assertEqual(moves[1]["command"], [0.5, 0.0, 0.5, 0, 0, -30])    # △ で j2 にしてから下
        for r in moves:
            for key in ("t", "button", "kind", "selected_joint", "step_deg", "measured_before",
                        "command", "reached_angles", "reached", "grip_state", "clip", "stopped"):
                self.assertIn(key, r)
        self.assertEqual(meta["controllers"][0]["name"], "PS4 Controller")
        self.assertEqual(meta["controllers"][0]["guid"], "fake-guid")
        self.assertFalse(meta["execute"])
        self.assertEqual(meta["end_reason"], "window_closed")
        self.assertEqual(meta["guards"]["J1_MAX_DEG"], 90.87)
        for key in ("python", "pygame", "sdl", "start_pose", "git"):
            self.assertIn(key, meta)


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