#!/usr/bin/env python3
"""gamepad_teleop.py — ゲームパッドで mechArm 270 を手で動かす (依頼 68. 2026-11-02)

PICK で爪の位置がずれたときに, エイイチがゲームパッドで左右・上げ下げ・爪の開閉を直す.
押した分だけそのまま動く. 自動で動く要素は入れない (自動の握り直し・失敗時の自動 HOME などはない).
押した操作と, 送った角度・届いた角度をすべて log.jsonl に残す (あとで「人が直した記録」として使う).

操作 (エイイチが決めた. 変えない):
  十字キー 左 / 右 (13 / 14)  j0 を 1° ずつ (2026-11-03 に 0.5° から変えた. 0.5° ではサーボが反応しなかった). どの関節を選んでいても いつも j0
                              (向き: 左 = J0_LEFT_SIGN. 依頼に向きの指定がないので, 右 = +, 左 = − にしてある)
  十字キー 上 / 下 (11 / 12)  選んでいる関節を 1° ずつ. 上 = −, 下 = +. 始めは j1
  十字キーの長押し            押している間, 止めずに動かし続ける. 短く押せば 1 刻み.
                              (2026-11-05: 押した瞬間には送らない. 0.4 秒より前に離せば 1 刻み, 押し続ければ長押しの
                              どちらか片方だけを送る. 動いている間に押した十字キーは, 最後の 1 つだけ 終わってから送る)
                              (2026-11-05 作り直し. 3° 先を 0.1 秒おきに送り直す形はカクついた. jog_probe の 3 回の実機で,
                              送り直すたびに加速からやり直すこと, ゆっくりの速さではどの命令も 0.2 秒ごとに止まることが分かった)
                              押して HOLD_START_SEC (0.4 秒) たっても押していれば, 「いま読んだ角度から HOLD_AHEAD_DEG (15°) 先」を
                              speed 20 (毎秒約 40°) で 1 回だけ送り, アーム自身の加速・減速で動かす. HOLD_INTERVAL_SEC (0.1 秒) おきに
                              角度を読み, 目標まで残り HOLD_REFILL_DEG (5°) 以下になったら次の 15° 先を送る.
                              離したら daemon の stop (jog_stop と stop) を送って, その場で止める (行きすぎは実機で約 3°).
                              stop が届かなくても, アームは送ってある目標 (最大 15° 先) で止まる.
                              長押しの送信では届いたかを待たない.
                              R1 は短い押しの刻み (2°) にだけ効く. 長押しの速さは speed で決まる (20 のまま).
                              離したら, 送ると決める前の送信は取り消す.
                              押し始めに受け付けられなかった押し (動いている間・停止中) は, 押し続けても長押しにしない.
                              離した合図を取りこぼしても止まるよう, 毎コマ コントローラーのボタンの状態も見る.
                              HOLD_STALL_SEC (1 秒) のあいだ動かした関節が進んでいなければ, その長押しは止める (当たっているなど).
                              離す・L2+R2・範囲の端・△・ほかのボタン・接続が切れる で長押しは止まる
  △ (3)                       上下で動かす関節を切り替える: j1 → j2 → j3 → j4 → j5 → j1
  R1 (10) を押しながら        1 回 2° ずつ (十字キーの左右・上下とも)
  × (0) / ○ (1)              爪を閉じる / 開く
  HOME (5) を 1 秒長押し      HOME [0, 0, 0, 0, 0, -30] に戻る. 1 秒続けて押したときに 1 回だけ送る
  L2 と R2 (軸 4・5) の両方が 0.0 を超えたとき  停止. 以後の動く操作を受け付けない. OPTIONS (6) で再開
                              2026-11-05 から, 停止のときも daemon の stop を送り, 動いている途中の動き (HOME を含む) も止める
  ほかのボタン (□・SHARE・L1・L3・R3・タッチパッド・スティック) は何もしない

守り:
  - j1 は 90.87 (arm_kinematics.md の PICK_J1_FIXED) を超える指令を送らない
    (fk_lookup.J1_SAFE_MAX = 88.0 は別の用途の値なので, ここでは使わない)
  - 各関節は物理可動域 (wr.JOINT_LIMITS) まで. j4 は J4_SAFE (−90〜+60)
    (基準の姿勢から j0 ±15°・j1〜j5 ±10° の枠は 2026-11-03 にやめた. HOME から手でブロックの上まで運ぶため)
  - 範囲で切られたら理由を画面とログに出す. 別の関節で補わない
  - HOME だけは範囲の検査を通さずに送れる (run_policy_client.py の h と同じ扱い)

送り方:
  - 十字キー (1 刻み・長押し) は, 動かす関節だけを send_angle で送る (2026-11-05. ほかの関節には指令を送らない).
    HOME は send_angles で 6 関節. send_coords は使わない. speed 20 (j0_sweep_scan.SPEED)
  - 1 回押すごとに目標を 1 つ作って送り, 3° 以内に届いたかを読んで確かめる.
    送った指令が無視されることがある (どの関節も動かない. 2026-11-03 の実機で 3 回, 8 秒止まった.
    waypoint_replay.py の「単発は無視されるが, 打ち直せば通る」と同じ). 送ってから IGNORED_CHECK_SEC
    (0.5 秒) たっても, 届いておらず, どの関節も IGNORED_MOVE_DEG (0.3°) 動いていなければ, 同じ目標を
    送り直す (IGNORED_MAX_RESENDS 回まで). 送り直しも SendGate の中で行うので, 停止のあとは送らない
  - 送ってから届くまで (または待ちの上限まで) と, 爪のあとの GRIPPER_SLEEP の間は
    次の動く操作を受け付けない (ためない). 停止はいつでも受け付ける
    (送る仕事は別のスレッドで行い, 画面とコントローラーの読み取りは止めない)
  - 停止が入ったあとは新しい指令を送らない. 動いている途中の動きは daemon の stop で止める
    (2026-11-05 に daemon へ stop を足した. tools/daemon_add_stop.sh. stop のない古い daemon なら,
    今の角度へ send_angles して止める. ただしこれは実機で効かない回があった)

1 回の出発点 (2026-11-05 に関節ごとにした):
  関節ごとに, 直前の指令と送る前に読んだ角度が 3° 以内なら 指令, 離れていれば 読んだ角度 (start_pose).
  動かす関節は, 直前の指令に届いていなければ 読んだ角度.
  長押しを止めたあとは何も送らない. 止まった関節だけ止まった角度を指令にし, ほかの関節の指令は変えない
  (重さで下がった分を出発点にすると, 止めるたびにずれが積み上がった. 下がった j1 を指令へ送り直す形は,
   届かない指令のせいで次の指令が無視されやすくなったので 11/5 16:13 にやめた).
  動かさない関節は, 指令と読んだ角度が 10° 以内なら指令のまま (START_FALLBACK_DEG).
  1 刻みの「届いた」は, 動かした関節だけを見て 0.5° 以内 (STEP_REACH_TOL_DEG). 動かなければ 3 回まで送り直し, それでも動かなければ戻る.
  長押しで目標の手前で止まったら, 0.3 秒で同じ目標を送り直す (3 回まで).
  (以前の説明: 直前に送った目標に届いていて, 送る前に読んだ角度とも 3° 以内なら その目標. そうでなければ 読んだ角度.)
  (j1 の重さによる 2〜3° の不足があっても, 0.5° の刻みが読んだ値に引きずられないように.
   手で動かされたなど 3° を超えて離れていたら, 古い目標から大きく動かさない)
  出発点の 6 関節のどれかが範囲の外なら送らない (範囲の端へ切って 1 回で大きく動かさない).

停止と送信の順番 (codex R1):
  「送ると決める」と「停止・終了の取り消し」を 仕事ごとの鍵 (SendGate) で並べる (codex R1・R2).
  停止のあとに送ると決める指令はない. 停止は待たされない (画面も止まらない). 停止より前に決めた送信は
  通信の上限 SEND_RPC_TIMEOUT_SEC (8 秒) の中で終わる. 止める前に作った仕事は OPTIONS で再開しても生き返らない.
  1 コマの中では L2+R2 と読み取りの失敗を ボタンより先に見る. 送ったあと確かめる途中で関節角を
  読めなければ停止にする. log.jsonl に書けなくなったら停止し, 再開させない.

終わり方:
  Ctrl+C / ウィンドウを閉じる. 新しい指令は送らずに終わる. アームは動かさない
  (HOME 復帰もサーボ解放もしない. run_policy_client.py の finally と同じ)

Usage:
  python3 tools/gamepad_teleop.py              # ドライラン (daemon にもアームにも触らない. HOME から始まる仮の姿勢)
  python3 tools/gamepad_teleop.py --execute    # daemon に送る

記録 (--execute のときだけ): /Volumes/KIOXIA/r2r2r-data/runs/experiments/gamepad_teleop/{run_stem}/
      log.jsonl (押した操作 1 つにつき 1 行) / meta.json / gamepad_teleop.py (使ったスクリプトの写し)
      通しの完走ではないので台帳には書かない.
      ドライランは記録しない (KIOXIA に何も書かない・フォルダも作らない. KIOXIA がなくても動く. 依頼 69)

読み込み (依頼 69):
  run_policy_client・j0_sweep_scan・visual_servo_pick は cv2 を読み, cv2 は自分の libSDL2
  (site-packages/cv2/.dylibs/) を持っている. pygame の libSDL2 と 2 つ入ると objc の
  「Class SDLApplication is implemented in both ...」が十数行出る. なのでこの 3 つは --execute のときだけ
  load_daemon_modules() で読む (ドライランでは cv2 も daemon の送り口も読まない).
  ドライランでも要る値 (HOME・J4_SAFE・GRIPPER_SLEEP・物理可動域) は, 読み込まずに元のファイルの
  代入から読む (二重に書かない). --execute で読んだあと, 読み込んだ値と同じかを確かめる.
  --execute では cv2 を読むので 警告は今までどおり出る (動きには影響なし).
"""
import argparse
import ast
import hashlib
import json
import os
import platform
import queue
import shutil
import subprocess
import sys
import threading
import time
from datetime import datetime
from pathlib import Path

# ウィンドウが手前にないときもコントローラーの入力を受け取る (gamepad_check.py と同じ)
os.environ.setdefault("SDL_JOYSTICK_ALLOW_BACKGROUND_EVENTS", "1")
os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")

import numpy as np  # noqa: E402
import pygame  # noqa: E402

_HERE = Path(__file__).resolve().parent
_ROOT = _HERE.parent
sys.path.insert(0, str(_ROOT / "pi05"))
_RPC_PATH = _ROOT / "pi05" / "run_policy_client.py"
_WR_PATH = _ROOT / "dynamic_tracking_limits" / "drag_teaching" / "waypoint_replay.py"


def read_constants(path, names):
    """path を読み込まずに (実行せずに), 一番外の `名前 = 値` の値を読む. 1 回だけ代入されている名前に限る.
    ファイルのどこかで ほかの形 (+= ・型注釈つき・連鎖・for・global の中など) で同じ名前に代入していれば止める
    (--execute では読み込んだ値とも比べる)."""
    found = {}
    tree = ast.parse(Path(path).read_text(encoding="utf-8"))
    simple = set()
    for node in tree.body:
        if isinstance(node, ast.Assign) and len(node.targets) == 1 and isinstance(node.targets[0], ast.Name):
            name = node.targets[0].id
            if name in names:
                if name in found:
                    raise RuntimeError(f"{path} で {name} が 2 回代入されています")
                found[name] = ast.literal_eval(node.value)
                simple.add(id(node.targets[0]))
    for node in ast.walk(tree):
        if (isinstance(node, ast.Name) and node.id in names and isinstance(node.ctx, (ast.Store, ast.Del))
                and id(node) not in simple):
            raise RuntimeError(f"{path} で {node.id} がほかの形でも代入されています (行 {node.lineno})")
    missing = [n for n in names if n not in found]
    if missing:
        raise RuntimeError(f"{path} に {missing} が見つかりません")
    return found


# 爪 (gripper_cmd)・HOME・J4_SAFE・関節角の読み方 (read_angles) は run_policy_client と同じものを使う.
# daemon の送り口と run_policy_client は --execute のときだけ load_daemon_modules() で読む (cv2 の SDL を避ける)
_RPC_CONST = read_constants(_RPC_PATH, ("HOME", "J4_SAFE", "GRIPPER_SLEEP"))
_WR_CONST = read_constants(_WR_PATH, ("JOINT_LIMITS",))
rpc = None
wr = None
SPEED = POLL_SLEEP = REACH_TIMEOUT_SEC = None     # j0_sweep_scan の 20 / 0.3 / 8.0 (--execute で読む)
close_daemon_socket = daemon_send_angles = daemon_set_gripper = daemon_set_gripper_value = get_daemon_socket = None
daemon_request = None


def load_daemon_modules():
    """--execute のときだけ呼ぶ. run_policy_client (パスと pymycobot のスタブも入る)・j0_sweep_scan・
    visual_servo_pick の daemon の送り口を読む. 2 回目からは何もしない.
    読み込まずに読んだ値 (HOME・J4_SAFE・GRIPPER_SLEEP・JOINT_LIMITS) と違えば止める."""
    global rpc, wr, SPEED, POLL_SLEEP, REACH_TIMEOUT_SEC
    global close_daemon_socket, daemon_send_angles, daemon_set_gripper, daemon_set_gripper_value, get_daemon_socket
    global daemon_request
    if rpc is not None:
        return
    import run_policy_client as _rpc
    import j0_sweep_scan as _j0
    import visual_servo_pick as _vsp
    pairs = (("HOME", _rpc.HOME, HOME), ("J4_SAFE", _rpc.J4_SAFE, J4_SAFE),
             ("GRIPPER_SLEEP", _rpc.GRIPPER_SLEEP, GRIPPER_SLEEP), ("JOINT_LIMITS", _rpc.wr.JOINT_LIMITS, JOINT_LIMITS))
    diff = [n for n, loaded, read in pairs if _norm(loaded) != _norm(read)]
    if diff:
        raise RuntimeError(f"読み込んだ値がファイルから読んだ値と違います: {diff}")
    SPEED, POLL_SLEEP, REACH_TIMEOUT_SEC = _j0.SPEED, _j0.POLL_SLEEP, _j0.REACH_TIMEOUT_SEC
    close_daemon_socket, daemon_send_angles = _vsp.close_daemon_socket, _vsp.daemon_send_angles
    daemon_set_gripper, daemon_set_gripper_value = _vsp.daemon_set_gripper, _vsp.daemon_set_gripper_value
    get_daemon_socket = _vsp.get_daemon_socket
    daemon_request = _vsp._daemon_request
    wr = _rpc.wr
    rpc = _rpc


def _norm(v):
    if isinstance(v, (list, tuple)):
        return [_norm(x) for x in v]
    return float(v)


# ---- ボタンと軸 (/Volumes/KIOXIA/r2r2r-data/diag/gamepad_check_20261102/ で確かめた番号) ----
BTN_CROSS = 0
BTN_CIRCLE = 1
BTN_SQUARE = 2
BTN_TRIANGLE = 3
BTN_SHARE = 4
BTN_HOME = 5
BTN_OPTIONS = 6
BTN_L3 = 7
BTN_R3 = 8
BTN_L1 = 9
BTN_R1 = 10
BTN_UP = 11
BTN_DOWN = 12
BTN_LEFT = 13
BTN_RIGHT = 14
BTN_TOUCHPAD = 15
BUTTON_NAMES = {
    BTN_CROSS: "cross", BTN_CIRCLE: "circle", BTN_SQUARE: "square", BTN_TRIANGLE: "triangle",
    BTN_SHARE: "share", BTN_HOME: "home", BTN_OPTIONS: "options", BTN_L3: "l3", BTN_R3: "r3",
    BTN_L1: "l1", BTN_R1: "r1", BTN_UP: "dpad_up", BTN_DOWN: "dpad_down", BTN_LEFT: "dpad_left",
    BTN_RIGHT: "dpad_right", BTN_TOUCHPAD: "touchpad",
}
AXIS_L2 = 4
AXIS_R2 = 5
TRIGGER_STOP_ABOVE = 0.0      # L2 と R2 の両方がこれを超えたら停止 (離して -1.0, 押し込んで +1.0)

# ---- 動かし方 ----
STEP_FINE_DEG = 1.0            # 2026-11-03: 0.5° はサーボが反応しなかった (読んだ角度が変わらない). 1° にした
STEP_COARSE_DEG = 2.0          # R1 を押しながら
J0_LEFT_SIGN = -1.0            # 十字キー左で j0 を動かす向き (右はその逆). 依頼に向きの指定がない
UP_SIGN = -1.0                 # 十字キー上 = −, 下 = + (PICK の上空 → 床で j1・j2・j4 が増える向き)
SELECT_ORDER = (1, 2, 3, 4, 5)  # △ で回す順. 始めは j1
HOME_HOLD_SEC = 1.0
HOME = [float(v) for v in _RPC_CONST["HOME"]]   # [0, 0, 0, 0, 0, -30]

# ---- 守り ----
J1_MAX_DEG = 90.87             # arm_kinematics.md の PICK_J1_FIXED (エイイチと決めた. J1_SAFE_MAX 88 は使わない)
J4_SAFE = tuple(float(v) for v in _RPC_CONST["J4_SAFE"])   # (-90, 60)
JOINT_LIMITS = [tuple(float(v) for v in lim) for lim in _WR_CONST["JOINT_LIMITS"]]   # 物理可動域 (wr.JOINT_LIMITS)
GRIPPER_SLEEP = float(_RPC_CONST["GRIPPER_SLEEP"])   # 爪のあとの待ち (rpc.GRIPPER_SLEEP. ドライランと画面の表示で使う)
LIMIT_EPS_DEG = 1e-6
REACH_TOL_DEG = 3.0            # 届いたかの判定 (推論の到着確認と同じ 3°)
STEP_REACH_TOL_DEG = 0.5       # 1 刻み (1°・2°) で, 動かした関節が届いたかの判定 (2026-11-05. 3° では動かなくても「届いた」になり,
                               # 1° と 2° の違いが出なかった. 実機の記録で 1 刻みの 30 回中 23 回は 1 ° も動いていなかった)

# ---- 長押し (2026-11-03) ----
HOLD_START_SEC = 0.4           # 押してからこれだけたっても押していれば, 長押しとして送り続ける
STEP_PENDING_MAX_SEC = 1.0     # 離したときの 1 刻みを, 前の仕事が終わるまで待つ上限 (古い押しを後から動かさない)
HOLD_INTERVAL_SEC = 0.1        # 長押しで目標を送り直す間隔
HOLD_AHEAD_DEG = 15.0          # 長押しの目標 = 読んだ角度からこれだけ先 (2026-11-05 に 3 → 15. 離すと stop で止める)
HOLD_REFILL_DEG = 5.0          # 目標まで残りがこれ以下になったら次の目標を送る (送るたびに加速からやり直すので, 送る回数を減らす)
HALT_SETTLE_SEC = 0.1          # stop を送ってから角度を読むまで (11/5 に 0.3 → 0.1. 止める間に押した操作が受付なしになっていた)
STEP_POLL_SEC = 0.05           # 1 刻みで角度を読む間隔 (11/5. 0.3 秒おきでは, 届いたと分かるまで毎回 0.3 秒以上かかった)
STEP_SETTLE_SEC = 0.25         # 1 刻みで動き出したあと, これだけ角度が変わらなければ「止まった」として戻る
                               # (重さで 1〜2° 手前で止まる関節で, 届くのを 8 秒待って受付なしが続いた)
STEP_IGNORED_CHECK_SEC = 0.3   # 1 刻みで動き出さなければ送り直すまで (6 関節の送り方の 0.5 秒より短く)
STEP_MAX_RESENDS = 2           # 1 刻みの送り直しの上限 (動かないときに 3 秒近く待たないように)
HOLD_STALL_SEC = 1.5           # このあいだ動かした関節が HOLD_STALL_DEG 進んでいなければ, 長押しを止める (11/5 に 1.0 → 1.5. 送り直す間を持たせる)
HOLD_RESEND_AFTER_SEC = 0.3    # 長押しで目標の手前で止まったら (この時間たっても前の読みから動いていない), 同じ目標を送り直す
HOLD_MAX_RESENDS = 3           # 1 つの目標を送り直す回数の上限
START_FALLBACK_DEG = 10.0      # 動かさない関節は, 指令と読んだ角度がこれより離れていなければ 指令のまま (重さで 3〜6° 下がるのを拾わない)
HOLD_STALL_DEG = 0.3
DRY_HOLD_MAX_DEG = 1.5         # ドライランの長押しで 1 回読むごとに目標へ寄せる量 (0.1 秒おきに読むので 15°/秒 のつもり)

# ---- 無視された指令の送り直し (2026-11-03) ----
IGNORED_CHECK_SEC = 0.5        # 送ってからこれだけたっても動いていなければ, 無視されたとみなす
IGNORED_MOVE_DEG = 0.3         # どの関節もこれより動いていなければ「動いていない」
IGNORED_MAX_RESENDS = 3        # 送り直す回数の上限

DRY_MOVE_SEC = 0.3             # ドライランで「動いている」状態を見せる時間
SEND_RPC_TIMEOUT_SEC = 8.0     # send_angles 1 回の通信 (接続・ping・送信・返事) の上限. 通信が詰まった
                               # 送信が 停止のずっとあとに届かないように (daemon の返事は送信後の読み直し込みで最長 3.7 秒. LOG352)
WORKER_JOIN_SEC = 12.0         # 終わるとき, 送っている途中の仕事が戻るのを待つ上限 (読み取り 1 回は最長 3.7 秒)

DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data")
LOG_ROOT = DATA_ROOT / "runs" / "experiments" / "gamepad_teleop"
SSD_MOUNT = "/Volumes/KIOXIA"

STATE_IDLE = "IDLE"
STATE_MOVING = "MOVING"
STATE_STOPPED = "STOPPED"


def ensure_ssd_mounted():
    if not os.path.ismount(SSD_MOUNT):
        sys.exit("[ABORT] KIOXIA がマウントされていません。記録をローカルには逃がさず、ここで止めます。")


def _r(a):
    return None if a is None else [round(float(v), 2) for v in a]


def _fmt(a):
    return "None" if a is None else " ".join(f"{float(v):+.2f}" for v in a)


# =============================================================================
# 守り (純関数. テストで直接呼ぶ)
# =============================================================================

def joint_window():
    """各関節に送ってよい範囲 (lo, hi, 各端の出どころ) を作る.
    出どころ: PHYS (物理可動域) / J4_SAFE / J1_MAX. 狭い方が勝つ."""
    lo, hi, lo_src, hi_src = [], [], [], []
    for k in range(6):
        cands_lo = [(JOINT_LIMITS[k][0], "PHYS")]
        cands_hi = [(JOINT_LIMITS[k][1], "PHYS")]
        if k == 4:
            cands_lo.append((J4_SAFE[0], "J4_SAFE"))
            cands_hi.append((J4_SAFE[1], "J4_SAFE"))
        if k == 1:
            cands_hi.append((J1_MAX_DEG, "J1_MAX"))
        v_lo, s_lo = max(cands_lo, key=lambda c: c[0])
        v_hi, s_hi = min(cands_hi, key=lambda c: c[0])
        lo.append(v_lo); hi.append(v_hi); lo_src.append(s_lo); hi_src.append(s_hi)
    return np.array(lo), np.array(hi), lo_src, hi_src


LIMIT_JA = {"PHYS": "物理可動域", "J4_SAFE": "J4_SAFE",
            "J1_MAX": "j1 の上限 90.87 (PICK_J1_FIXED)"}


def start_pose(cmd, measured):
    """次の出発点: 関節ごとに, 指令と読んだ角度が START_FALLBACK_DEG (10°) 以内なら指令, 離れていれば読んだ角度.
    (11/5: 3° にしていたら, 腕を伸ばした姿勢で j1 が重さで 3〜6° 下がり, 読んだ角度を拾ってずれが積み上がった)
    (重さで 1〜2° 沈んだ分を読んだ角度でなぞると, 送るたびにずれが積み上がる. 2026-11-05.
     手で動かされた・止められたなど 3° より離れていれば, 古い指令から大きく動かさない)"""
    cmd = np.asarray(cmd, dtype=float)
    q = np.asarray(measured, dtype=float)
    return np.where(np.abs(cmd - q) <= START_FALLBACK_DEG, cmd, q)


def plan_step(start, joint, delta):
    """start (今回の出発点) の joint を delta 動かした目標を作り, 範囲を通す.

    戻り値 dict:
      target   送る目標 (6) or None (送らない)
      clip     切られた中身 [{joint, want, cmd, limit, bound}] (切られて端まで送るとき)
      refused  送らない理由 [{joint, value, limit, lo, hi, ...}] (空なら送ってよい)
    動かした関節は範囲の端で切る (端までは送る). 端にいて押した向きに 1 ° も進めないときは送らない.
    ほかの関節は変えない (補わない). 6 関節のどれかが範囲の外なら送らない.
    """
    lo, hi, lo_src, hi_src = joint_window()
    want = np.asarray(start, dtype=float)[:6].copy()
    out = {"target": None, "clip": [], "refused": []}
    if not np.all(np.isfinite(want)):
        out["refused"].append({"joint": None, "why": "nonfinite_start", "value": _r(want)})
        return out
    # 出発点の 6 関節がどれか範囲の外なら送らない (切ってから送ると, 範囲の外から 1 回で
    # 刻みより大きく動く. 例: j1=100 から −0.5 を押すと 90.87 へ −9.13° になる. codex 必須 2)
    for k in range(6):
        if want[k] < lo[k] - LIMIT_EPS_DEG or want[k] > hi[k] + LIMIT_EPS_DEG:
            src = hi_src[k] if want[k] > hi[k] else lo_src[k]
            out["refused"].append({"joint": k, "why": "start_out_of_window", "value": round(want[k], 2),
                                   "lo": round(lo[k], 2), "hi": round(hi[k], 2), "limit": src})
    if out["refused"]:
        return out
    v = want[joint] + delta
    if lo[joint] > hi[joint]:
        out["refused"].append({"joint": joint, "why": "empty_window", "value": round(v, 2),
                               "lo": round(lo[joint], 2), "hi": round(hi[joint], 2),
                               "limit": f"{lo_src[joint]}/{hi_src[joint]}"})
        return out
    c = min(max(v, lo[joint]), hi[joint])
    if abs(c - v) > LIMIT_EPS_DEG:
        src = hi_src[joint] if v > hi[joint] else lo_src[joint]
        bound = hi[joint] if v > hi[joint] else lo[joint]
        out["clip"].append({"joint": joint, "want": round(v, 2), "cmd": round(c, 2),
                            "limit": src, "bound": round(float(bound), 2)})
    if (c - want[joint]) * np.sign(delta) <= LIMIT_EPS_DEG:
        # 端にいる (または出発点が範囲の外で, 押した向きに進めない)
        src = hi_src[joint] if delta > 0 else lo_src[joint]
        out["refused"].append({"joint": joint, "why": "at_limit", "value": round(v, 2),
                               "lo": round(lo[joint], 2), "hi": round(hi[joint], 2), "limit": src})
        return out
    want[joint] = c
    for k in range(6):
        if want[k] < lo[k] - LIMIT_EPS_DEG or want[k] > hi[k] + LIMIT_EPS_DEG:
            src = hi_src[k] if want[k] > hi[k] else lo_src[k]
            out["refused"].append({"joint": k, "why": "out_of_window", "value": round(want[k], 2),
                                   "lo": round(lo[k], 2), "hi": round(hi[k], 2), "limit": src})
    if not out["refused"]:
        out["target"] = want
    return out


def limit_text_ascii(plan):
    """画面用 (英数字だけ)."""
    parts = []
    for c in plan.get("clip", []):
        parts.append(f"j{c['joint']} clipped {c['want']:+.2f}->{c['cmd']:+.2f} ({c['limit']} {c['bound']:+.2f})")
    for r in plan.get("refused", []):
        if r.get("joint") is None:
            parts.append(f"refused: {r['why']}")
        else:
            parts.append(f"j{r['joint']} refused {r['why']} {r['value']:+.2f} "
                         f"[{r['lo']:+.2f}, {r['hi']:+.2f}] ({r['limit']})")
    return " / ".join(parts)


def limit_text_ja(plan):
    """ターミナル用 (日本語)."""
    parts = []
    for c in plan.get("clip", []):
        parts.append(f"j{c['joint']} を {c['want']:+.2f}° → {c['cmd']:+.2f}° に切った"
                     f"（{LIMIT_JA.get(c['limit'], c['limit'])} {c['bound']:+.2f}°）")
    for r in plan.get("refused", []):
        if r.get("joint") is None:
            parts.append(f"送らない: {r['why']}")
            continue
        why = {"at_limit": "範囲の端にいて、押した向きに進めない",
               "out_of_window": "範囲の外",
               "start_out_of_window": "出発点が範囲の外（HOME の長押しで戻せます）",
               "empty_window": "範囲が空"}.get(r["why"], r["why"])
        parts.append(f"j{r['joint']} {r['value']:+.2f}° は送らない：{why}"
                     f"（範囲 [{r['lo']:+.2f}, {r['hi']:+.2f}]・{LIMIT_JA.get(r['limit'], r['limit'])}）")
    return " / ".join(parts)


# =============================================================================
# アームへの送り口
# =============================================================================

class SendGate:
    """1 つの仕事の「送ると決める」と「停止・終了の取り消し」を順番に並べる鍵 (codex R1 必須 1 / R2).

    送る側は 鍵の中で取り消しを確かめて「送ると決めた」(committed) を立て, 鍵の外で送る.
    停止・終了は 鍵の中で取り消しを立て, そのときに送ると決めていたかを受け取る.
    鍵を持つのは確かめる一瞬だけなので, 停止は待たされない (画面も止まらない).
    なので「停止のあとに送ると決めた指令」はない. 停止より前に決めた送信は, 通信が遅れても
    daemon_send_angles の timeout_override (SEND_RPC_TIMEOUT_SEC) の中で終わる.
    取り消しは仕事ごとに作り, OPTIONS で再開しても戻さない (止める前の仕事が生き返らない).
    """

    def __init__(self):
        self.lock = threading.Lock()
        self.cancel = threading.Event()
        self.committed = False

    def run(self, fn):
        """取り消されていなければ「送ると決めた」を立ててから fn() を呼ぶ. 戻り値: 呼んだか."""
        with self.lock:
            if self.cancel.is_set():
                return False
            self.committed = True
        fn()
        return True

    def cancel_now(self):
        """取り消しを立てる. 戻り値: それより前に送ると決めていたか (送信中・送信済み)."""
        with self.lock:
            self.cancel.set()
            return self.committed


class DaemonArm:
    """--execute: daemon に送る. 送り方・読み方は visual_servo_pick / run_policy_client と同じ."""

    execute = True

    def read(self):
        return rpc.read_angles()          # 5 回まで読み直す. 読めなければ None

    @staticmethod
    def _send(target, joint):
        """joint があれば その関節だけ send_angle で送る (ほかの関節には指令を送らない. 2026-11-05 の jog_probe small で
        12 回中 12 回動き, 動いた量もそろった. 6 関節まとめて送ると, 重さで下がった関節の指令とぶつかって
        ゲームパッドでは動かないことが多かった). joint が None (HOME) なら send_angles で 6 関節."""
        if joint is None:
            return daemon_send_angles([float(v) for v in target], speed=SPEED, wait=rpc.SEND_WAIT,
                                      timeout_override=SEND_RPC_TIMEOUT_SEC)
        resp = daemon_request({"cmd": "send_angle", "joint_id": int(joint) + 1, "angle": float(target[joint]),
                               "speed": SPEED, "wait": rpc.SEND_WAIT}, timeout_override=SEND_RPC_TIMEOUT_SEC)
        if not resp.get("ok"):
            raise RuntimeError(f"send_angle: {resp.get('error')}")
        return resp

    def send_and_check(self, target, gate, quitting, check=True, joint=None):
        """target を send_angles (speed 20) で送り, 3° 以内に届くかを読んで確かめる.
        gate が取り消されていれば送らない. 無視されたら送り直す (IGNORED_*). 確かめる途中で読めなければ error.
        check=False (長押し): 送るだけで, 届いたかは待たない (0.1 秒後に次の目標を送るため)."""
        target = np.asarray(target, dtype=float)
        if not check:
            t_send = time.time()
            try:
                sent = gate.run(lambda: self._send(target, joint))
            except Exception as e:
                return {"sent": "unknown", "error": f"send_failed: {type(e).__name__}: {e}", "t_send": t_send}
            if not sent:
                return {"sent": False, "why": "halted_before_send"}
            return {"sent": True, "reached": None, "no_check": True, "t_send": t_send}
        pre = self.read()
        if pre is None:
            return {"sent": False, "error": "read_failed_before_send"}
        pre_max = float(np.max(np.abs(target - pre)))
        timeout = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max))
        t_send = time.time()
        try:
            sent = gate.run(lambda: self._send(target, joint))
        except Exception as e:
            # 届いたかどうか分からない. 呼び出し側で停止にする
            return {"sent": "unknown", "error": f"send_failed: {type(e).__name__}: {e}",
                    "measured_before_send": _r(pre), "t_send": t_send}
        if not sent:
            return {"sent": False, "why": "halted_before_send", "measured_before_send": _r(pre)}
        t0 = time.monotonic()
        t_last_send = t0
        last, err = None, None
        step = joint is not None
        check_sec = STEP_IGNORED_CHECK_SEC if step else IGNORED_CHECK_SEC
        max_resends = STEP_MAX_RESENDS if step else IGNORED_MAX_RESENDS
        poll = STEP_POLL_SEC if step else POLL_SLEEP
        q_seen, t_change = None, t0          # 1 刻み: 動かした関節の角度が最後に変わった時刻
        resends = []                      # 送り直した時刻 (送ってからの秒)
        base = {"sent": True, "timeout_sec": round(timeout, 2), "t_send": t_send, "measured_before_send": _r(pre),
                "resends": resends}
        while time.monotonic() - t0 < timeout:
            if quitting.is_set():
                return dict(base, reached=None, interrupted=True, reached_angles=_r(last),
                            reach_err=None if err is None else round(err, 3))
            if gate.cancel.is_set():
                # 停止 (L2+R2) が入った: 届くのを待たずに戻り, すぐに stop を送らせる (2026-11-05 レビュー 必須 1)
                return dict(base, reached=None, halted_during_check=True, reached_angles=_r(last),
                            reach_err=None if err is None else round(err, 3),
                            elapsed_sec=round(time.monotonic() - t0, 2))
            a = self.read()
            if a is None:
                # 動いているはずの間に読めない: 届いたか分からないまま次を受け付けない (codex 必須 6)
                return dict(base, reached=None, reached_angles=_r(last),
                            reach_err=None if err is None else round(err, 3),
                            error="read_failed_during_check", elapsed_sec=round(time.monotonic() - t0, 2))
            last = a
            err = float(np.max(np.abs(a - target)))          # 丸める前の値で判定する (codex 推奨 3)
            err_j = None if joint is None else abs(float(a[joint] - target[joint]))
            # 1 刻みは 動かした関節だけで判定する (ほかの関節は重さで 3〜6° 下がっていても「届いた」にする. 11/5)
            if (err_j <= STEP_REACH_TOL_DEG) if err_j is not None else (err <= REACH_TOL_DEG):
                return dict(base, reached=True, reached_angles=_r(a), reach_err=round(err, 3),
                            elapsed_sec=round(time.monotonic() - t0, 2))
            moved = float(np.max(np.abs(a - pre))) if joint is None else abs(float(a[joint] - pre[joint]))
            if step:
                if q_seen is None or abs(float(a[joint]) - q_seen) > 0.05:
                    q_seen, t_change = float(a[joint]), time.monotonic()
                elif moved >= IGNORED_MOVE_DEG and time.monotonic() - t_change >= STEP_SETTLE_SEC:
                    # 動き出して止まった (重さで手前で止まった): 待たずに戻る. 指令はこの目標のまま (次の押しはこの先から)
                    return dict(base, reached=False, settled=True, reached_angles=_r(a), reach_err=round(err, 3),
                                elapsed_sec=round(time.monotonic() - t0, 2))
            if (len(resends) >= max_resends and moved < IGNORED_MOVE_DEG
                    and time.monotonic() - t_last_send >= check_sec):
                # 送り直しても動かない: 待ちの上限 (8 秒) まで待たずに戻る
                return dict(base, reached=False, not_moved=True, reached_angles=_r(a), reach_err=round(err, 3),
                            elapsed_sec=round(time.monotonic() - t0, 2))
            if (len(resends) < max_resends and moved < IGNORED_MOVE_DEG
                    and time.monotonic() - t_last_send >= check_sec):
                # 無視された: 同じ目標を送り直す (停止・離したあとなら送らない)
                try:
                    again = gate.run(lambda: self._send(target, joint))
                except Exception as e:
                    return dict(base, reached=None, reached_angles=_r(a), reach_err=round(err, 3),
                                error=f"resend_failed: {type(e).__name__}: {e}",
                                elapsed_sec=round(time.monotonic() - t0, 2))
                if not again:
                    return dict(base, reached=None, halted_during_check=True, reached_angles=_r(a),
                                reach_err=round(err, 3), elapsed_sec=round(time.monotonic() - t0, 2))
                t_last_send = time.monotonic()
                resends.append(round(t_last_send - t0, 2))
                continue
            time.sleep(poll)
        return dict(base, reached=False, reached_angles=_r(last),
                    reach_err=None if err is None else round(err, 3),
                    elapsed_sec=round(time.monotonic() - t0, 2))

    def halt(self):
        """動いている途中で止める (2026-11-05). daemon の stop (jog_stop と stop). stop のない古い daemon なら
        今の角度へ send_angles する. そのあと HALT_SETTLE_SEC 待って角度を読む. 停止の鍵 (SendGate) は通さない
        (止める命令は停止のあとにも送る). 送れなくても停止にはしない (止める → 失敗 → 止める の繰り返しを避ける)."""
        t_send = time.time()
        out = {"t_halt": t_send}
        try:
            resp = daemon_request({"cmd": "stop"}, timeout_override=SEND_RPC_TIMEOUT_SEC)
        except Exception as e:
            resp = None
            out["halt_error"] = f"{type(e).__name__}: {e}"
        if resp is not None and resp.get("ok"):
            out.update(halt="stop", halt_result=resp.get("result"))
        else:
            if resp is not None:
                out["halt_daemon_resp"] = resp
            q = self.read()
            if q is not None:
                try:
                    daemon_send_angles([float(v) for v in q], speed=SPEED, wait=rpc.SEND_WAIT,
                                       timeout_override=SEND_RPC_TIMEOUT_SEC)
                    out["halt"] = "send_current"
                except Exception as e:
                    out["halt_error"] = f"{type(e).__name__}: {e}"
            else:
                out["halt_error"] = out.get("halt_error") or "read_failed"
        time.sleep(HALT_SETTLE_SEC)
        out["measured_after_halt"] = _r(self.read_once())    # 読めなくても読み直さない (11/5. 止める処理が 11 秒かかった)
        return out

    def read_once(self):
        """daemon に 1 回だけ角度を聞く (Mac 側では読み直さない). 読めなければ None."""
        try:
            resp = daemon_request({"cmd": "get_angles"}, timeout_override=SEND_RPC_TIMEOUT_SEC)
        except Exception:
            return None
        a = resp.get("angles") if resp.get("ok") else None
        return np.array([float(v) for v in a]) if a is not None and len(a) == 6 else None

    def gripper(self, state, gate):
        """run_policy_client.gripper_cmd と同じ中身 (閉 = daemon_set_gripper_value(GRIP_VALUE) /
        開 = daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT). そのあと GRIPPER_SLEEP 待つ).
        指令だけを gate の中で送り, 待ちは gate の外で行う (停止の受け付けを待ちの分だけ遅らせない).
        戻り値: 送ったか."""
        if state == 1:
            sent = gate.run(lambda: daemon_set_gripper_value(rpc.GRIP_VALUE))
        else:
            sent = gate.run(lambda: daemon_set_gripper(0, wait=rpc.GRIP_OPEN_DAEMON_WAIT))
        if sent:
            time.sleep(rpc.GRIPPER_SLEEP)
        return sent


class DryArm:
    """ドライラン: daemon にもアームにも触らない. HOME から始まる仮の姿勢を動かす."""

    execute = False

    def __init__(self, start=HOME):
        self.q = np.asarray(start, dtype=float).copy()
        self.hold_target = None           # 長押しで送った目標. 読むたびに DRY_HOLD_MAX_DEG ずつ寄せる

    def read(self):
        if self.hold_target is not None:
            self.q = self.q + np.clip(self.hold_target - self.q, -DRY_HOLD_MAX_DEG, DRY_HOLD_MAX_DEG)
        return self.q.copy()

    def halt(self):
        self.hold_target = None
        return {"halt": "dry", "measured_after_halt": _r(self.q), "dry": True}

    def send_and_check(self, target, gate, quitting, check=True, joint=None):
        t_send = time.time()
        if not gate.run(lambda: None):
            return {"sent": False, "why": "halted_before_send", "measured_before_send": _r(self.q)}
        if not check:                     # 長押し: 読むたびに少しずつ目標へ寄せる (待たない)
            self.hold_target = np.asarray(target, dtype=float).copy()
            return {"sent": True, "reached": None, "no_check": True, "t_send": t_send, "dry": True}
        self.hold_target = None
        time.sleep(DRY_MOVE_SEC)
        self.q = np.asarray(target, dtype=float).copy()
        return {"sent": True, "reached": True, "reached_angles": _r(self.q), "reach_err": 0.0,
                "t_send": t_send, "dry": True}

    def gripper(self, state, gate):
        if not gate.run(lambda: None):
            return False
        time.sleep(GRIPPER_SLEEP)         # 実機と同じ間だけ受け付けない
        return True


# =============================================================================
# 記録
# =============================================================================

class JsonlLog:
    def __init__(self, path):
        self.path = Path(path)
        self.f = open(self.path, "x", encoding="utf-8")

    def write(self, rec):
        self.f.write(json.dumps(rec, ensure_ascii=False) + "\n")
        self.f.flush()                    # 途中で止まっても残るように 1 行ごとに

    def close(self):
        if not self.f.closed:
            self.f.close()


class NullLog:
    """ドライラン: 何も書かない (依頼 69)."""

    def write(self, rec):
        pass

    def close(self):
        pass


def write_json_atomic(path, obj):
    tmp = Path(str(path) + ".tmp")
    with open(tmp, "w", encoding="utf-8") as f:
        json.dump(obj, f, ensure_ascii=False, indent=2)
        f.flush()
        os.fsync(f.fileno())
    os.replace(tmp, path)


# =============================================================================
# 操作の状態 (pygame に依存しない. テストで直接呼ぶ)
# =============================================================================

class Teleop:
    """押したボタンを受けて, 送る仕事を別のスレッドで 1 つずつ行う.

    送っている間 (busy) の動く操作は受け付けない (ためない). 停止はいつでも受け付ける.
    状態を書き換えるのは呼び出し側のスレッド (pygame のループ) だけ. 送る仕事の結果は
    results に入り, poll() で受け取って状態とログに反映する.
    送る直前の確かめは 仕事ごとの SendGate で行い, 停止・終了はその鍵を取ってから取り消す.
    """

    def __init__(self, arm, start_pose, log, out=print, spawn=None):
        self.arm = arm
        self.log = log
        self.out = out
        self.spawn = spawn or self._spawn_thread
        self.cmd = np.asarray(start_pose, dtype=float).copy()        # 直前の目標 (始めは起動時の姿勢)
        self.cmd_reached = True
        self.measured = np.asarray(start_pose, dtype=float).copy()   # 最後に読めた角度
        self.sel = 0                       # SELECT_ORDER の何番目 (始めは j1)
        self.r1 = False
        self.stopped = False
        self.stop_reason = None
        self.busy = False
        self.pending = None                # 送っている操作のログ行
        self.thread = None
        self.grip = "unknown"
        self.last_limit = ""               # 直前に範囲で切られた理由 (画面用. 英数字)
        self.last_msg = ""
        self.gate = None                   # 今の仕事の SendGate (停止・終了で取り消す)
        self.log_failed = False            # log.jsonl に書けなくなったら 再開させない (終わらせる)
        self.quitting = threading.Event()
        self.results = queue.Queue()
        self.home_down_at = None
        self.home_handled = False
        self.triggers = [-1.0, -1.0]
        self.both_triggers = False
        self.read_error = False
        self.seq = 0
        self.n_sent = 0
        self.hold = None                   # 十字キーの長押し {id, button, joint, sign, down_at, n, last_req_at}
        self.hold_seq = 0
        self.hold_last_sent_id = None      # 長押しの目標を送った長押しの id (その長押しが終わったら止める)
        self.hold_sent = None              # 最後に送った長押しの目標 {q (送る前に読んだ角度), t, resent}
        self.halt_pending = None           # 止める命令を送る予定 {reason, hold_id, measured_at_request}
        self.step_pending = None           # 離したときの 1 刻みで, 前の仕事が終わるのを待っているもの

    # ---- 見せる値 ----
    @property
    def joint(self):
        return SELECT_ORDER[self.sel]

    @property
    def step(self):
        return STEP_COARSE_DEG if self.r1 else STEP_FINE_DEG

    @property
    def state(self):
        if self.stopped:
            return STATE_STOPPED
        return STATE_MOVING if self.busy else STATE_IDLE

    # ---- 記録 ----
    def _entry(self, button, kind, **kw):
        self.seq += 1
        e = {"seq": self.seq, "t": datetime.now().isoformat(timespec="milliseconds"),
             "t_unix": round(time.time(), 3), "button": button,
             "button_name": BUTTON_NAMES.get(button) if isinstance(button, int) else button,
             "kind": kind, "selected_joint": self.joint, "step_deg": self.step,
             "execute": bool(self.arm.execute)}
        e.update(kw)
        return e

    def _finish(self, e):
        e.update({"grip_state": self.grip, "stopped": self.stopped, "stop_reason": self.stop_reason})
        try:
            self.log.write(e)
        except Exception as ex:          # SSD が外れたなど. ローカルには逃がさない
            first = not self.log_failed
            self.log_failed = True
            if first:
                self.out(f"[記録エラー] log.jsonl に書けません: {ex}。停止にします"
                         "（再開はできません。終わらせて KIOXIA を確かめてください）")
            if not self.stopped:
                self.stop_reason = "log_write_failed"
            self.stopped = True
            self._cancel_gate()
            self._drop_home_hold()

    # ---- 停止 ----
    def _cancel_gate(self):
        """今の仕事をまだ送ると決めていなければ, もう送らせない (待たない).
        戻り値: 停止より前に送ると決めていたか (None = 仕事なし)."""
        if self.gate is None:
            return None
        return self.gate.cancel_now()

    def _drop_home_hold(self):
        """HOME の長押しを無効にする (押し直しを求める. codex 必須 4)."""
        if self.home_down_at is not None and not self.home_handled:
            self.home_handled = True
            self.out("[HOME] 長押しの途中で受け付けられなくなったので取り消しました（押し直してください）")
            self._finish(self._entry(BTN_HOME, "home", accepted=False, reason="hold_cancelled"))

    def _end_hold(self, msg=None, reason="hold_end"):
        h = self.hold
        if h is not None:
            if h.get("q0") is not None:
                self.out(f"[長押し] j{h['joint']} を {h['q0']:+.2f}° → {h['q_last']:+.2f}° まで"
                         f"（目標を {h['n_sent']} 回送信。止めます）")
            if msg:
                self.out(msg)
            if self.hold_last_sent_id == h["id"]:
                # 長押しの目標 (最大 15° 先) へ向かって動いているので止める (離した・停止・端・進まない・ほかのボタン)
                self.request_halt(reason, hold_id=h["id"], joint=h["joint"])
            self.hold = None

    def request_halt(self, reason, hold_id=None, joint=None):
        """止める命令を送る予定にする. 送っている仕事があれば, それが終わってすぐに送る (daemon の通信は 1 本なので).
        joint: 長押しで動かしていた関節 (止めたあと, ほかの関節を指令の角度へ戻すため)."""
        if self.halt_pending is None:
            self.halt_pending = {"reason": reason, "hold_id": hold_id, "joint": joint,
                                 "measured_at_request": _r(self.measured)}
        self.hold_last_sent_id = None
        self.hold_sent = None
        self._dispatch_halt()

    def _dispatch_halt(self):
        if self.halt_pending is None or self.busy:
            return
        hp = self.halt_pending
        self.halt_pending = None
        e = self._entry(None, "halt", accepted=True, halt_reason=hp["reason"], hold_id=hp["hold_id"],
                        halt_joint=hp.get("joint"), measured_at_request=hp["measured_at_request"])
        self.busy = True
        self.pending = e
        self.gate = None                   # 止める命令は停止の鍵で取り消さない
        snap = {"cmd": self.cmd.copy(), "cmd_reached": self.cmd_reached, "hold_last_sent_id": None,
                "halt_joint": hp.get("joint")}
        self.spawn(lambda: self._run_job(("halt",), snap, SendGate()))

    def _hold_progress(self, e, job):
        """長押しの 1 回の送信のあと: 進み具合を覚え, HOLD_STALL_SEC 進んでいなければ止める."""
        h = self.hold
        if h is None or h["id"] != e.get("hold_id") or not e.get("measured_before"):
            return
        qj = float(e["measured_before"][job[1]])
        t = h["last_req_at"]
        if h.get("q0") is None:
            h.update(q0=qj, n_sent=0, prog_q=qj, prog_t=t)
        h["q_last"] = qj
        if e.get("sent") is True:
            h["n_sent"] += 1
        if abs(qj - h["prog_q"]) >= HOLD_STALL_DEG:
            h["prog_q"], h["prog_t"] = qj, t
        elif t - h["prog_t"] >= HOLD_STALL_SEC:
            self._end_hold(f"[長押し] j{job[1]} が {HOLD_STALL_SEC:.0f} 秒進まないので止めました"
                           "（どこかに当たっていれば、逆向きを1回押すか HOME で戻してください）", reason="hold_stall")

    def enter_stop(self, reason, button=None, kind="stop"):
        was = self.stopped
        self.stopped = True
        self._end_hold(reason="stop")
        committed = self._cancel_gate()
        if not was:
            self.stop_reason = reason
        self.out(f"[停止] {reason}。新しい指令は送りません（OPTIONS で再開）"
                 + ("。止める前に送った動きは stop で止めます" if committed else "")
                 + ("。まだ送っていなかった操作は送りません" if committed is False else ""))
        self._finish(self._entry(button, kind, accepted=True, reason=reason, already_stopped=was,
                                 busy=self.busy, send_committed_before_stop=committed,
                                 triggers=[round(v, 3) for v in self.triggers]))
        self._drop_home_hold()
        if not was:
            # 動いている途中の動き (長押し・HOME・1 刻み) を止める. 止まっている腕に送っても何も起きない
            self.request_halt(reason)

    def set_triggers(self, l2, r2):
        self.triggers = [float(l2), float(r2)]
        both = l2 > TRIGGER_STOP_ABOVE and r2 > TRIGGER_STOP_ABOVE
        if both and not self.both_triggers:
            self.both_triggers = True
            self.enter_stop("L2+R2", button="L2+R2")
        self.both_triggers = both

    def controller_lost(self, reason):
        self.enter_stop(reason, button=None, kind="controller_lost")
        self.r1 = False                      # 押しっぱなしの状態を持ち越さない
        self.home_down_at = None
        self.home_handled = False
        self.both_triggers = False

    def controller_read_failed(self, msg):
        if not self.read_error:
            self.read_error = True
            self.enter_stop(f"read_error: {msg}", button=None, kind="read_error")

    def controller_read_ok(self):
        self.read_error = False

    def _resume(self, e):
        if not self.stopped:
            e.update(accepted=False, reason="not_stopped")
            self.out("[再開] 止まっていないので何もしません")
        elif self.both_triggers:
            e.update(accepted=False, reason="triggers_still_pressed")
            self.out("[再開] L2 と R2 を離してから OPTIONS を押してください")
        elif self.read_error:
            e.update(accepted=False, reason="controller_read_error")
            self.out("[再開] コントローラーを読めていないので再開しません")
        elif self.log_failed:
            e.update(accepted=False, reason="log_write_failed")
            self.out("[再開] 記録が書けないので再開しません（終わらせて KIOXIA を確かめてください）")
        else:
            # 止める前に作った仕事の取り消しは戻さない (仕事ごとの SendGate. 再開で生き返らない)
            self.stopped = False
            self.stop_reason = None
            e.update(accepted=True)
            self.out("[再開] 操作を受け付けます")
        self._finish(e)

    # ---- ボタン ----
    def press(self, button, now):
        if button == BTN_R1:
            self.r1 = True
            self._finish(self._entry(button, "modifier", accepted=True))
            return
        self._end_hold()                     # 十字キーの長押しは, ほかのボタン (R1 以外) で止まる
        if button == BTN_TRIANGLE:
            self.sel = (self.sel + 1) % len(SELECT_ORDER)
            self.out(f"[関節] 上下で動かす関節を j{self.joint} にしました")
            self._finish(self._entry(button, "select_joint", accepted=True))
            return
        if button == BTN_OPTIONS:
            self._resume(self._entry(button, "resume"))
            return
        if button == BTN_HOME:
            self.home_down_at = now
            self.home_handled = False
            if self.stopped or self.busy:
                # 押し始めに受け付けられないなら, 長押しを続けても送らない (押し直しを求める)
                self.home_handled = True
                why = "stopped" if self.stopped else "busy"
                self.out("[受付なし] HOME: " + ("停止中です" if self.stopped else "前の操作が終わっていません")
                         + "（押し直してください）")
                self._finish(self._entry(button, "home", accepted=False, reason=why))
            return                           # 1 秒続いたとき (tick) か 離したとき (release) に 1 行書く
        if button in (BTN_LEFT, BTN_RIGHT, BTN_UP, BTN_DOWN):
            if button in (BTN_LEFT, BTN_RIGHT):
                sign = J0_LEFT_SIGN if button == BTN_LEFT else -J0_LEFT_SIGN
                j = 0
            else:
                sign = UP_SIGN if button == BTN_UP else -UP_SIGN
                j = self.joint
            if self.stopped:
                self._request(self._entry(button, "move", joint=j, delta=sign * self.step), ("move", j, 0.0, None))
                return                       # 停止中: 受付なしを 1 行書くだけ (_request が断る)
            # 2026-11-05: 押した瞬間には送らない. 離したのが HOLD_START_SEC より前なら 1 刻み (release),
            # 押し続けたら長押し (tick) の, どちらか片方だけを送る. 押した瞬間に 1 刻みを送ると,
            # 向きを変えるたびに「1° 動く → 止まる → 動き出す」になり, 前の長押しを止める処理とも重なって受付なしになった
            self.hold_seq += 1
            self.hold = {"id": self.hold_seq, "button": button, "joint": j, "sign": sign,
                         "down_at": now, "n": 0, "last_req_at": now - HOLD_INTERVAL_SEC}
            return
        if button in (BTN_CROSS, BTN_CIRCLE):
            state = 1 if button == BTN_CROSS else 0
            self._request(self._entry(button, "grip_close" if state else "grip_open", grip_cmd=state),
                          ("grip", state))
            return
        self._finish(self._entry(button, "ignored", accepted=False, reason="unassigned"))

    def release(self, button, now):
        if self.hold is not None and button == self.hold["button"] and self.hold["n"] == 0:
            # 長押しになる前に離した: 1 刻みを送る (動いている間なら, 終わってから送る. 1 秒たったら捨てる)
            h = self.hold
            self.hold = None
            self.step_pending = {"button": button, "joint": h["joint"], "delta": h["sign"] * self.step,
                                 "hold_id": h["id"], "at": now}
            self._dispatch_step(now)
        elif self.hold is not None and button == self.hold["button"]:
            hid = self.hold["id"]
            self._end_hold(reason="release")   # 離したら長押しを止める (目標を送っていれば stop も送る)
            p = self.pending
            if (self.busy and self.gate is not None and p is not None
                    and p.get("hold_id") == hid and (p.get("hold_n") or 0) >= 1):
                # 長押しの 2 回目からの刻みを, まだ送ると決めていなければ送らない (1 回目 = 短い押しは取り消さない)
                p["released_during_job"] = True
                self.gate.cancel_now()
        if button == BTN_R1:
            self.r1 = False
        elif button == BTN_HOME:
            if self.home_down_at is not None and not self.home_handled:
                held = now - self.home_down_at
                self.out(f"[HOME] {held:.2f} 秒で離したので送りません（{HOME_HOLD_SEC:.0f} 秒の長押しで送ります）")
                self._finish(self._entry(button, "home_short", accepted=False, reason="held_too_short",
                                         held_sec=round(held, 3)))
            self.home_down_at = None
            self.home_handled = False

    def _dispatch_step(self, now=None):
        sp = self.step_pending
        if sp is None or self.busy or self.halt_pending is not None:
            return
        self.step_pending = None
        if now is not None and now - sp["at"] > STEP_PENDING_MAX_SEC:
            self._finish(self._entry(sp["button"], "move", joint=sp["joint"], delta=sp["delta"],
                                     accepted=False, reason="step_expired"))
            return
        e = self._entry(sp["button"], "move", joint=sp["joint"], delta=sp["delta"], hold_id=sp["hold_id"], hold_n=0)
        self._request(e, ("move", sp["joint"], sp["delta"], None))

    def tick(self, now):
        self._dispatch_halt()
        self._dispatch_step(now)
        if self.home_down_at is not None and not self.home_handled and (self.busy or self.stopped):
            self._drop_home_hold()           # 長押しの途中で別の操作が動いた・止まった
        if (self.home_down_at is not None and not self.home_handled
                and now - self.home_down_at >= HOME_HOLD_SEC):
            self.home_handled = True
            self._request(self._entry(BTN_HOME, "home", held_sec=round(now - self.home_down_at, 3)),
                          ("home",))
        h = self.hold
        if (h is not None and not self.stopped and not self.busy
                and now - h["down_at"] >= HOLD_START_SEC - 1e-6
                and now - h["last_req_at"] >= HOLD_INTERVAL_SEC - 1e-6):
            h["n"] += 1
            h["last_req_at"] = now
            d = h["sign"] * HOLD_AHEAD_DEG
            self._request(self._entry(h["button"], "move", joint=h["joint"], delta=d,
                                      hold_id=h["id"], hold_n=h["n"],
                                      held_sec=round(now - h["down_at"], 3)),
                          ("move", h["joint"], d, h["id"]))

    def _hold_alive(self, hold_id):
        h = self.hold
        return h is not None and h["id"] == hold_id

    # ---- 送る仕事 ----
    def _request(self, e, job):
        if self.stopped:
            e.update(accepted=False, reason="stopped")
            self.out("[受付なし] 停止中です（OPTIONS で再開）")
            self._finish(e)
            return
        if self.busy:
            e.update(accepted=False, reason="busy")
            self.out("[受付なし] 前の操作が終わっていません（ためません）")
            self._finish(e)
            return
        e["accepted"] = True
        self.busy = True
        self.pending = e
        gate = SendGate()
        self.gate = gate
        snap = {"cmd": self.cmd.copy(), "cmd_reached": self.cmd_reached,
                "hold_last_sent_id": self.hold_last_sent_id,
                "hold_sent": None if self.hold_sent is None else dict(self.hold_sent)}
        self.spawn(lambda: self._run_job(job, snap, gate))

    def _spawn_thread(self, fn):
        self.thread = threading.Thread(target=fn, daemon=True)
        self.thread.start()

    def _run_job(self, job, snap, gate):
        res = {}
        try:
            if job[0] == "move":
                _, j, d, hold_id = job
                q = self.arm.read()
                res["measured_before"] = _r(q)
                if q is None:
                    res["error"] = "read_failed"
                    return
                if hold_id is not None:
                    # 長押しの 2 回目から: 動かす関節は「いま読んだ角度 + 3° 先」. ほかの関節は直前の指令のまま
                    # (読んだ角度で送り直すと, 重さで沈んだ分を毎回なぞって j1 などがずり下がる)
                    cmd = np.asarray(snap["cmd"], dtype=float)
                    others = [k for k in range(6) if k != j]
                    gap_o = float(np.max(np.abs(cmd[others] - q[others])))
                    start = start_pose(cmd, q)          # ほかの関節は 関節ごとに 指令から (3° 以内なら)
                    start[j] = q[j]
                    res["start_source"] = "hold" if gap_o <= REACH_TOL_DEG else "mixed"
                    res["cmd_gap_deg"] = round(gap_o, 3)
                    res["hold_lead_deg"] = round(abs(float(cmd[j] - q[j])), 3)
                    lead = float(cmd[j] - q[j]) * float(np.sign(d))
                    hs = snap.get("hold_sent")
                    if snap.get("hold_last_sent_id") == hold_id and lead > HOLD_REFILL_DEG:
                        if (hs is not None and hs["resends"] < HOLD_MAX_RESENDS
                                and time.monotonic() - hs["t"] >= HOLD_RESEND_AFTER_SEC
                                and abs(float(q[j]) - float(hs["last_q"][j])) < IGNORED_MOVE_DEG):
                            # 目標の手前で止まった (次の目標が無視され, 前の目標で止まった. 11/5 の実機で 3 回):
                            # 待たずに同じ目標を送り直す
                            res["command"] = _r(cmd)
                            res["_target"] = cmd.copy()
                            res["hold_resend"] = True
                            res.update(self.arm.send_and_check(cmd, gate, self.quitting, check=False, joint=j))
                            return
                        # この長押しの目標へ向かっている途中: 送らない (送るたびに加速からやり直してカクつく)
                        res.update(sent=False, why="lead_ok", hold_remaining_deg=round(lead, 3))
                        return
                    plan = plan_step(start, j, d)
                    if (plan["target"] is not None and plan["clip"]
                            and abs(float(plan["target"][j] - cmd[j])) < 0.01):
                        if snap.get("hold_last_sent_id") == hold_id and lead > REACH_TOL_DEG:
                            # 範囲の端の目標へ向かっている途中: 端まで行かせる (ここで止めると端の手前 5° で止まる)
                            res.update(sent=False, why="lead_ok", hold_remaining_deg=round(lead, 3), at_limit_target=True)
                            return
                        # 範囲の端の目標をもう送ってあり, ほぼ着いた: 同じ目標を送り続けない (長押しを止める)
                        c = plan["clip"][0]
                        plan = {"target": None, "clip": [], "refused": [{
                            "joint": j, "why": "at_limit", "value": c["want"], "lo": c["bound"], "hi": c["bound"],
                            "limit": c["limit"]}]}
                    if (plan["target"] is not None and abs(float(cmd[j] - q[j])) <= HOLD_AHEAD_DEG + REACH_TOL_DEG
                            and (plan["target"][j] - cmd[j]) * np.sign(d) < 0):
                        # 読んだ角度が古いと「読んだ角度 + 3°」が前の目標より後ろになる. 後ろへは戻さない (引き戻さない)
                        plan["target"][j] = cmd[j]
                        res["held_previous_target"] = True
                    res["clip"] = plan["clip"]
                    res["refused"] = plan["refused"]
                    res["limit_ja"] = limit_text_ja(plan)
                    res["limit_ascii"] = limit_text_ascii(plan)
                    if plan["target"] is None:
                        res["sent"] = False
                        return
                    res["command"] = _r(plan["target"])
                    res["_target"] = plan["target"]
                    res.update(self.arm.send_and_check(plan["target"], gate, self.quitting, check=False, joint=j))
                    return
                # 出発点: 直前の目標に届いていて, 今読んだ角度とも 3° 以内ならその目標.
                # そうでなければ今読んだ角度 (手で動かされたなど. 古い目標から大きく動かさない. codex 必須 3)
                gap = float(np.max(np.abs(np.asarray(snap["cmd"]) - q)))
                cmd0 = np.asarray(snap["cmd"], dtype=float)
                start = start_pose(cmd0, q)
                if not snap["cmd_reached"] or abs(float(cmd0[j] - q[j])) > REACH_TOL_DEG:
                    start[j] = q[j]                     # 動かす関節が届いていない・3° より離れていれば, 読んだ角度から
                res["start_source"] = "command" if start[j] == cmd0[j] else "measured"   # 動かす関節の出発点
                res["others_from_measured"] = [k for k in range(6) if k != j and start[k] != cmd0[k]]
                res["cmd_gap_deg"] = round(gap, 3)
                plan = plan_step(start, j, d)
                res["clip"] = plan["clip"]
                res["refused"] = plan["refused"]
                res["limit_ja"] = limit_text_ja(plan)
                res["limit_ascii"] = limit_text_ascii(plan)
                if plan["target"] is None:
                    res["sent"] = False
                    return
                res["command"] = _r(plan["target"])
                res["_target"] = plan["target"]
                res.update(self.arm.send_and_check(plan["target"], gate, self.quitting, joint=j))
            elif job[0] == "home":
                q = self.arm.read()
                res["measured_before"] = _r(q)
                if q is None:                # 読めなければ送らない (codex 必須 6)
                    res["error"] = "read_failed"
                    return
                res["command"] = _r(HOME)
                res["_target"] = np.array(HOME)
                res["range_check"] = "skipped_for_home"
                res.update(self.arm.send_and_check(np.array(HOME), gate, self.quitting))
            elif job[0] == "halt":
                res.update(self.arm.halt())
                q = res.get("measured_after_halt")
                hj = snap.get("halt_joint")
                if False and q is not None and hj is not None and not self.stopped:
                    # 11/5 16:13 にやめた: 重さで届かない j1 の指令を送ると, そのあとの指令が無視されやすくなった
                    # stop のあと, 動かしていない関節が 1〜2° 下がる (11/5 の記録で j1 +1°・j4 +1.5° 前後, 毎回).
                    # 止まった関節は止まった角度, ほかの関節は指令の角度で送り直して元に戻す.
                    # 戻さずに読んだ角度を次の出発点にすると, 止めるたびにずれが積み上がった (j1 が 5° → 40°)
                    pin = np.asarray(snap["cmd"], dtype=float).copy()
                    pin[hj] = float(q[hj])
                    lo, hi, _ls, _hs = joint_window()
                    if np.all(pin >= lo - LIMIT_EPS_DEG) and np.all(pin <= hi + LIMIT_EPS_DEG):
                        res["pin_command"] = _r(pin)
                        res["_pin"] = pin
                        r2 = self.arm.send_and_check(pin, gate, self.quitting, check=False)
                        res["pin_sent"] = r2.get("sent")
                    else:
                        res["pin_command"] = None
                        res["pin_refused"] = "out_of_window"
            elif job[0] == "grip":
                res["measured_before"] = None
                sent = self.arm.gripper(job[1], gate)
                res["sent"] = bool(sent)
                if not sent:
                    res["why"] = "halted_before_send"
        except Exception as ex:
            res["error"] = f"{type(ex).__name__}: {ex}"
        finally:
            self.results.put((job, res))

    def poll(self):
        """送る仕事の結果を受け取り, 状態とログに反映する (pygame のループから毎回呼ぶ)."""
        while True:
            try:
                job, res = self.results.get_nowait()
            except queue.Empty:
                return
            self._apply(job, res)

    def _apply(self, job, res):
        e = self.pending or self._entry(None, "orphan_result")
        self.pending = None
        self.busy = False
        target = res.pop("_target", None)
        e.update(res)
        if res.get("reached_angles") is not None:
            self.measured = np.asarray(res["reached_angles"], dtype=float)
        elif res.get("measured_before") is not None:
            self.measured = np.asarray(res["measured_before"], dtype=float)
        sent = res.get("sent")
        if job[0] == "halt":
            self._apply_halt(e, res)
            return
        if job[0] == "move" and sent is True and res.get("no_check") and e.get("hold_id") is not None:
            if self._hold_alive(e["hold_id"]):
                self.hold_last_sent_id = e["hold_id"]
                mb = e.get("measured_before")
                if res.get("hold_resend"):
                    if self.hold_sent is not None:
                        self.hold_sent["resends"] += 1
                        self.hold_sent["t"] = time.monotonic()
                        if mb is not None:
                            self.hold_sent["last_q"] = list(mb)
                elif mb is not None:
                    self.hold_sent = {"q": list(mb), "last_q": list(mb), "t": time.monotonic(), "resends": 0}
            else:
                # 送っている間に離された・止められた: 送った目標へ動き出しているので止める
                self.request_halt("released_while_sending", hold_id=e["hold_id"], joint=job[1])
        if (job[0] == "move" and res.get("why") == "lead_ok" and self.hold_sent is not None
                and e.get("measured_before") is not None):
            self.hold_sent["last_q"] = list(e["measured_before"])     # 次の確かめで「止まったか」を見るため
        if job[0] in ("move", "home") and sent is True:
            self.n_sent += 1
            self.cmd = np.asarray(target, dtype=float)
            # 長押しの送信は届くのを待たないが, アームは目標へ向かって動き続けるので「届く目標」として扱う
            # (次の短い押しは, 読んだ角度と 3° 以内ならこの目標から. 離れていれば読んだ角度から)
            # 1 刻みが送り直しても動かなかったとき (サーボが小さな動きに反応しない) も, 指令はその目標のままにする.
            # 次の押しはそこから 1 刻み先になるので, 押した分が積み上がって動き出す (3° 離れるまで. start_pose)
            self.cmd_reached = True if (res.get("no_check") or res.get("not_moved") or res.get("settled")) \
                else bool(res.get("reached"))
        elif sent == "unknown":
            self.cmd_reached = False                      # 次は読んだ角度から
        if job[0] == "grip" and res.get("error"):
            self.grip = "unknown"
        if job[0] == "grip" and sent is True:
            self.grip = "closed" if job[1] == 1 else "open"
        if job[0] == "move":
            self.last_limit = res.get("limit_ascii", "")
        e["reached"] = res.get("reached") if sent is True else None
        # ---- ターミナル ----
        if job[0] == "move":
            label = f"j{job[1]} {job[2]:+.1f}°" + (f"（長押し {e.get('hold_n')}）" if job[3] is not None else "")
        elif job[0] == "home":
            label = "HOME"
        else:
            label = "爪を" + ("閉じる" if job[1] == 1 else "開く")
        if res.get("limit_ja"):
            self.out(f"[範囲] {res['limit_ja']}")
        if res.get("error"):
            self.out(f"[エラー] {label}: {res['error']}。停止にします")
        elif sent is True and job[0] == "grip":
            self.out(f"[爪] {label}（待ち {GRIPPER_SLEEP:.1f} 秒のあと受付）")
        elif sent is True and res.get("no_check"):
            self._hold_progress(e, job)                    # 長押し: 1 回ずつは出さない (離したときに 1 行)
        elif res.get("why") == "lead_ok":
            self._hold_progress(e, job)                    # 目標へ向かっている途中 (送っていない). 進み具合だけ見る
        elif sent is True:
            r = res.get("reached")
            how = "届いた" if r else ("確かめる途中で終了" if r is None else
                                     ("動かなかった（次の押しは この目標の先から）" if res.get("not_moved") else
                                      ("手前で止まった（次の押しは この目標の先から）" if res.get("settled") else "届かなかった")))
            if res.get("resends"):
                how += f"・無視されたので {len(res['resends'])} 回送り直した"
            self.out(f"[送信] {label} → {_fmt(res.get('command'))}  {how}"
                     f"（ずれ {res.get('reach_err')}°）  届いた角度 {_fmt(res.get('reached_angles'))}")
        elif res.get("why") == "halted_before_send" and e.get("released_during_job") and not self.stopped:
            pass                                           # 長押しを離したので取り消した. 何も出さない
        elif res.get("why") == "halted_before_send":
            self.out(f"[送らない] {label}: 停止が入ったので送りませんでした")
        else:
            self.out(f"[送らない] {label}")
        # 長押しの中で送れなかったら (範囲の端・停止), その長押しは止める. 端で同じ行を繰り返さない
        if (job[0] == "move" and sent is not True and res.get("why") != "lead_ok"
                and e.get("hold_id") is not None and self._hold_alive(e["hold_id"])):
            self._end_hold(reason="hold_not_sent")
        self.gate = None
        self._finish(e)
        if res.get("error"):
            self.enter_stop(f"arm_error: {res['error']}", kind="arm_error")
        self._dispatch_halt()
        self._dispatch_step()

    def _apply_halt(self, e, res):
        pin = res.pop("_pin", None)
        e.pop("_pin", None)                                # _apply で先に e へ入っている (JSON にしない)
        e.update(res)
        q = res.get("measured_after_halt")
        if q is not None:
            self.measured = np.asarray(q, dtype=float)
        if pin is not None and res.get("pin_sent") is True:
            self.cmd = np.asarray(pin, dtype=float)       # 止まった関節は止まった角度, ほかは指令のまま
            self.cmd_reached = True
        elif q is not None and e.get("halt_joint") is not None:
            # 長押しを止めた: 止まった関節だけ止まった角度にする. ほかの関節の指令は変えない (重さで下がった分を拾わない)
            self.cmd[e["halt_joint"]] = float(q[e["halt_joint"]])
            self.cmd_reached = True
        elif q is not None:
            # 停止 (L2+R2) で HOME などの途中で止めた: 止まった所を指令にする (古い目標へ急に動かさない)
            self.cmd = np.asarray(q, dtype=float).copy()
            self.cmd_reached = True
        else:
            self.cmd_reached = False
        before = e.get("measured_at_request")
        if q is not None and before is not None:
            d = np.asarray(q, dtype=float) - np.asarray(before, dtype=float)
            e["moved_after_request_deg"] = _r(d)
        how = {"stop": "stop", "send_current": "今の角度へ送り直し（daemon に stop がない）",
               "dry": "ドライラン"}.get(res.get("halt"), "送れなかった")
        if res.get("halt_error"):
            self.out(f"[止める] 止める命令を送れませんでした: {res['halt_error']}（L2+R2 で停止・必要なら電源を切ってください）")
        elif e.get("halt_reason") not in ("release", "hold_end"):
            self.out(f"[止める] {how}（{e.get('halt_reason')}）  止まった角度 {_fmt(q)}")
        self.gate = None
        self._finish(e)
        self._dispatch_halt()
        self._dispatch_step()

    # ---- 終わり ----
    def shutdown(self):
        """新しい指令を送らずに終わる. 送っている途中の仕事が戻るのを WORKER_JOIN_SEC まで待つ.
        戻り値: 仕事が終わっているか (終わっていなければ daemon のソケットを閉じない)."""
        self.quitting.set()
        committed = self._cancel_gate()
        if self.pending is not None:
            self.pending["send_committed_before_exit"] = committed
        if self.thread is not None:
            self.thread.join(WORKER_JOIN_SEC)
        self.poll()
        done = self.thread is None or not self.thread.is_alive()
        if done and (self.halt_pending is not None or self.hold_last_sent_id is not None
                     or (self.hold is not None and self.hold_last_sent_id == self.hold["id"])):
            # 長押しの目標へ動いている途中で終わった: stop を 1 回だけ送る (新しく動かす指令ではない. レビュー 推奨 3)
            try:
                r = self.arm.halt()
            except Exception as ex:
                r = {"halt_error": f"{type(ex).__name__}: {ex}"}
            self.halt_pending = None
            self.hold_last_sent_id = None
            e = self._entry(None, "halt", accepted=True, halt_reason="exit")
            e.update(r)
            self._finish(e)
        if self.pending is not None:
            e = self.pending
            self.pending = None
            e.update(reached=None, interrupted_at_exit=True)
            self._finish(e)
        return done

    def status_lines(self, q_now=None):
        """画面の文字 (英数字だけ)."""
        q = self.measured if q_now is None else q_now
        lines = [
            ("big", f"JOINT  j{self.joint}    STEP {self.step:.1f} deg"),
            ("big", f"STATE  {self.state}" + ("  (moving)" if self.stopped and self.busy else "")),
            ("big", f"GRIP   {self.grip}"),
        ]
        for k in range(6):
            lines.append(("mono", f"j{k}  {q[k]:+8.2f}   cmd {self.cmd[k]:+8.2f}"))
        lines.append(("small", "last limit: " + (self.last_limit or "-")))
        if self.stopped:
            lines.append(("small", f"stop reason: {self.stop_reason}  (OPTIONS = resume)"))
        return lines


# =============================================================================
# pygame のループ
# =============================================================================

WINDOW_SIZE = (900, 560)


def controller_info(js):
    return {"name": js.get_name(), "guid": js.get_guid(), "instance_id": js.get_instance_id(),
            "axes": js.get_numaxes(), "buttons": js.get_numbuttons(), "hats": js.get_numhats(),
            "connected_at": datetime.now().isoformat(timespec="seconds")}


def run_loop(tp, screen, fonts, mode_text, on_controller=None, on_frame=None):
    """ウィンドウを閉じるまで回す. 戻り値: 終わった理由."""
    js = None
    clock = pygame.time.Clock()
    frame = 0
    while True:
        if on_frame is not None:
            on_frame(frame, tp)
        frame += 1
        now = time.monotonic()
        events = pygame.event.get()
        buttons = []
        for ev in events:
            if ev.type == pygame.QUIT:
                return "window_closed"
            if ev.type == pygame.JOYDEVICEADDED:
                if js is None:
                    js = pygame.joystick.Joystick(ev.device_index)
                    info = controller_info(js)
                    tp.out(f"[接続] {info['name']}  GUID={info['guid']}  軸={info['axes']}  ボタン={info['buttons']}")
                    if on_controller is not None:
                        on_controller(info)
            elif ev.type == pygame.JOYDEVICEREMOVED:
                if js is not None and ev.instance_id == js.get_instance_id():
                    js = None
                    tp.controller_lost("controller_disconnected")
            elif ev.type in (pygame.JOYBUTTONDOWN, pygame.JOYBUTTONUP):
                if js is not None and ev.instance_id == js.get_instance_id():
                    buttons.append(ev)
        # 停止 (L2+R2) と 読み取りの失敗を ボタンより先に見る (同じコマのボタンで送らない. codex 必須 5)
        read_ok = js is not None
        if js is not None:
            try:
                l2, r2 = js.get_axis(AXIS_L2), js.get_axis(AXIS_R2)
                tp.controller_read_ok()
                tp.set_triggers(l2, r2)
            except Exception as ex:          # pygame.error など
                read_ok = False
                tp.controller_read_failed(f"{type(ex).__name__}: {ex}")
        if read_ok:
            for ev in buttons:
                if ev.type == pygame.JOYBUTTONDOWN:
                    tp.press(ev.button, now)
                else:
                    tp.release(ev.button, now)
        # 長押しの最中は, 離した合図を取りこぼしても止まるよう, ボタンの状態も毎コマ見る
        if js is not None and read_ok and tp.hold is not None:
            hb = tp.hold["button"]
            try:
                held = js.get_button(hb)
            except Exception as ex:          # pygame.error など
                tp.controller_read_failed(f"{type(ex).__name__}: {ex}")
            else:
                if not held:
                    tp.release(hb, now)
        tp.tick(now)
        tp.poll()
        draw(screen, fonts, tp, mode_text, js)
        clock.tick(60)


def draw(screen, fonts, tp, mode_text, js):
    screen.fill((20, 20, 24))
    color_state = {STATE_IDLE: (120, 220, 140), STATE_MOVING: (240, 200, 80),
                   STATE_STOPPED: (240, 90, 90)}[tp.state]
    y = 10
    screen.blit(fonts["small"].render(f"{mode_text}   controller: "
                                      + (js.get_name()[:40] if js is not None else "none"),
                                      True, (200, 220, 255)), (10, y))
    y += 28
    for kind, text in tp.status_lines():
        col = color_state if text.startswith("STATE") else (235, 235, 235)
        screen.blit(fonts[kind].render(text[:110], True, col), (10, y))
        y += {"big": 44, "mono": 26, "small": 24}[kind]
    y += 6
    for t in ("dpad L/R: j0   dpad U/D: selected joint (up = -, down = +)   hold: keep moving   triangle: next joint",
              "R1 hold: 2 deg   cross: close   circle: open   HOME hold 1s: home",
              "L2+R2: stop   OPTIONS: resume   window close / Ctrl+C: quit (arm is not moved)"):
        screen.blit(fonts["small"].render(t, True, (150, 150, 160)), (10, y))
        y += 22
    pygame.display.flip()


# =============================================================================
# main
# =============================================================================

def git_info(path):
    def run(*a):
        try:
            return subprocess.run(["git", *a], cwd=_ROOT, capture_output=True, text=True,
                                  timeout=5).stdout.strip()
        except Exception:
            return None
    rel = os.path.relpath(path, _ROOT)
    return {"commit": run("rev-parse", "HEAD"),
            "script_status": run("status", "--porcelain", "--", rel) or "clean"}


def main(argv=None, on_frame=None, start_pose_dry=None):
    ap = argparse.ArgumentParser(description="ゲームパッドで mechArm を手で動かす (既定はドライラン)")
    ap.add_argument("--execute", action="store_true", help="daemon に送る (付けなければドライラン)")
    args = ap.parse_args(argv)

    stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    run_stem = f"teleop_{stamp}" + ("" if args.execute else "_dry")
    if args.execute:
        ensure_ssd_mounted()
        try:
            load_daemon_modules()
        except Exception as ex:          # 読み込みの失敗・値の違い. 記録も daemon もまだ触っていない
            sys.exit(f"[ABORT] daemon の送り口を読めません: {type(ex).__name__}: {ex}。何も送らずに終わります")
        run_dir = LOG_ROOT / run_stem
        if run_dir.exists():
            sys.exit(f"[ABORT] 同じ名前のフォルダがあります: {run_dir}")
        run_dir.mkdir(parents=True)
        script_copy = run_dir / Path(__file__).name
        shutil.copy2(__file__, script_copy)
        log = JsonlLog(run_dir / "log.jsonl")
        print(f"[記録] {run_dir}")
    else:
        # ドライランは記録しない: KIOXIA に何も書かない・フォルダも作らない (依頼 69)
        run_dir = script_copy = None
        log = NullLog()
        print("[記録] ドライランなので記録しません")

    tp = None
    meta = None
    end_reason = None
    daemon_used = False
    try:
        if args.execute:
            get_daemon_socket()
            daemon_used = True
            rpc.install_daemon_lock()
            arm = DaemonArm()
            start = arm.read()
            if start is None:
                end_reason = "start_read_failed"
                sys.exit("[ABORT] 起動したときの関節角を読めません（daemon を確かめてください）。何も送らずに終わります")
            print("[実機] daemon に送ります。起動したときの姿勢: " + _fmt(start))
        else:
            start = np.asarray(start_pose_dry if start_pose_dry is not None else HOME, dtype=float)
            arm = DryArm(start)
            print("[ドライラン] daemon にもアームにも触りません。HOME から始まる仮の姿勢を動かします")
        lo, hi, _, _ = joint_window()
        bad = [k for k in range(6) if not (lo[k] - LIMIT_EPS_DEG <= start[k] <= hi[k] + LIMIT_EPS_DEG)]
        if bad:
            print("[注意] 起動したときの姿勢が守りの外: " + ", ".join(f"j{k}={start[k]:+.2f}" for k in bad)
                  + "。この関節を含む操作は送りません（HOME の長押しは送れます）")

        meta = None if not args.execute else {
            "run_stem": run_stem, "started_at": datetime.now().isoformat(timespec="seconds"),
            "execute": bool(args.execute), "argv": sys.argv,
            "controllers": [],
            "python": sys.version, "pygame": pygame.version.ver,
            "sdl": ".".join(str(v) for v in pygame.get_sdl_version()), "platform": platform.platform(),
            "start_pose": _r(start),
            "guards": {"J1_MAX_DEG": J1_MAX_DEG, "range": "physical_limits (no base window. 2026-11-03)",
                       "J4_SAFE": list(J4_SAFE), "JOINT_LIMITS": [list(x) for x in JOINT_LIMITS],
                       "window": {"lo": _r(lo), "hi": _r(hi)},
                       "home_bypasses_range_check": True},
            "motion": {"speed": SPEED, "step_fine_deg": STEP_FINE_DEG, "step_coarse_deg": STEP_COARSE_DEG,
                       "reach_tol_deg": REACH_TOL_DEG, "reach_timeout_min_sec": REACH_TIMEOUT_SEC,
                       "poll_sec": POLL_SLEEP, "gripper_sleep_sec": rpc.GRIPPER_SLEEP,
                       "grip_close_value": rpc.GRIP_VALUE, "send_wait": rpc.SEND_WAIT,
                       "home": HOME, "home_hold_sec": HOME_HOLD_SEC, "trigger_stop_above": TRIGGER_STOP_ABOVE,
                       "hold_start_sec": HOLD_START_SEC, "hold_interval_sec": HOLD_INTERVAL_SEC,
                       "hold_ahead_deg": HOLD_AHEAD_DEG, "hold_stall_sec": HOLD_STALL_SEC,
                       "ignored_check_sec": IGNORED_CHECK_SEC, "ignored_max_resends": IGNORED_MAX_RESENDS,
                       "j0_left_sign": J0_LEFT_SIGN, "up_sign": UP_SIGN, "select_order": list(SELECT_ORDER)},
            "button_map": {str(k): v for k, v in BUTTON_NAMES.items()},
            "git": git_info(__file__),
            "script_copy": script_copy.name,
            "script_sha256": hashlib.sha256(script_copy.read_bytes()).hexdigest(),
        }
        if meta is not None:
            write_json_atomic(run_dir / "meta.json", meta)

        def on_controller(info):
            if meta is None:
                return
            meta["controllers"].append(info)
            try:
                write_json_atomic(run_dir / "meta.json", meta)
            except Exception as ex:
                print(f"[記録エラー] meta.json に書けません: {ex}")

        tp = Teleop(arm, start, log)
        pygame.init()
        pygame.joystick.init()
        screen = pygame.display.set_mode(WINDOW_SIZE)
        pygame.display.set_caption("gamepad_teleop" + (" [EXECUTE]" if args.execute else " [DRY RUN]"))
        mono = "menlo,consolas,dejavusansmono"
        fonts = {"big": pygame.font.SysFont(mono, 34, bold=True), "mono": pygame.font.SysFont(mono, 20),
                 "small": pygame.font.SysFont(mono, 16)}
        print("コントローラーを待っています（つないでいなければ、今つないでください）")
        end_reason = run_loop(tp, screen, fonts, "EXECUTE" if args.execute else "DRY RUN",
                              on_controller=on_controller, on_frame=on_frame)
    except KeyboardInterrupt:
        end_reason = "ctrl_c"
        print("\n[終了] Ctrl+C")
    finally:
        # 終わるときにアームを動かさない (HOME 復帰もサーボ解放もしない). 新しい指令も送らない
        worker_done = True
        if tp is not None:
            worker_done = tp.shutdown()
            if not worker_done:
                print("[終了] 送っている途中の仕事が戻らないまま終わります（ソケットは閉じません）")
        if meta is not None:
            meta.update(ended_at=datetime.now().isoformat(timespec="seconds"), end_reason=end_reason or "error",
                        n_sent=(tp.n_sent if tp is not None else 0))
            try:
                write_json_atomic(run_dir / "meta.json", meta)
            except Exception as ex:
                print(f"[記録エラー] meta.json に書けません: {ex}")
        log.close()
        if daemon_used and worker_done:
            try:
                close_daemon_socket()
            except Exception:
                pass
        pygame.quit()
        print("[終了] アームは動かしていません。" + (f"記録: {run_dir}" if run_dir is not None else "記録なし（ドライラン）"))
    return 0


if __name__ == "__main__":
    sys.exit(main())
