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

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

操作 (エイイチが決めた. 変えない):
  十字キー 左 / 右 (13 / 14)  j0 を 0.5° ずつ. どの関節を選んでいても いつも j0
                              (向き: 左 = J0_LEFT_SIGN. 依頼に向きの指定がないので, 右 = +, 左 = − にしてある)
  十字キー 上 / 下 (11 / 12)  選んでいる関節を 0.5° ずつ. 上 = −, 下 = +. 始めは j1
  △ (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) で再開
  ほかのボタン (□・SHARE・L1・L3・R3・タッチパッド・スティック) は何もしない

守り:
  - j1 は 90.87 (arm_kinematics.md の PICK_J1_FIXED) を超える指令を送らない
    (fk_lookup.J1_SAFE_MAX = 88.0 は別の用途の値なので, ここでは使わない)
  - j0 は 基準の姿勢 ±15°, j1〜j5 は ±10°. j4 は J4_SAFE (−90〜+60). 物理可動域 (wr.JOINT_LIMITS) も通す
  - 基準の姿勢 = 起動したときに読んだ姿勢. HOME を送ったあとは HOME を基準にし直す
  - 範囲で切られたら理由を画面とログに出す. 別の関節で補わない
  - HOME だけは範囲の検査を通さずに送れる (run_policy_client.py の h と同じ扱い)

送り方:
  - send_angles だけ (send_coords は使わない). speed 20 (j0_sweep_scan.SPEED)
  - 1 回押すごとに目標を 1 つ作って 1 回だけ送り, 3° 以内に届いたかを読んで確かめる.
    送り直しはしない (_send_and_wait_arrival は届かないと 5 秒後に同じ目標を送り直すので,
    その間に停止が押されても送ってしまう. ここでは使わない)
  - 送ってから届くまで (または待ちの上限まで) と, 爪のあとの GRIPPER_SLEEP の間は
    次の動く操作を受け付けない (ためない). 停止はいつでも受け付ける
    (送る仕事は別のスレッドで行い, 画面とコントローラーの読み取りは止めない)
  - 停止が入ったあとは新しい指令を送らない. **動いている途中の動きは止められない**
    (daemon に止める命令がない. 依頼 68 の報告を参照)

1 回の出発点:
  直前に送った目標に届いていて, 送る前に読んだ角度とも 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 に送る

記録: /Volumes/KIOXIA/r2r2r-data/runs/experiments/gamepad_teleop/{run_stem}/
      log.jsonl (押した操作 1 つにつき 1 行) / meta.json / gamepad_teleop.py (使ったスクリプトの写し)
      通しの完走ではないので台帳には書かない.
"""
import argparse
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"))

# run_policy_client を読むと, パス (place_modules / drag_teaching) と pymycobot のスタブが入る.
# 爪 (gripper_cmd)・HOME・J4_SAFE・関節角の読み方 (read_angles) は それと同じものを使う
import run_policy_client as rpc  # noqa: E402
from j0_sweep_scan import POLL_SLEEP, REACH_TIMEOUT_SEC, SPEED  # noqa: E402  20 / 0.3 / 8.0
from visual_servo_pick import (  # noqa: E402
    close_daemon_socket, daemon_send_angles, daemon_set_gripper, daemon_set_gripper_value, get_daemon_socket,
)

wr = rpc.wr

# ---- ボタンと軸 (/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 = 0.5
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.HOME]   # [0, 0, 0, 0, 0, -30]

# ---- 守り ----
J1_MAX_DEG = 90.87             # arm_kinematics.md の PICK_J1_FIXED (エイイチと決めた. J1_SAFE_MAX 88 は使わない)
J0_RANGE_DEG = 15.0            # 基準の姿勢から j0 は ±15°
JOINT_RANGE_DEG = 10.0         # j1〜j5 は ±10°
J4_SAFE = tuple(float(v) for v in rpc.J4_SAFE)   # (-90, 60)
JOINT_LIMITS = [tuple(float(v) for v in lim) for lim in wr.JOINT_LIMITS]   # 物理可動域
LIMIT_EPS_DEG = 1e-6
REACH_TOL_DEG = 3.0            # 届いたかの判定 (推論の到着確認と同じ 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(base):
    """基準の姿勢 base から, 各関節に送ってよい範囲 (lo, hi, 各端の出どころ) を作る.
    出どころ: BASE (基準 ±15/±10) / PHYS (物理可動域) / J4_SAFE / J1_MAX. 狭い方が勝つ."""
    lo, hi, lo_src, hi_src = [], [], [], []
    for k in range(6):
        r = J0_RANGE_DEG if k == 0 else JOINT_RANGE_DEG
        cands_lo = [(float(base[k]) - r, "BASE"), (JOINT_LIMITS[k][0], "PHYS")]
        cands_hi = [(float(base[k]) + r, "BASE"), (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 = {"BASE": "基準の姿勢からの範囲", "PHYS": "物理可動域", "J4_SAFE": "J4_SAFE",
            "J1_MAX": "j1 の上限 90.87 (PICK_J1_FIXED)"}


def plan_step(start, joint, delta, base):
    """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(base)
    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

    def send_and_check(self, target, gate, quitting):
        """target を 1 回だけ send_angles (speed 20) で送り, 3° 以内に届くかを読んで確かめる.
        gate が取り消されていれば送らない. 送り直しはしない. 確かめる途中で読めなければ error."""
        target = np.asarray(target, dtype=float)
        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: daemon_send_angles([float(v) for v in target], speed=SPEED,
                                                       wait=rpc.SEND_WAIT,
                                                       timeout_override=SEND_RPC_TIMEOUT_SEC))
        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()
        last, err = None, None
        base = {"sent": True, "timeout_sec": round(timeout, 2), "t_send": t_send, "measured_before_send": _r(pre)}
        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))
            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)
            if 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))
            time.sleep(POLL_SLEEP)
        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 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()

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

    def send_and_check(self, target, gate, quitting):
        t_send = time.time()
        if not gate.run(lambda: None):
            return {"sent": False, "why": "halted_before_send", "measured_before_send": _r(self.q)}
        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(rpc.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()


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.base = np.asarray(start_pose, dtype=float).copy()       # 基準の姿勢
        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

    # ---- 見せる値 ----
    @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,
                  "base_pose": _r(self.base)})
        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 enter_stop(self, reason, button=None, kind="stop"):
        was = self.stopped
        self.stopped = True
        committed = self._cancel_gate()
        if not was:
            self.stop_reason = reason
        self.out(f"[停止] {reason}。新しい指令は送りません（OPTIONS で再開）"
                 + ("。止める前に送った動きは止まらずに最後まで行きます" 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()

    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
        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):
            sign = J0_LEFT_SIGN if button == BTN_LEFT else -J0_LEFT_SIGN
            self._request(self._entry(button, "move", joint=0, delta=sign * self.step),
                          ("move", 0, sign * self.step))
            return
        if button in (BTN_UP, BTN_DOWN):
            sign = UP_SIGN if button == BTN_UP else -UP_SIGN
            j = self.joint
            self._request(self._entry(button, "move", joint=j, delta=sign * self.step),
                          ("move", j, sign * self.step))
            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 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 tick(self, 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",))

    # ---- 送る仕事 ----
    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, "base": self.base.copy()}
        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 = job
                q = self.arm.read()
                res["measured_before"] = _r(q)
                if q is None:
                    res["error"] = "read_failed"
                    return
                # 出発点: 直前の目標に届いていて, 今読んだ角度とも 3° 以内ならその目標.
                # そうでなければ今読んだ角度 (手で動かされたなど. 古い目標から大きく動かさない. codex 必須 3)
                gap = float(np.max(np.abs(np.asarray(snap["cmd"]) - q)))
                use_cmd = snap["cmd_reached"] and gap <= REACH_TOL_DEG
                start = snap["cmd"] if use_cmd else q
                res["start_source"] = "command" if use_cmd else "measured"
                res["cmd_gap_deg"] = round(gap, 3)
                plan = plan_step(start, j, d, snap["base"])
                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))
            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] == "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] in ("move", "home") and sent is True:
            self.n_sent += 1
            self.cmd = np.asarray(target, dtype=float)
            self.cmd_reached = bool(res.get("reached"))
            if job[0] == "home":
                self.base = np.array(HOME, dtype=float)    # HOME を送ったあとは HOME を基準にし直す
        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}°"
        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}（待ち {rpc.GRIPPER_SLEEP:.1f} 秒のあと受付）")
        elif sent is True:
            r = res.get("reached")
            how = "届いた" if r else ("確かめる途中で終了" if r is None else "届かなかった")
            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":
            self.out(f"[送らない] {label}: 停止が入ったので送りませんでした")
        else:
            self.out(f"[送らない] {label}")
        self.gate = None
        self._finish(e)
        if res.get("error"):
            self.enter_stop(f"arm_error: {res['error']}", kind="arm_error")

    # ---- 終わり ----
    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 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}   base {self.base[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)
        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 = +)   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)

    ensure_ssd_mounted()
    stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    run_stem = f"teleop_{stamp}" + ("" if args.execute else "_dry")
    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}")

    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(start)
        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 = {
            "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, "J0_RANGE_DEG": J0_RANGE_DEG, "JOINT_RANGE_DEG": JOINT_RANGE_DEG,
                       "J4_SAFE": list(J4_SAFE), "JOINT_LIMITS": [list(x) for x in JOINT_LIMITS],
                       "window_at_start": {"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,
                       "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(),
        }
        write_json_atomic(run_dir / "meta.json", meta)

        def on_controller(info):
            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}")
    return 0


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