#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""record_teach_demo.py — 手で教えた動きを daemon 経由で再生しながら π0.5 のお手本として記録する（Mac 側）.

目的:
  drag_teach_v2.py で教えた記録（teach JSON）を、Pi の検証済み再生
  （waypoint_streaming_replay.py = WPSR）と同じ計画・同じ送信流儀で
  **Mac から daemon 経由**で再生し、上カメラ映像（recorder.py）と実測角を
  一緒に残して、既存の学習パイプライン
  （build_demo_episodes.py → convert_to_lerobot.py）にそのまま流せる形にする。

サブコマンド:
  measure  上カメラでブロックと箱の Unity 座標を測って保存する（アームに触らない）
  run      teach JSON を Pi から取得 → 位置を測り直して照合 → 計画表示 → 再生＋記録

1 本のお手本を作る流れ:
  1. ブロックを置く（印は付けない）
  2. 【Mac】 record_teach_demo.py measure
  3. 【Pi】  daemon 停止 → drag_teach_v2.py teach → daemon 起動
  4. 【Mac】 recorder.py --top --no-side を別ターミナルで常駐
  5. 【Mac】 record_teach_demo.py run --teach drag_v2_XXXX.json --execute

再生の計画（姿勢抽出・区間分割・補間）は作り直さず、Pi と同じ実装を import して使う:
  - waypoint_replay           _load_teach / _extract_waypoints / JOINT_LIMITS 系ヘルパ
  - waypoint_streaming_replay _build_segments / _interpolate / _validate_endpoints と送信定数

アーム制御は daemon TCP 経由のみ（send_coords は使わない）。
台帳列は run_pick_place.py の LEDGER_COLS を import して使う（追記ガードを二重定義しない）。

CLI:
  python3 record_teach_demo.py measure [--repeat 5] [--log 352]
  python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json            # ドライラン
  python3 record_teach_demo.py run --teach drag_v2_20221013_144348.json --execute
  python3 record_teach_demo.py run --teach ... --execute --no-record               # 段階C 検証用
  python3 record_teach_demo.py run --teach ... --execute --no-currents             # 電流を残さない（依頼 121）
  python3 record_teach_demo.py run --teach ... --execute --power 2A                # 電源の前後比較（依頼 156。pi05/POWER_AB.md）
"""

from __future__ import print_function

import argparse
import csv
import hashlib
import json
import math
import os
import re
import shutil
import subprocess
import sys
import tempfile
import threading
import time
import traceback
import types
from datetime import datetime
from pathlib import Path


# ---- パス解決 ----------------------------------------------------------------
_THIS = Path(__file__).resolve()
_DRAG_TEACHING_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[2]
_PLACE_MODULES_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "place" / "place_modules"
for _p in (_SCRIPTS_ROOT, _PLACE_MODULES_DIR, _DRAG_TEACHING_DIR):
    sys.path.insert(0, str(_p))
# 依頼 121: 爪と肩の電流の記録は pi05/policy_recorder.py の部品を使う（読み方を 2 か所に書かない）。
# pi05 の同じ名前のモジュールが上の 3 つを隠さないよう、最後に足す。
_PI05_DIR = _SCRIPTS_ROOT / "pi05"
if str(_PI05_DIR) not in sys.path:
    sys.path.append(str(_PI05_DIR))


# ---- pymycobot スタブ --------------------------------------------------------
# waypoint_replay / waypoint_streaming_replay は Pi 用に
# `from pymycobot.mycobot import MyCobot` を持つ。Mac 上で import を通すために
# スタブを置く（run_pick_place.py / run_policy_client.py と同じやり方）。
# 呼び出すと例外になるので、うっかりシリアル直叩きに流れることはない。
if "pymycobot" not in sys.modules:
    _pkg = types.ModuleType("pymycobot")
    _mod = types.ModuleType("pymycobot.mycobot")

    class _MyCobotStub(object):
        def __init__(self, *args, **kwargs):
            raise RuntimeError(
                "record_teach_demo.py: MyCobot stub is not callable. "
                "アーム制御は daemon TCP 経由のみ。")

    _mod.MyCobot = _MyCobotStub
    sys.modules["pymycobot"] = _pkg
    sys.modules["pymycobot.mycobot"] = _mod


# ---- 流用 import ------------------------------------------------------------
# 再生の計画は Pi の検証済み版と同じものを使う（作り直さない）。
import waypoint_replay as wr                       # noqa: E402
import waypoint_streaming_replay as wpsr           # noqa: E402

# 位置検出（アーム制御・daemon 接続を一切しない読み取り専用モジュール）
from detect_positions_once import detect_positions  # noqa: E402

# daemon 経由のアーム制御（π0.5 実行 run_policy_client.py と同じ経路）
from visual_servo_pick import (                    # noqa: E402
    get_daemon_socket,
    close_daemon_socket,
    daemon_send_angles,
    daemon_get_angles,
    daemon_set_gripper,
    daemon_set_gripper_value,
    daemon_set_fresh_mode,
)
import visual_servo_pick as _vsp                   # noqa: E402


def _daemon_request(*args, **kwargs):
    """LOG352: 新 cmd (send_angles_fast / get_angles_once) 用。
    visual_servo_pick.py は無改変のまま、同じモジュールの _daemon_request を
    借りて同じ 1 本の TCP 接続を共有する（本線が依存するファイルを触らない）。
    依頼 121: import した時点の関数ではなく **呼んだときの** _vsp._daemon_request を使う
    （電流の記録スレッドと同じ鍵を通すため。policy_recorder.install_daemon_lock が差し替える）。
    """
    return _vsp._daemon_request(*args, **kwargs)

# 台帳列・保存先・マウントガード・recorder 通知・run_stem 採番は本線から import する。
# LEDGER_COLS を写し取らないので、本線が列を増やしたときに勝手にズレない。
import run_pick_place as rpp                       # noqa: E402
# 到達判定と silent_drop（送信の取りこぼし）の対処は本線から import する。
# run_pick_place.py が `from j0_sweep_scan import _send_and_wait_arrival` で
# 既に読み込んでいるモジュールなので、import による新しい副作用は無い。
import j0_sweep_scan as jss                        # noqa: E402
# 本線の FK 引き（fk_scan_motion_based_v2.csv を **読むだけ**・IK は解かない）。
# run_pick_place.py L74 が同じモジュールを import している。
import fk_lookup as fkl                            # noqa: E402
# fk_lookup が使うのと同じ numpy。FK テーブルの検索にだけ使う。
import numpy as np                                 # noqa: E402
# 依頼 121: 爪と肩の電流（currents.csv）。実機推論の記録（依頼 116）と同じ部品
import policy_recorder as _prec                    # noqa: E402


# ---- 定数 -------------------------------------------------------------------
LOG_PREFIX = "[RTD   ]"

# 再生の送信定数は WPSR からそのまま引く（v2 play 流儀の実績値）。
HOME_ANGLES = list(wpsr.HOME_ANGLES)
SPEED_FIRST = wpsr.SPEED_FIRST                     # 20
SPEED_PLAY = wpsr.SPEED_PLAY                       # 30（補間ストリーミング用・現在未使用）
# 単発送信方式の速度。19 本（run_pick_place.py の SPEED）と
# π0.5 実行（run_policy_client.py）に合わせて 20。
SPEED_SINGLE = 20
FIRST_MOVE_SETTLE_SEC = wpsr.FIRST_MOVE_SETTLE_SEC  # 3.0
GRIPPER_SPEED = wpsr.GRIPPER_SPEED                 # 50
# close の後の待ち。2026-10-19（エイイチ判断・項目4）: 1.5 → 本線と同値の 2.0。
# 出典: run_pick_place.py GRIPPER_SLEEP = 2.0（"DAY175 対策（>=2s）"）。
# _do_gripper は set_gripper_value の直後に time.sleep(GRIPPER_SLEEP) する。
GRIPPER_DWELL_SEC = rpp.GRIPPER_SLEEP              # 2.0
GRIP_VALUE = wpsr.GRIP_VALUE                       # 20（close の押し込み保持目標値）
# run --grip-value N で上の値を置き換える（依頼 152。付けなければ 20 のまま）。試行 JSON の grip_value と
# constants.GRIP_VALUE に残す
GRIP_VALUE_RANGE = (0, 100)
# run --power 2A / 5A（依頼 156。電源の前後比較）。試行 JSON の power に残し、爪の位置と電圧の番地も読む
POWER_RE = re.compile(r"^[0-9]+(\.[0-9]+)?A$")
POWER = None                                        # cmd_run が --power の値を入れる（GRIP_VALUE と同じ扱い）
STREAM_INTERVAL_SEC = wpsr.STREAM_INTERVAL_SEC     # 0.1
STEP_DEG_PER_TICK = wpsr.STEP_DEG_PER_TICK         # 2.0
ARRIVAL_POLL_SLEEP = jss.POLL_SLEEP                # 0.3（本線と同値）
# ---- 到達判定（2026-10-19 エイイチ判断・項目1）------------------------------
# 全区間一律 8.0° をやめ、**本線 run_pick_place.py の stage ごとの値**に揃える。
# 理由: 8.0° では「まだ動いている最中」に到達扱いになり、その姿勢で爪を閉じていた。
#   段階C 1回目の実測: PICK（max|Δ| 14.76°）を **0.383 秒**で到達扱い（残り 6.06°）。
#   j1 は 64.16° → 74.53° と 0.383s で 10.4°（≒27°/s）動いていた＝降下中。
#   その直後に close し、次の lift の send_angles が取りこぼされた。
# 値はすべて run_pick_place.py からの import（数値を書き写さない）。
ARRIVAL_TOL_BY_SEGMENT = {
    # 区間名（wpsr._build_segments の to_name）: (許容差, 本線の出典)
    "HOME":                     (rpp.TOL_HOME,          "TOL_HOME（stage1_to_home / HOME 復帰）"),
    "APPROACH_PICK":            (rpp.TOL_APPROACH,      "TOL_APPROACH（approach 系）"),
    "PICK":                     (rpp.TOL_PICK_PLACE,    "TOL_PICK_PLACE（stage4_pick）"),
    # 2026-10-19（エイイチ判断）: lift だけ 2.0 → 3.0 に緩める。
    # 段階C で 3 回とも残差 2.19° のまま 8 秒待ち切っていた
    #（ブロックの重みで下がる分）。本線 stage10 lift_back の TOL_LIFT_BACK = 3.0 と
    # 同じ理由（"実測残差 2.46〜2.55°（j2主因）を吸収"）なので、その定数を使う。
    "APPROACH_PICK (lift)":     (rpp.TOL_LIFT_BACK,     "TOL_LIFT_BACK（lift の残差 2.19° を吸収）"),
    # 本線は stage7 approach_place だけ TOL_APPROACH_PLACE = 3.0 を使う
    #（"大移動で最後の数度が埋まらない対策"）。approach 系だが 2.0 ではない。
    "APPROACH_PLACE":           (rpp.TOL_APPROACH_PLACE, "TOL_APPROACH_PLACE（stage7）"),
    "PLACE":                    (rpp.TOL_PICK_PLACE,    "TOL_PICK_PLACE（stage8_place）"),
    # 本線は stage10 lift_back だけ TOL_LIFT_BACK = 3.0
    #（"実測残差 2.46〜2.55°（j2主因）を吸収"）。これも approach 系だが 2.0 ではない。
    "APPROACH_PLACE (retreat)": (rpp.TOL_LIFT_BACK,     "TOL_LIFT_BACK（stage10_lift_back）"),
}
# 到達を待つ最大時間も本線に揃える（固定 8.0s をやめる）。
# 本線: timeout = max(jss.REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max))
#   wr._compute_arrival_timeout = clamp(2.0 + (max|Δ| / 20) * 3.0, 8.0, 30.0)
# 大きい区間（max|Δ| 86.7° → 15.0s）に十分な時間を与える。許容差を締めたので
# 固定 8.0s のままだと「間に合わなかっただけ」で ABORT しうる。
ARRIVAL_TIMEOUT_MIN_SEC = jss.REACH_TIMEOUT_SEC    # 8.0
# ---- 爪を動かす前の静止確認（codex R11 A1）----------------------------------
# 許容差を締めても「1 回でも tol 内に読めたら到達」なので、**通過中**でも到達に
# なりうる（段階C 1回目: 14.76° の移動を 0.383s・残り 6.06° で到達扱い）。
# 爪の指令は「アームが止まってから」出す必要があるので、gripper の直前に
#   (a) 直前の move の目標から tol 内にいる
#   (b) 連続する実測の差が GRIPPER_STILL_TOL_DEG 以下
# を GRIPPER_STILL_READS 回続けて確認する。読めない回は成立とみなさない。
# 期限内に確認できなければ **爪へ何も送らずに ABORT**。
# しきい値は本線の「動いていない」判定と同じ 1.0°（jss.RETRY_NO_MOVE_THRESHOLD_DEG）。
# ポーリング間隔は到達確認と同じ 0.3s なので、速度 20（実測 ≒27°/s）で動いていれば
# 連続差は 8° 前後になり、静止（読み取りノイズ 1° 未満）とは明確に分かれる。
GRIPPER_STILL_TOL_DEG = jss.RETRY_NO_MOVE_THRESHOLD_DEG   # 1.0
GRIPPER_STILL_READS = 3            # 連続 3 回（≒0.9s）
GRIPPER_STILL_TIMEOUT_SEC = jss.REACH_TIMEOUT_SEC         # 8.0
# ---- silent_drop（送信の取りこぼし）の対処（項目3）--------------------------
# 本線 j0_sweep_scan._send_and_wait_arrival と同じ条件・同じ回数・同じ待ち。
#   到達タイムアウト後、送信前姿勢からの移動が 1.0° 未満なら「送信無視」とみなし、
#   5.0s 待って現在姿勢を読み直し、**同じ目標**へ 1 回だけ再送する。
#   1.0° 以上動いていたら部分移動/ストール疑いとして再送しない。
#   2 回目も届かなければ ABORT（ループしない・HOME へ自動で戻らない）。
RETRY_PAUSE_SEC = jss.RETRY_PAUSE_SEC              # 5.0
RETRY_NO_MOVE_THRESHOLD_DEG = jss.RETRY_NO_MOVE_THRESHOLD_DEG   # 1.0
# 本線は daemon_send_angles(..., wait=SEND_WAIT_SEC) で送る。daemon は書き込みの
# あと wait 秒 sleep してから送信後の角度を読む。wait=0 だと書き込み直後に読むため
# 読み直し 12 回（≒3.7s）に化けやすい（段階C 1回目の lift が実測 3.7196s）。
SEND_WAIT_SEC = rpp.SEND_WAIT_SEC                  # 1.0
# 依頼 213（依頼 197 の案 2・エイイチ判断 2026-11-15）: 送信のあとの 1.0 秒を **どこで待つか**。
#   "mac"（既定）: daemon には wait=0 で送り、返ってから Mac 側で SEND_WAIT_SEC 待つ。
#                  この間は電流の記録スレッドが読める（_sleep_open で眠りを知らせる）。
#   "daemon"     : 依頼 202 までと同じ。daemon が wait=SEND_WAIT_SEC で寝てから返す（この間は読めない）。
# 待つ長さ（1.0 秒・DAY266: 短縮しない）は変えない。場所だけを移す。cmd_run が --send-wait-on の値を入れる。
SEND_WAIT_ON_CHOICES = ("mac", "daemon")
SEND_WAIT_ON = "mac"
# 送信のソケットの期限。"daemon" では visual_servo_pick._daemon_request が wait+3.0 = 4.0 秒にする。
# "mac" の wait=0 では既定 DAEMON_TIMEOUT_SEC（35 秒）になってしまうので、同じ 4.0 秒を timeout_override で渡す。
SEND_SOCKET_TIMEOUT_SEC = SEND_WAIT_SEC + 3.0
# 直近の _send_angles_guarded の daemon とのやりとりだけの往復（Mac 側の待ちを含まない）。"daemon" では
# daemon の中の 1.0 秒を含む。区間の記録に残す（send_rtt_sec はどちらでも待ちを含む送信の始めから終わりまで）。
_LAST_SEND = {"daemon_rtt_sec": None}
FIRST_MOVE_WARN_DEG = wpsr.FIRST_MOVE_WARN_DEG     # 30.0

# 2026-10-19（エイイチ判断・項目1）: 一律 8.0° はやめた。
# 区間ごとに ARRIVAL_TOL_BY_SEGMENT（本線 run_pick_place.py の stage 値）を使う。
# 旧 8.0° は Pi 配備版 waypoint_streaming_replay.py（0.1s 補間ストリーミング）の
# 値で、ストリーミングでは「少し遅れている」の意味だったが、単発送信では
# 「まだ着いていない」の意味になり、動作中に爪を動かす原因になった
#（段階C 1回目: PICK を 0.383s・残り 6.06° で到達扱い → その姿勢で close）。

# 安全範囲。run_pick_place.py のモジュール定数をそのまま使う（二重定義しない）。
J4_SAFE_MIN = rpp.J4_SAFE_MIN                      # -90.0
J4_SAFE_MAX = rpp.J4_SAFE_MAX                      # +60.0

# ---- 肩（j1・0 始まりの index 1）の上限と、超えた姿勢の置き換え ------------
# 【訂正】2026-10-19 夕に入れた「j2 が +90° を超えたら WARN」は **軸を間違えて
# いた**（index 2 を見ていた）。問題の軸は **肩 = index 1** で、
# arm_kinematics.md の PICK_J1_FIXED も descend_angles[1] に入る値。
#
# 肩がこれを超える姿勢は、コントローラが受け付けず send_angles が捨てられる:
#   手で教えた PICK 肩 95.53 / 97.03 / 99.22 → 4 回とも無視（書き込みは通るのに不動）
#   手で教えた PICK 肩 80.59 → 通った
#   2026-06-26 に 110.5 が無視された記録あり
#   台帳の実測最大 89.73 / **命令の最大 88.0**（エイイチ 2026-10-19）
# 値は本線 fk_lookup.py の J1_SAFE_MAX をそのまま使う（二重定義しない）。
# 同モジュールの docstring が "silent drop の温床となる j1>88° の姿勢は候補から
# 除外する"（DAY257 後段）と書いており、本線の 647 試行が実際に送った j1 は
# 87.74 / 87.00 / 85.00（試行 JSON の stage4_pick target_angles 実測）。
SHOULDER_MAX_DEG = fkl.J1_SAFE_MAX          # 88.0
# 置き換え候補を絞る j0 の窓。候補行の爪先位置はその行の j0 で計算されているので、
# j0 を教示値に差し替えると窓の幅ぶん爪先がずれる（半径 0.32m で 3° ≒ 17mm）。
# 狭くすると候補が減り、広げると爪先差の見積りが当てにならなくなる。
# ±2.0° で「選定時の差」と「置き換え後の再評価の差」が一致することを実測で確認。
SHOULDER_FIX_J0_TOL_DEG = 2.0
# 候補の選び方は本線 PLACE と同じ形（距離 + 角度ペナルティ）。
#   score = 爪先の 3D 距離[m] + W * (|Δj2| + |Δj3| + |Δj4|)
# W は本線 fk_lookup.PLACE_W_J1_PENALTY_DEFAULT（0.002 m/deg = 1°≈2mm 相当）。
# 爪先が同程度なら、教えた姿勢に近い方を採る。
SHOULDER_FIX_ANGLE_PENALTY = fkl.PLACE_W_J1_PENALTY_DEFAULT
# 置き換えで爪先がこれ以上ずれたら、目立つ WARN を出す（**止めない**）。
SHOULDER_FIX_TIP_WARN_MM = 15.0

# ブロックが動いていないかの確認しきい値（measure 時 → 再生直前）
BLOCK_MOVE_TOL_MM = 5.0
# アームの根元（Unity 原点）から これ以内で検出した赤いものは**ブロックとして扱わない**
# （エイイチ判断 2026-10-19）。13:14 の measure が (+48, -34) mm = 根元から 58.8mm の
# 何か（赤いケーブルの可能性）をブロックとして拾っていたのに誰も気づけなかった。
# 教示で使う位置は根元から 200mm 以上離れている。
BLOCK_MIN_DIST_FROM_BASE_MM = 120.0

# ---- 検出のばらつき対策（エイイチ判断 2026-10-19）--------------------------
# homography はマット四隅から**毎回動的に**作られる。四隅の検出が揺れると、
# 同じ画素が別の mm になる。15:25 の measure は、同じ位置のブロックを
# (+212.0, +186.3) mm と記録した（直後の直読みは (+175.1, +195.9) mm）。
# 画素は (505, 585) と (590, 606) で 85px ずれており、mm で 37mm の外れ値だった。
# よって:
#   - measure は homography と四隅の画素位置を保存する
#   - 再生前の確認は **画素** で比べる（homography を作り直さない）
#   - mm は記録用に measure の homography で換算する
#   - 四隅が大きくずれていたら「カメラが動いた可能性」として止める
#   - measure 自体も、5 回の検出がばらついていたら保存せずに終える
#
# setup_homography_and_mat_range が使う四隅の mm 定義（同ファイル L275-280 と同値）。
# homography から四隅の画素位置を厳密に復元するために持つ。
MAT_CORNERS_MM = [(+310.0, -89.0),   # UL
                  (-310.0, -89.0),   # UR
                  (-310.0, +361.0),  # LR
                  (+310.0, +361.0)]  # LL
# 再生前の四隅が measure からこれ以上ずれていたら「カメラが動いた可能性」
MAT_CORNER_SHIFT_ABORT_PX = 15.0
# 四隅の検出は時々外れる（2026-10-19 実測: 同じカメラ・同じ配置で 30px 外れる回があった）。
# 1 回で決めず、15px 以内に収まるまで最大この回数やり直す（エイイチ判断）。
# 1 回でも収まればその回の値で続ける。全部外れたら「カメラが動いた可能性」で止める。
MAT_CORNER_RETRY_MAX = 3
# measure の 5 回の検出で、ブロックの位置がこれ以上ばらついたら保存しない。
# 2026-10-19（エイイチ判断）: 実測のばらつきが 1.30 / 1.80 / 3.15mm で、
# 3.0mm だと 3 回に 1 回落ちた。検出のぶれが 1〜3mm 程度あるので 4.0mm に緩める。
# この位置確認は**つかみ損ねを防ぐための補助**であって、アームの安全装置ではない
# （最終判定は人の y/n と動画）。誤って止まる回数を減らす方を優先する。
MEASURE_BLOCK_SPREAD_MAX_MM = 4.0
# ばらつきの判定に必要な成功サンプル数（codex R8 P5: 1 件だと広がり 0 で通ってしまう）
MEASURE_BLOCK_MIN_SAMPLES = 3
# detect_positions が出した位置と、測り直した中央値の差の上限（mm）。
# 元の検出だけが外れて、あとの画像が安定している場合を取り逃がさないため。
MEASURE_BLOCK_VS_RECHECK_MAX_MM = 3.0
# 再生前のブロック位置の照合に使う許容（mm）。measure の homography で画素に換算する
BLOCK_MOVE_TOL_MM_PX_BASIS = 5.0
# ばらつきを測るときに撮る枚数
INSPECT_FRAMES = 5

# 計画時に j4 の余裕がこれ未満の姿勢があれば WARN（codex R1 C2 追加分）。
# 2026-10-19 の教示データは PICK 姿勢 j4 = -88.68 で下限 -90 に 1.32° しかない。
J4_MARGIN_WARN_DEG = 3.0

# ---- 実測 j4 の扱い（エイイチ判断 2026-10-19・codex R2 C2 への回答）----------
# J4_SAFE (-90〜+60) は **指令値の制限**であって実測の絶対限界ではない。
#   - これまで J4_SAFE は送る前の候補を弾く用途だけで使ってきた
#     （DAY275 で j4=-104.20 の候補を送信前に除外）
#   - 実測は指令からずれる前提（DAY274 で指令 -89.2 に対し実測 -60.73）
#   - 電源投入直後の静止姿勢で j4=-95.97 と読めていて破損もない
# よって実測が J4_SAFE を少し外れるのは WARN + 試行 JSON 記録のままにする
# （到達許容 8° に対し j4 下限の余裕が 1.32° しかなく、実測で止めると正常な試行まで止まる）。
#
# ただし安全網を 1 つ置く: 実測が J4_SAFE ± J4_HARD_MARGIN_DEG の外に読めたら、
# 「到達の遅れ」では説明できない量なので ABORT する。
#
# 2026-10-19（項目1）: 到達許容差を本線の 2.0〜3.0° に締めたが、**この帯は
# 8.0° のまま据え置く**（-98.0 / +68.0 は変えない）。
# 理由: この 8.0° は「到達の遅れとして許す量」ではなく「物理的にありえない量の
# 線引き」として実機で決まった値で、締めると誤 ABORT が増える
#（教示データには計画時点で j4 の余裕が最小 1.32° の点がある）。
J4_HARD_MARGIN_DEG = 8.0
J4_HARD_MIN = J4_SAFE_MIN - J4_HARD_MARGIN_DEG   # -98.0
J4_HARD_MAX = J4_SAFE_MAX + J4_HARD_MARGIN_DEG   # +68.0
# measure が古いときの注意喚起（止めはしない）
MEASURE_STALE_WARN_SEC = 3600.0

# 位置検出の繰り返し回数（中央値集約）
MEASURE_REPEAT_DEFAULT = 5
MEASURE_INTERVAL_DEFAULT = 0.3

# 実測角の読み取り。move 区間は N tick に 1 回読む（0.1s 周期を崩さないため）。
# 3 tick に 1 回 = 約 3.3Hz（これまでの 19 本と同程度の 3Hz 以上）。
ACTUAL_READ_EVERY_N_TICKS = 3
ACTUAL_READ_TARGET_HZ = 3.0
# 依頼 202: 電流の記録スレッド（read_servo・本体が眠っている間だけ）が読んだ角度も states.csv の actual 行にする。
# 列は足さず note 列に入れて、本体の読み（到達・静止の確かめなど。note は空のまま）と区別する。
CURRENT_THREAD_NOTE = "source=current_thread"
CURRENT_ACTUALS_FLUSH_SEC = 1.0   # trial_end の前に、積んだ通知を送り切るのを待つ上限（ふつうは数 ms で終わる）
# 1 コマンドに許す時間。既定 DAEMON_TIMEOUT_SEC=35.0 のままだと 1 回の通信失敗で
# ストリームが長時間止まる（codex R1 C6）。send / get の両方に override を付ける。
# ---- LOG352: daemon の新 cmd を使う（段階C の実測を受けて）-------------------
# 既存 send_angles は送信後に read_angles_with_retry() を呼び、動作中は最悪
# 約 3.7 秒ブロックする（2026-10-19 実測: 474 送信中 27 回）。既存 get_angles も
# 同じ 12 回リトライなので、記録用の読み取りでも同じ停止が起きる。
# よって:
#   送信           → send_angles_fast（読み直し無し。Pi 直と同じ送り方）
#   記録用の読み取り → get_angles_once（1 回だけ読む。読めなければ None）
#   区間終端の到達確認 → 既存 get_angles（アームが止まりかけなのでリトライ有りで良い）
# 2026-10-19（エイイチ判断・方針変更）: 0.1 秒ごとの補間送信をやめ、
# **区間ごとに 1 回だけ send_angles を送って到達を待つ**方式にする
# （run_pick_place.py の 19 本と同じ動かし方）。
# 送信が 158 回 → 9 回になるので、新 cmd（send_angles_fast）は使わない。
# 既存 send_angles（送信後に角度を読み直す）のままで足りる。
USE_FAST_DAEMON_CMDS = False
# get_angles_once 1 回に許す時間（リトライしないので短くて足りる）
DAEMON_GET_ONCE_TIMEOUT_SEC = 2.0

# A4-3（エイイチ判断 2026-10-19）: 送信は 4.0s、読み取りは 2.0s。
# Pi の daemon は send_angles ハンドラの中で、送信後に read_angles_with_retry() を
# 呼ぶ（最悪 GET_ANGLES_RETRIES=12 × GET_ANGLES_INTERVAL_SEC=0.2 ≈ 2.4s）。
# 送信 timeout を 2.0s にすると健全な状態でも空振りし、3 回続くと
# SEND_FAIL_ABORT_STREAK で ABORT してしまう。よって最悪値より長い 4.0s にする。
# codex R8 O2（2026-10-19）: 単発送信では「1 回の失敗 = その区間唯一の指令が
# 未送達かもしれない」。到達確認で捕まえる作りにしていたが、
#   (a) 送信前の姿勢がたまたま目標の 8° 以内だと、送っていなくても「到達」になる
#   (b) 先頭 HOME は settle するだけで到達確認が無い
# ので「必ず捕まえる」は成立しなかった。**送信例外はその場で ABORT** にする
# （自動再送はしない）。補間ストリーミング時代の「連続 3 回で ABORT」は、
# 1 区間 1 送信になった時点で前提が変わっている。
SEND_FAIL_ABORT_ON_FIRST = True

# 単発送信方式では daemon の send_angles ハンドラが送信後に
# read_angles_with_retry() を呼ぶ（動き始めで失敗すると最悪 ≈3.7s）。
# 4.0s では際どいので 6.0s にする。1 試行の送信は 10 回なので待ちは問題にならない。
DAEMON_SEND_TIMEOUT_SEC = 6.0
# 区間終端の到達確認で使う既存 get_angles（12 回リトライ・最悪 ≈3.7s）の期限。
# codex R3 A2/C6: 2.0s だと健全な応答も打ち切りうるので、最悪値より長い 4.0s。
DAEMON_GET_TIMEOUT_SEC = 4.0
# ---- グリッパーの開閉（エイイチ判断 2026-10-19）----------------------------
# 経緯:
#  (1) open を送るだけで「開いたか」を確かめていなかった。2026-10-19 の段階C
#      3本目で PLACE の open が効かず、ブロックを持ったまま HOME へ戻った
#      （daemon の戻り値は None で、DAY175 の既知問題どおり成否と無関係）。
#  (2) そこで daemon の読み取り専用 cmd get_gripper_value（gv）で自動確認する
#      作りにした。が、段階C で PLACE の open 直後に gv が -1 を返し、その直後から
#      アームが send_angles を受け付けなくなった（書き込みは通り elapsed 10.5ms、
#      get_angles も 3.1Hz で読めるのにモーターが動かない。daemon を再起動する
#      まで gv は -1 のままだった = シリアル同期が壊れた疑い）。
#      T1/T2 で「静止中の gv 読み取りは壊さない」ことは確認したが、
#      爪が動いている最中の読み取りが引き金の可能性が残る（T3 は実施しない判断）。
#  (3) よって **gv による自動確認はやめる**。run_pick_place.py の stage9 と同じ形
#      にする（STAGE9_RELEASE_PRE_SLEEP = 2.0 / GRIPPER_SLEEP = 2.0 /
#      STAGE9_RELEASE_OPEN_ATTEMPTS = 2）。647 試行で詰まっていない実績のある形。
# 開いたかは **人が目で見て y/n で答える**（自動確認はしない）。
# 初回 open（PICK の前）も同じ形で、**Enter の前に目視確認の y/n を必須**にする。
# **爪の開閉の待ち時間は縮めない**。
# 2026-10-19（エイイチ判断・項目6）: open は **1 回の命令**にする。
# 本線 run_pick_place.py の _do_gripper("open") と同じ形:
#   daemon_set_gripper(0) 1 回 + time.sleep(GRIPPER_SLEEP = 2.0)
#   （daemon の set_gripper ハンドラが前に sleep(2)、ヘルパ既定 wait=2 で後に sleep(2)）
# → 固定待ちの合計 2.0 + 2.0 + 2.0 = 6.0s。
# stage9（PLACE の離し）だけは本線でも 2 回送り・前 2.0s つきで計 14.0s だが、
# 確認を減らす判断で **PLACE も 1 回**に揃える（本線 _do_gripper と同形）。
GRIPPER_OPEN_DWELL_SEC = rpp.GRIPPER_SLEEP   # 2.0（本線 GRIPPER_SLEEP）
GRIPPER_OPEN_ATTEMPTS = 1                    # 本線 _do_gripper("open") と同じ 1 回
# codex R10 S2: stage9 は daemon_set_gripper(0) を**既定 wait=2** で呼ぶので、
# Pi 側は「事前 sleep(2) → set_gripper_state → 事後 sleep(2)」になる
# （visual_servo_pick.py の daemon_set_gripper(state, speed=100, wait=2) /
#  daemon の set_gripper ハンドラ）。こちらは通知時刻を「指令が爪へ出た時刻」に
# したいので wait=0 で呼ぶ（事後 sleep を daemon にさせない）。
# そのぶんを **Mac 側で同じ 2.0s 待つ**。これを入れないと固定待ちの合計が
# 実績（stage9）より open 1 回あたり 2.0s 短くなる。**待ち時間は縮めない**。
# この 2.0s は **実測角の読み取りを挟まない**（爪を動かした直後にシリアルを
# 触らない。段階C の固まりは gv 読み取りの直後に起きたため）。
GRIPPER_OPEN_POST_SLEEP_SEC = 2.0
# daemon の set_gripper ハンドラが open/close の**前**に必ず入れる固定 sleep。
# DAY175 対策で daemon 側に入っている（daemon は無改変）。所要時間の見込みに使う。
DAEMON_SET_GRIPPER_PRE_SLEEP_SEC = 2.0

# 人の判定入力の再試行上限（stdin が空文字を返し続ける環境で無限ループにしない）
ASK_MAX_ATTEMPTS = 10
# 実測 j4 が J4_SAFE 外に読めたときに値を残すサンプル数の上限（総数は別に数える）
J4_SAMPLE_MAX = 200
# 観測異常（非有限値・実測 j4 の範囲外）の WARN を出す上限回数。
# 総数は試行 JSON の actual_sampling に必ず残すので、画面だけ絞る。
NONFINITE_WARN_MAX = 3
# 待ち時間中も実測角を読むときのスライス幅。
# 「実際に time.sleep した合計」が要求値を下回らない形で使う（codex R1 A7/D7）。
SLEEP_SLICE_SEC = 0.05

# recorder に trial_start を送ってから動き出すまでの間（mp4 writer が開く時間）
RECORDER_START_SETTLE_SEC = 1.0

# send_angles が連続でこの回数失敗したら ABORT する（エイイチ判断・2026-10-19）。
# Pi 直の send 例外（シリアル）と違い、daemon 経由の例外は通信断を意味する。
# 通信が切れたまま区間を進めても、記録が意味のない試行になるだけなので止める。
# 単発の例外は WARN のまま続行する（Pi 配備版と同じ）。成功1回で連続数はリセット。
SEND_FAIL_ABORT_STREAK = 3

# 保存先（KIOXIA のみ。未マウント時にローカルへ fallback しない）
# ismount はマウントポイント自身に対して効くので、volume ルートと
# データルートを分けて持つ（recorder.py の KIOXIA_MOUNT / RUNS_ROOT と同じ形）。
KIOXIA_MOUNT = Path("/Volumes/KIOXIA")
DATA_ROOT = KIOXIA_MOUNT / "r2r2r-data"
# MASTER 保存先テーブルの「校正データ → mecharm_calibration/{topic}_LOG{NNN}/」の行に当てはめる。
# 通し番号は DAY から LOG に切り替わっている（記事ファイルも LOG{NNN}_article.md）。
MEASURE_ROOT = DATA_ROOT / "mecharm_calibration"
MEASURE_DIR_PREFIX = "teach_poses_"

# Pi 側の teach JSON 置き場（ホーム相対）
PI_HOST = "192.168.2.1"
PI_USER = "ubuntu"
PI_KEY_PATH = os.path.expanduser("~/.ssh/id_rsa_pi")
PI_TEACH_DIR = "r2r2r-scripts/dynamic_tracking_limits/drag_teaching"
SCP_TIMEOUT_SEC = 30.0

# 台帳（本線と同じ 1 本・同じ列定義）
LEDGER_CSV_PATH = rpp.LEDGER_CSV_PATH
LEDGER_COLS = rpp.LEDGER_COLS

# recorder が書く recording_meta.json の有界待ち（本線 _append_ledger と同値）
RECORDING_META_POLL_TIMEOUT_SEC = 2.0
RECORDING_META_POLL_INTERVAL_SEC = 0.1


# ---- ログ -------------------------------------------------------------------
def _log(msg):
    print("{} {}".format(LOG_PREFIX, msg))


def _warn(msg):
    print("{} [WARN] {}".format(LOG_PREFIX, msg))


def _abort_msg(msg):
    print("{} [ABORT] {}".format(LOG_PREFIX, msg))


def _fmt_angles(a):
    return wr._format_angles(a)


# ---- 非有限値（NaN / inf）を一律「無効」として扱う -------------------------
# codex R1 C1/C2/C3: NaN は比較が全部 False になるので、上下限検査・距離しきい値を
# すり抜ける。さらに Python の max() は NaN を飛ばすことがあるため
# wr._max_axis_diff([0,0,0,0,NaN,0], [0]*6) が 0.0 を返し、未到達が「到達」に化ける。
# JSON も NaN / Infinity をパースするので、daemon や検出から入りうる。
# よって wr の検査（変更しない）の前段に有限値チェックを必ず置く。
def _is_finite_num(v):
    return (isinstance(v, (int, float)) and not isinstance(v, bool)
            and math.isfinite(float(v)))


def _is_finite_6(a):
    """6 要素すべてが有限の数値なら True."""
    if a is None or not isinstance(a, (list, tuple)) or len(a) != 6:
        return False
    return all(_is_finite_num(v) for v in a)


def _is_valid_angles(a):
    """有限値チェック + wr._is_valid_angles（物理マージン内）。両方通って初めて有効."""
    return _is_finite_6(a) and wr._is_valid_angles(a)


def _max_axis_diff_finite(a, b):
    """6 軸の |a[i]-b[i]| 最大値。非有限が混じるか長さ不正なら None を返す.

    wr._max_axis_diff は NaN を飛ばして偽の 0.0 を返しうるので、こちらで包む。
    """
    if not (_is_finite_6(a) and _is_finite_6(b)):
        return None
    diffs = [abs(float(a[i]) - float(b[i])) for i in range(6)]
    m = max(diffs)
    if not math.isfinite(m):
        return None
    return m


def _arrival_tol_for(to_name):
    """区間名から到達許容差を引く（本線 run_pick_place.py の stage 値）.

    知らない区間名は (None, None) を返す。呼び出し側は **ABORT** する
    （既定値で黙って通さない。教示データの区間構成が変わったことに
    気づかないまま緩い許容差で走るのを防ぐ）。
    戻り値: (tol_deg, 出典の文字列)
    """
    hit = ARRIVAL_TOL_BY_SEGMENT.get(to_name)
    if hit is None:
        return None, None
    return float(hit[0]), hit[1]


def _arrival_timeout_for(max_delta_deg):
    """到達を待つ最大時間（本線と同じ算出）.

    本線 j0_sweep_scan._send_and_wait_arrival:
      timeout = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max))
      wr._compute_arrival_timeout = clamp(2.0 + (max|Δ| / 20) * 3.0, 8.0, 30.0)
    """
    if max_delta_deg is None or not _is_finite_num(max_delta_deg):
        return ARRIVAL_TIMEOUT_MIN_SEC
    return max(ARRIVAL_TIMEOUT_MIN_SEC,
               float(wr._compute_arrival_timeout(float(max_delta_deg))))


def _note_tol_source():
    """到達許容差の出所を表示する（項目1 で本線の値に揃えたことを毎回残す）."""
    _log("到達許容差は本線 run_pick_place.py の stage 値を使います"
         "（区間ごと。一律 8.0° は 2026-10-19 にやめた）:")
    for _name in ("HOME", "APPROACH_PICK", "PICK", "APPROACH_PICK (lift)",
                  "APPROACH_PLACE", "PLACE", "APPROACH_PLACE (retreat)"):
        _tol, _src = _arrival_tol_for(_name)
        _log("  {:<26} {:.1f}°  ← rpp.{}".format(_name, _tol, _src))
    _log("到達を待つ最大時間 = clamp(2.0 + max|Δ|/20*3.0, {:.1f}, 30.0) 秒"
         "（本線と同じ wr._compute_arrival_timeout）".format(ARRIVAL_TIMEOUT_MIN_SEC))


# ---- マウントガード ----------------------------------------------------------
def _ensure_ssd_mounted():
    """KIOXIA が真にマウントされ書き込めるかを確認する.

    os.path.ismount で「ただのパス存在（shim）」を弾き、書き込みテストまで行う
    （recorder.py の ensure_kioxia_mounted と同じ強さ）。
    未マウント時はローカルへ fallback せず False を返す。
    """
    try:
        if not os.path.ismount(str(KIOXIA_MOUNT)):
            return False
    except Exception:
        return False
    try:
        if not DATA_ROOT.is_dir():
            return False
        test = DATA_ROOT / ".record_teach_demo_write_test"
        test.write_text("x")
        test.unlink()
        return True
    except Exception:
        return False


def _require_ssd(what):
    if _ensure_ssd_mounted():
        return
    _abort_msg("KIOXIA が未マウント or 書込不可: mount={} data={}（{}）。"
               "ローカルへは書きません".format(KIOXIA_MOUNT, DATA_ROOT, what))
    sys.exit(2)


class _MountLost(Exception):
    """書き込み直前にマウントが消えていた（codex R1 D6）."""


def _require_mount_now(what):
    """各保存の直前にマウントを再確認する（codex R1 D6）.

    Enter 待ちや再生中に SSD が外れた場合、ローカル側に /Volumes/KIOXIA/... の
    ディレクトリを新規作成させない（fallback 禁止）。失敗させる。
    """
    if _ensure_ssd_mounted():
        return
    raise _MountLost("KIOXIA が未マウント or 書込不可（{}）: mount={}".format(
        what, KIOXIA_MOUNT))


def _atomic_write_json(path, data):
    """本線 run_pick_place._atomic_write_json と同じ書き方（tmp → replace）."""
    rpp._atomic_write_json(path, data)


def _next_run_dir():
    """試行フォルダを 1 つ確保して (run_dir, run_stem) を返す.

    本線 run_pick_place._next_log_path() と同じ採番規則
    （LOG_ROOT / "run_{:04d}_{YYYYMMDD_HHMMSS}"）だが、
    codex R1 B4 を受けて **mkdir(exist_ok=False) で取り、取れなければ次の番号へ**進む。
    本線は exists() 判定 → mkdir(exist_ok=True) なので、同時起動で同じフォルダを
    掴みうる（本線側は変更しない。こちらだけ競合しない取り方にする）。
    """
    log_root = rpp.LOG_ROOT
    log_root.mkdir(parents=True, exist_ok=True)
    ts = datetime.now().strftime("%Y%m%d_%H%M%S")
    for n in range(1, 10000):
        stem = "run_{:04d}_{}".format(n, ts)
        run_dir = log_root / stem
        try:
            run_dir.mkdir(parents=True, exist_ok=False)
        except FileExistsError:
            continue
        return run_dir, stem
    raise RuntimeError("run_stem の採番が 9999 を超えました")


# ---- measure ----------------------------------------------------------------
def _measure_dir(log_no=None):
    """計測バッチの置き場 mecharm_calibration/teach_poses_LOG{NNN}/.

    MASTER 保存先テーブルの「校正データ → mecharm_calibration/{topic}_LOG{NNN}/」に従う。
    --log 未指定時は LOG 番号を推測せず日付名にする（番号を勘で焼かない）。
    """
    if log_no is None:
        name = MEASURE_DIR_PREFIX + datetime.now().strftime("%Y%m%d")
    else:
        name = MEASURE_DIR_PREFIX + "LOG{}".format(int(log_no))
    return MEASURE_ROOT / name


def _h_to_corners_px(H):
    """homography から マット四隅の画素位置を厳密に復元する.

    H = getPerspectiveTransform(四隅画素, 四隅mm) なので、inv(H) を四隅 mm に
    当てれば、その H を作ったときの四隅画素がそのまま戻る。
    """
    import cv2
    import numpy as np
    Hinv = np.linalg.inv(np.asarray(H, dtype=np.float64))
    pts = np.array([[list(c) for c in MAT_CORNERS_MM]], dtype=np.float32)
    r = cv2.perspectiveTransform(pts, Hinv)
    return [[float(x), float(y)] for x, y in r[0]]


def _mm_from_px(H, px, py):
    """指定した homography で 画素 → Unity(m) に換算する（global を使わない）."""
    import cv2
    import numpy as np
    pt = np.array([[[float(px), float(py)]]], dtype=np.float32)
    r = cv2.perspectiveTransform(pt, np.asarray(H, dtype=np.float64))
    return float(r[0][0][0]) * 0.001, float(r[0][0][1]) * 0.001


def _px_from_mm(H, xz):
    """指定した homography で Unity(m) → 画素 に戻す."""
    import cv2
    import numpy as np
    if not (isinstance(xz, (list, tuple)) and len(xz) == 2
            and _is_finite_num(xz[0]) and _is_finite_num(xz[1])):
        return None
    Hinv = np.linalg.inv(np.asarray(H, dtype=np.float64))
    pt = np.array([[[float(xz[0]) * 1000.0, float(xz[1]) * 1000.0]]],
                  dtype=np.float32)
    r = cv2.perspectiveTransform(pt, Hinv)
    x, y = float(r[0][0][0]), float(r[0][0][1])
    if not (math.isfinite(x) and math.isfinite(y)):
        return None
    return [x, y]


def _mm_tol_to_px(H, xz, tol_mm):
    """その場所での tol_mm が何画素にあたるかを、H から換算する."""
    a = _px_from_mm(H, xz)
    b = _px_from_mm(H, [float(xz[0]) + tol_mm / 1000.0, float(xz[1])])
    c = _px_from_mm(H, [float(xz[0]), float(xz[1]) + tol_mm / 1000.0])
    if a is None or b is None or c is None:
        return None
    d1 = math.hypot(b[0] - a[0], b[1] - a[1])
    d2 = math.hypot(c[0] - a[0], c[1] - a[1])
    # x 方向と z 方向で画素換算が違うので、厳しい方（小さい方）を使う
    return min(d1, d2)


def _inspect_top_camera(n_frames=INSPECT_FRAMES):
    """上カメラを 1 回開いて、マット四隅とブロック画素を n 回測る.

    **homography は作り直さない**（visual_servo_pick._homography を上書きしない）。
    detect_positions が立てた homography と、報告された mm の対応を壊さないため。

    戻り値 dict または None:
      corners_px      : 四隅の画素（検出・平均後）
      block_px_samples: 各フレームのブロック画素（検出できた分だけ）
      block_px        : その中央値
      frame           : 最後のフレーム（描画用）
    """
    cap = None
    try:
        import cv2
        import numpy as np
        import camera_resolver
        import visual_servo_pick as vsp

        cams = camera_resolver.resolve_cameras(
            require_top=True, require_side=False, verbose=False)
        idx = (cams or {}).get("top")
        if idx is None:
            _warn("上カメラが解決できません")
            return None
        cap = cv2.VideoCapture(idx)
        if not cap.isOpened():
            _warn("上カメラを開けません")
            return None
        frames = vsp.capture_frames(cap, max(n_frames, vsp.MAT_CORNER_NUM_FRAMES),
                                    vsp.MAT_CORNER_WARMUP_FRAMES,
                                    vsp.MAT_CORNER_FRAME_INTERVAL)
        if not frames:
            _warn("上カメラから映像が来ません")
            return None
        all_corners = [c for c in (vsp.detect_corners(f) for f in frames)
                       if c is not None]
        corners = None
        if all_corners:
            corners = [[float(x), float(y)]
                       for x, y in vsp.remove_outliers_and_average(all_corners)]
        else:
            _warn("マット四隅を検出できませんでした（カメラ移動の確認ができません）")
        samples = []
        for f in frames[-n_frames:]:
            q = vsp.detect_red_block(f)
            if q is not None:
                samples.append([float(q[0]), float(q[1])])
        block_px = None
        if samples:
            xs = sorted(v[0] for v in samples)
            ys = sorted(v[1] for v in samples)
            block_px = [xs[len(xs) // 2], ys[len(ys) // 2]]
        return {"corners_px": corners, "block_px_samples": samples,
                "block_px": block_px, "frame": frames[-1],
                "n_frames": len(frames),
                "n_corner_ok": len(all_corners)}
    except Exception as e:
        _warn("上カメラの確認で例外（記録のみ・本線は続行）: {}: {}".format(
            type(e).__name__, e))
        return None
    finally:
        if cap is not None:
            try:
                cap.release()
            except Exception:
                pass


def _save_reject_json(out_dir, stamp, kind, reason, payload):
    """弾いたときの診断 JSON を残す（codex R9 R3）.

    「3 回とも四隅が外れた」のような**拒否されたケースこそ原因分析に要る**のに、
    以前は何も残らなかった。ファイル名は `*_rejected.json` にして、
    _find_latest_measure の正規表現（measure_YYYYMMDD_HHMMSS.json のみ一致）に
    引っかからないようにする（再生に使われない）。
    """
    try:
        out = dict(payload or {})
        out.update({
            "script": _THIS.name,
            "kind": kind,
            "rejected": True,
            "reason": reason,
            "at": datetime.now().isoformat(),
            "mat_corner_shift_abort_px": MAT_CORNER_SHIFT_ABORT_PX,
            "mat_corner_retry_max": MAT_CORNER_RETRY_MAX,
        })
        path = Path(out_dir) / "{}_{}_rejected.json".format(kind, stamp)
        _require_mount_now("拒否した測定の診断 JSON")
        _atomic_write_json(path, out)
        _log("弾いた理由と各回の記録を残しました: {}".format(path))
        return str(path)
    except Exception as e:
        _warn("診断 JSON を残せませんでした: {}: {}".format(type(e).__name__, e))
        return None


def _corner_shift_px(ref_corners, now_corners):
    """四隅のずれ（各隅の距離のリスト, 最大）を返す. 比べられなければ (None, None)."""
    if not (ref_corners and now_corners
            and len(ref_corners) == 4 and len(now_corners) == 4):
        return None, None
    sh = [math.hypot(now_corners[i][0] - ref_corners[i][0],
                     now_corners[i][1] - ref_corners[i][1]) for i in range(4)]
    return sh, max(sh)


def _inspect_until_corners_match(ref_corners, label):
    """四隅が ref_corners と 15px 以内に収まるまで、最大 3 回測り直す.

    エイイチ判断 2026-10-19: 四隅の検出は時々外れるが、これは
    「つかみ損ねを防ぐ補助」であってアームの安全装置ではない。
    1 回で止めると誤って止まる回数が増えるので、やり直してから判断する。

    戻り値: (採用した inspect の結果 or None, 試行の記録リスト)
      試行の記録: [{attempt, max_shift_px, shifts_px, ok, corners_ok, block_px}]
    """
    attempts = []
    chosen = None
    for attempt in range(1, MAT_CORNER_RETRY_MAX + 1):
        cand = _inspect_top_camera()
        shifts, mx = _corner_shift_px(ref_corners,
                                      (cand or {}).get("corners_px"))
        rec = {
            "attempt": attempt,
            "at": datetime.now().isoformat(),
            "corners_ok": bool(cand and cand.get("corners_px")),
            # codex R9 R3: 四隅そのものも残す（クラスタの判別まで出来るように）
            "corners_px": ((cand or {}).get("corners_px")),
            "ref_corners_px": ([[round(v, 2) for v in c] for c in ref_corners]
                               if ref_corners else None),
            "shifts_px": ([round(x, 2) for x in shifts] if shifts else None),
            "max_shift_px": (round(mx, 2) if mx is not None else None),
            "ok": bool(mx is not None and mx <= MAT_CORNER_SHIFT_ABORT_PX),
            "block_px": ((cand or {}).get("block_px")),
            "block_px_samples": ((cand or {}).get("block_px_samples")),
        }
        attempts.append(rec)
        if cand is not None:
            chosen = cand            # 最後に測れたものは残す（全部外れた時の記録用）
        if rec["ok"]:
            if attempt > 1:
                _log("{}: 四隅が {} 回目で収まりました（最大 {:.1f}px <= {:.1f}px）"
                     .format(label, attempt, mx, MAT_CORNER_SHIFT_ABORT_PX))
            return cand, attempts
        if mx is None:
            _warn("{}: 四隅を比べられませんでした（{} 回目）".format(label, attempt))
        else:
            _warn("{}: 四隅が最大 {:.1f}px ずれています（{} 回目 / 上限 {:.1f}px）{}"
                  .format(label, mx, attempt, MAT_CORNER_SHIFT_ABORT_PX,
                          "。測り直します" if attempt < MAT_CORNER_RETRY_MAX
                          else "。やり直しの上限に達しました"))
    return chosen, attempts


def _px_spread(samples):
    """画素サンプルの広がり（最大の2点間距離）を返す. 2 点未満なら 0.0."""
    if not samples or len(samples) < 2:
        return 0.0
    m = 0.0
    for i in range(len(samples)):
        for j in range(i + 1, len(samples)):
            d = math.hypot(samples[i][0] - samples[j][0],
                           samples[i][1] - samples[j][1])
            if d > m:
                m = d
    return m


def _save_check_image(out_path, tag, block_xz, box_xz, H=None, frame=None):
    """検出したブロックと箱に丸を付けた上カメラ画像を保存する.

    エイイチ判断 2026-10-19: 13:14 の measure が (+48, -34) mm =
    根元から 58.8mm の何か（赤いケーブルの可能性）をブロックとして拾っていたのに、
    画像が無いので誰も気づけなかった。毎回 1 枚残して目で確かめられるようにする。

    detect_positions が内部で立てた homography（visual_servo_pick._homography）を
    逆に使って、**報告された Unity 座標そのもの**を画素に戻して丸を描く
    （検出をやり直さないので、丸の位置と記録された数値が必ず一致する）。
    失敗しても例外にせず False を返す（記録の都合で本線を止めない）。
    """
    cap = None
    try:
        import cv2
        import numpy as np
        import camera_resolver
        import visual_servo_pick as vsp

        # H は明示で渡すのが基本（再生前は measure の homography を使う。
        # 作り直さないので vsp._homography は立っていない）。
        if H is None:
            H = getattr(vsp, "_homography", None)
        if H is None:
            _warn("homography が無いので確認画像は作れません")
            return False
        Hinv = np.linalg.inv(np.asarray(H, dtype=np.float64))

        if frame is None:
            cams = camera_resolver.resolve_cameras(
                require_top=True, require_side=False, verbose=False)
            idx = (cams or {}).get("top")
            if idx is None:
                _warn("上カメラが解決できないので確認画像は作れません")
                return False
            cap = cv2.VideoCapture(idx)
            if not cap.isOpened():
                _warn("上カメラを開けないので確認画像は作れません")
                return False
            for _ in range(10):
                ok, f = cap.read()
                if ok and f is not None:
                    frame = f
        if frame is None:
            _warn("上カメラから映像が来ないので確認画像は作れません")
            return False
        frame = frame.copy()

        def to_px(xz):
            """Unity (m) → マット mm → 画素."""
            if not (isinstance(xz, (list, tuple)) and len(xz) == 2
                    and _is_finite_num(xz[0]) and _is_finite_num(xz[1])):
                return None
            pt = np.array([[[float(xz[0]) * 1000.0, float(xz[1]) * 1000.0]]],
                          dtype=np.float32)
            r = cv2.perspectiveTransform(pt, Hinv)
            px, py = float(r[0][0][0]), float(r[0][0][1])
            if not (math.isfinite(px) and math.isfinite(py)):
                return None
            return int(round(px)), int(round(py))

        # 根元（Unity 原点）と、ブロックとして扱わない半径の輪
        base = to_px([0.0, 0.0])
        ring = []
        for k in range(0, 360, 6):
            a = math.radians(k)
            q = to_px([BLOCK_MIN_DIST_FROM_BASE_MM * math.cos(a) / 1000.0,
                       BLOCK_MIN_DIST_FROM_BASE_MM * math.sin(a) / 1000.0])
            if q:
                ring.append(q)
        if len(ring) > 2:
            cv2.polylines(frame, [np.array(ring, dtype=np.int32)], True,
                          (0, 165, 255), 2)
        if base:
            cv2.drawMarker(frame, base, (0, 165, 255), cv2.MARKER_CROSS, 24, 2)
            cv2.putText(frame, "base (<{:.0f}mm = not block)".format(
                BLOCK_MIN_DIST_FROM_BASE_MM),
                (base[0] + 14, base[1] - 10), cv2.FONT_HERSHEY_SIMPLEX,
                0.5, (0, 165, 255), 1, cv2.LINE_AA)

        for name, xz, color in (("block", block_xz, (0, 0, 255)),
                                ("box", box_xz, (255, 180, 0))):
            q = to_px(xz)
            if q is None:
                cv2.putText(frame, "{}: NOT DETECTED".format(name.upper()),
                            (20, 40 if name == "block" else 66),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2, cv2.LINE_AA)
                continue
            cv2.circle(frame, q, 26, color, 3)
            cv2.drawMarker(frame, q, color, cv2.MARKER_CROSS, 18, 2)
            d = _dist_from_base_mm(xz)
            cv2.putText(frame, "{} ({:+.0f},{:+.0f})mm  base{:.0f}mm".format(
                name, float(xz[0]) * 1000.0, float(xz[1]) * 1000.0,
                d if d is not None else -1),
                (q[0] + 30, q[1] - 12), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
                color, 2, cv2.LINE_AA)
        cv2.putText(frame, tag, (20, frame.shape[0] - 16),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2, cv2.LINE_AA)

        out_path.parent.mkdir(parents=True, exist_ok=True)
        if not cv2.imwrite(str(out_path), frame):
            _warn("確認画像の書き込みに失敗しました: {}".format(out_path))
            return False
        _log("確認画像: {}".format(out_path))
        return True
    except Exception as e:
        _warn("確認画像を作れませんでした（記録のみ・本線は続行）: {}: {}".format(
            type(e).__name__, e))
        return False
    finally:
        if cap is not None:
            try:
                cap.release()
            except Exception:
                pass


def _detect_now(repeat, interval, tag):
    """上カメラでブロックと箱を検出する（アームに触らない）."""
    _log("{}: 上カメラで検出します（repeat={} interval={}s）".format(tag, repeat, interval))
    res = detect_positions(repeat=repeat, interval=interval)
    block = res.get("block_xz_m")
    box = res.get("box_xz_m")
    _log("{}: block = {}".format(tag, _fmt_xz_mm(block)))
    _log("{}: box   = {}  score={}".format(
        tag, _fmt_xz_mm(box), (res.get("detect_score") or {}).get("box")))
    return res


def _dist_from_base_mm(xz):
    """Unity 原点（アームの根元）からの距離 mm。計算できなければ None."""
    if not (isinstance(xz, (list, tuple)) and len(xz) == 2
            and _is_finite_num(xz[0]) and _is_finite_num(xz[1])):
        return None
    d = math.hypot(float(xz[0]) * 1000.0, float(xz[1]) * 1000.0)
    return d if math.isfinite(d) else None


def _block_too_close_to_base(block_xz):
    """ブロックが根元に近すぎないか。近すぎれば理由の文字列、問題なければ None."""
    d = _dist_from_base_mm(block_xz)
    if d is None:
        return None   # 値の妥当性は別のガード（_check_block_not_moved）が見る
    # codex R8 P2: 仕様は「120mm 以内」なので、ちょうど 120.0mm も弾く
    if d <= BLOCK_MIN_DIST_FROM_BASE_MM:
        return ("検出したブロックがアームの根元から {:.1f}mm しか離れていません"
                "（{:.1f}mm 以内は不可）。教示で使う位置は 200mm 以上離れているので、"
                "赤いケーブル等の別のものを拾っている可能性が高いです: {}".format(
                    d, BLOCK_MIN_DIST_FROM_BASE_MM, _fmt_xz_mm(block_xz)))
    return None


def _fmt_xz_mm(xz):
    if not (isinstance(xz, (list, tuple)) and len(xz) == 2
            and isinstance(xz[0], (int, float)) and isinstance(xz[1], (int, float))):
        return "None"
    return "({:+.1f}, {:+.1f}) mm".format(xz[0] * 1000.0, xz[1] * 1000.0)


def cmd_measure(args):
    _note_tol_source()
    _require_ssd("measure の保存")

    out_dir = _measure_dir(args.log)
    out_dir.mkdir(parents=True, exist_ok=True)

    t0 = time.time()
    res = _detect_now(args.repeat, args.interval, "measure")

    if res.get("block_xz_m") is None:
        _warn("ブロックが検出できませんでした。この measure では "
              "run の位置照合（ブロックが動いていないかの確認）が通りません")
    if res.get("box_xz_m") is None:
        _warn("白箱が検出できませんでした（台帳の box_x / box_z は空欄になります）")

    # codex R2 D6: 冒頭の _require_ssd のあとカメラ検出を挟むので、
    # 保存の直前にもマウントを再確認する（検出中の取り外しを想定）。
    try:
        _require_mount_now("measure の保存")
    except _MountLost as e:
        _abort_msg("{}。ローカルへは書きません（measure をやり直してください）".format(e))
        return 2

    stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    out_path = out_dir / "measure_{}.json".format(stamp)
    img_path = out_dir / "measure_{}.jpg".format(stamp)

    # detect_positions が立てた homography を押さえる（これが mm の基準になる）
    # 2026-10-19 方針変更（項目4）: measure の判定は**すべて WARN**にして、
    # 値が取れなくても（null でも）保存する。止めない。
    measure_warnings = []
    import visual_servo_pick as _vsp
    H = getattr(_vsp, "_homography", None)
    if H is None:
        _warn("homography が作れていません（マット四隅の検出に失敗）。"
              "mm と画素の対応が取れないので、関係する値は null で保存します")
        measure_warnings.append("homography_missing")
        H_list = None
        corners_px = None
    else:
        H_list = [[float(v) for v in row] for row in H]
        corners_px = _h_to_corners_px(H)

    corner_attempts = []

    def _reject_payload():
        return {
            "measure_dir": str(out_dir),
            "block_xz_m": res.get("block_xz_m"),
            "box_xz_m": res.get("box_xz_m"),
            "homography": H_list,
            "mat_corners_px": corners_px,
            "check_image": (str(img_path) if img_path.exists() else None),
            "mat_corner_attempts": corner_attempts,
        }

    # ばらつきの確認（同じ homography のまま、ブロック画素を複数回測る）。
    # 四隅が homography のもとと合わなければ最大 3 回やり直す（エイイチ判断）。
    _insp_res = _inspect_until_corners_match(corners_px, "measure")
    insp = _insp_res[0]
    corner_attempts[:] = _insp_res[1]
    if insp is None:
        _warn("上カメラの確認ができませんでした。測り直しの値は null で保存します")
        measure_warnings.append("inspection_failed")
    elif not insp.get("corners_px"):
        _warn("マット四隅を測り直せませんでした。homography の確かさを"
              "確認できません（WARN のみ・保存します）")
        measure_warnings.append("mat_corners_recheck_missing")
    n_samples = len(insp.get("block_px_samples") or []) if insp else 0
    if n_samples < MEASURE_BLOCK_MIN_SAMPLES:
        _warn("ブロックの検出サンプルが {} 件（最低 {} 件）。"
              "ばらつきを判定できません（WARN のみ・保存します）"
              .format(n_samples, MEASURE_BLOCK_MIN_SAMPLES))
        measure_warnings.append("too_few_samples")

    # 検出位置に丸を付けた上カメラ画像を、JSON と同じ時刻の名前で残す
    # （確認で撮ったフレームを使い回す。homography は detect_positions のもの）
    img_ok = _save_check_image(img_path, "measure {}".format(stamp),
                               res.get("block_xz_m"), res.get("box_xz_m"),
                               H=H, frame=(insp.get("frame") if insp else None))

    # measure 自身の homography が信用できるかを確かめる。
    # detect_positions は自前のフレームから四隅を取って homography を作る。
    # その四隅が、直後に測り直した四隅と大きく食い違うなら、homography が
    # 外れていて mm が信用できない（15:25 の 37mm 外れ値はこれ）。
    # 四隅の検出自体は安定している（続けて 4 回測って最大 1.1px）ので、
    # 食い違い = detect_positions 側の四隅が外れた、と読める。
    last_attempt = corner_attempts[-1] if corner_attempts else {}
    _log("homography のもとになった四隅と、測り直した四隅の差: 最大 {}px"
         "（上限 {:.1f}px・やり直し {} 回）".format(
             last_attempt.get("max_shift_px"), MAT_CORNER_SHIFT_ABORT_PX,
             len(corner_attempts)))
    if corner_attempts and not last_attempt.get("ok"):
        _warn("この measure の homography は信用できません: マット四隅が"
              "測り直しと {} 回とも食い違った（各回の最大: {}）。"
              "mm の値が外れている可能性があります（WARN のみ・保存します）"
              .format(len(corner_attempts),
                      ", ".join(str(a.get("max_shift_px"))
                                for a in corner_attempts)))
        measure_warnings.append("mat_corner_mismatch")
        if img_ok:
            _log("確認画像: {}".format(img_path))

    # 根元に近すぎるものはブロックとして扱わない（赤いケーブル等の誤検出）
    too_close = _block_too_close_to_base(res.get("block_xz_m"))
    if too_close:
        _warn(too_close + "（WARN のみ・保存します）")
        measure_warnings.append("block_too_close_to_base")
        if img_ok:
            _log("何を拾ったかは確認画像で見てください: {}".format(img_path))

    block_px = (_px_from_mm(H, res.get("block_xz_m")) if H is not None else None)
    # codex R8 P5: ばらつきは画素換算の概算ではなく、**固定した H で各サンプルを
    # mm に直して**判定する（斜め方向の換算誤差を持ち込まない）。
    _samples = (insp.get("block_px_samples") if insp else None) or []
    spread_px = _px_spread(_samples) if len(_samples) >= 2 else 0.0
    spread_mm = 0.0
    if H is not None and len(_samples) >= 2:
        mm_samples = [_mm_from_px(H, q[0], q[1]) for q in _samples]
        for i in range(len(mm_samples)):
            for j in range(i + 1, len(mm_samples)):
                d = math.hypot((mm_samples[i][0] - mm_samples[j][0]) * 1000.0,
                               (mm_samples[i][1] - mm_samples[j][1]) * 1000.0)
                if d > spread_mm:
                    spread_mm = d
    _log("ブロックのばらつき: {:.1f}px = {:.2f}mm（{} 枚・上限 {:.1f}mm）".format(
        spread_px, spread_mm, n_samples, MEASURE_BLOCK_SPREAD_MAX_MM))
    if spread_mm >= MEASURE_BLOCK_SPREAD_MAX_MM:
        _warn("検出が不安定です: ブロックのばらつきが {:.1f}px = {:.2f}mm"
              "（上限 {:.1f}mm）。（WARN のみ・保存します）".format(
                  spread_px, spread_mm, MEASURE_BLOCK_SPREAD_MAX_MM))
        measure_warnings.append("block_spread_over")
        if img_ok:
            _log("確認画像: {}".format(img_path))

    _b = res.get("block_xz_m")
    _b_ok = (isinstance(_b, (list, tuple)) and len(_b) == 2
             and _is_finite_num(_b[0]) and _is_finite_num(_b[1]))
    if not _b_ok:
        _warn("ブロックを検出できていません（{!r}）。位置は null で保存します"
              .format(_b))
        measure_warnings.append("block_not_detected")
    if insp is None or not insp.get("block_px"):
        _warn("ブロックの測り直しができていません（元の検出と突き合わせ不可）。"
              "（WARN のみ・保存します）")
        measure_warnings.append("block_recheck_missing")

    # codex R8 P5: detect_positions が出した位置と、測り直した中央値を突き合わせる。
    # **元の検出だけが外れて、あとの画像が安定している**場合を取り逃がさない
    # （15:25 の 37mm 外れ値はこの形だった）。
    recheck_mm = None
    vs_recheck_mm = None
    if H is not None and _b_ok and insp is not None and insp.get("block_px"):
        recheck_mm = _mm_from_px(H, insp["block_px"][0], insp["block_px"][1])
        vs_recheck_mm = math.hypot((recheck_mm[0] - _b[0]) * 1000.0,
                                   (recheck_mm[1] - _b[1]) * 1000.0)
        if not _is_finite_num(vs_recheck_mm):
            _warn("元の検出と測り直しの差を計算できません（null で保存します）")
            measure_warnings.append("vs_recheck_not_finite")
            vs_recheck_mm = None
        else:
            _log("元の検出と測り直しの差: {:.2f}mm（上限 {:.1f}mm）".format(
                vs_recheck_mm, MEASURE_BLOCK_VS_RECHECK_MAX_MM))
            if vs_recheck_mm > MEASURE_BLOCK_VS_RECHECK_MAX_MM:
                _warn("元の検出 {} と測り直し {} が {:.2f}mm 食い違います"
                      "（上限 {:.1f}mm）。元の検出が外れている可能性があります"
                      "（WARN のみ・保存します）".format(
                          _fmt_xz_mm(_b), _fmt_xz_mm(recheck_mm),
                          vs_recheck_mm, MEASURE_BLOCK_VS_RECHECK_MAX_MM))
                measure_warnings.append("vs_recheck_over")
                if img_ok:
                    _log("確認画像: {}".format(img_path))
    payload = {
        "script": _THIS.name,
        "kind": "teach_poses_measure",
        "log_no": (int(args.log) if args.log is not None else None),
        "measure_dir": str(out_dir),
        "measured_at": datetime.now().isoformat(),
        "measured_at_ts": t0,
        "repeat": args.repeat,
        "interval": args.interval,
        "block_xz_m": res.get("block_xz_m"),
        "box_xz_m": res.get("box_xz_m"),
        "detect_score": res.get("detect_score"),
        "n_captures": res.get("n_captures"),
        "resolved_top_index": res.get("resolved_top_index"),
        "check_image": (str(img_path) if img_ok else None),
        # 再生前の確認はこの homography と画素位置を基準にする（作り直さない）
        "homography": H_list,
        "mat_corners_px": corners_px,
        "mat_corners_mm": [list(c) for c in MAT_CORNERS_MM],
        "block_px": block_px,
        "box_px": (_px_from_mm(H, res.get("box_xz_m")) if H is not None else None),
        "block_px_samples": (insp.get("block_px_samples") if insp else None),
        "block_px_spread_px": round(spread_px, 2),
        "block_spread_mm": round(spread_mm, 2),
        "block_px_recheck": (insp.get("block_px") if insp else None),
        "block_recheck_xz_m": (list(recheck_mm) if recheck_mm else None),
        "block_vs_recheck_mm": (round(vs_recheck_mm, 2)
                                if vs_recheck_mm is not None else None),
        "block_vs_recheck_max_mm": MEASURE_BLOCK_VS_RECHECK_MAX_MM,
        "block_min_samples": MEASURE_BLOCK_MIN_SAMPLES,
        "block_px_spread_max_mm": MEASURE_BLOCK_SPREAD_MAX_MM,
        "mat_corners_px_recheck": (insp.get("corners_px") if insp else None),
        "mat_corner_attempts": corner_attempts,
        "mat_corner_retry_max": MAT_CORNER_RETRY_MAX,
        "mat_corner_shift_abort_px": MAT_CORNER_SHIFT_ABORT_PX,
        "block_dist_from_base_mm": (
            round(_dist_from_base_mm(res.get("block_xz_m")), 1)
            if _dist_from_base_mm(res.get("block_xz_m")) is not None else None),
        "box_dist_from_base_mm": (
            round(_dist_from_base_mm(res.get("box_xz_m")), 1)
            if _dist_from_base_mm(res.get("box_xz_m")) is not None else None),
        "block_min_dist_from_base_mm": BLOCK_MIN_DIST_FROM_BASE_MM,
        "note": "ブロックに印は付けない（シールが映ると π0.5 が印を目当てに動くおそれがある）",
        # 2026-10-19 方針変更（項目4）: 判定は WARN だけで、保存を止めない。
        # 何が引っかかったかは warnings に残す（空なら全部クリア）。
        "warnings": list(measure_warnings),
        "gate_policy": "warn_only (2026-10-19)",
    }
    _atomic_write_json(out_path, payload)
    _log("保存しました: {}".format(out_path))
    if measure_warnings:
        _warn("この measure には WARN が {} 件あります: {}".format(
            len(measure_warnings), ", ".join(measure_warnings)))
        _warn("値が外れている可能性があります。気になるなら取り直してください"
              "（保存はしてあります）")
    _log("次は Pi で教示 → daemon 起動 → recorder.py --top --no-side 常駐 → "
         "run --teach <file> --execute")
    return 0


_MEASURE_STAMP_RE = re.compile(r"^measure_(\d{8}_\d{6})\.json$")


def _find_latest_measure(explicit=None):
    """直近の measure JSON を返す. (path, payload) または (None, None)."""
    if explicit:
        p = Path(explicit).expanduser()
        if not p.is_file():
            return None, None
        try:
            return p, json.loads(p.read_text())
        except Exception:
            return p, None

    # codex R1 C3: パス全体の文字列順で並べると親ディレクトリ名が先に効く。
    # teach_poses_20261019/ と teach_poses_LOG352/ が混在すると "2" < "L" なので
    # 時刻の古い LOG352 側が「直近」に選ばれてしまう。
    # よって **ファイル名の時刻 measure_{YYYYMMDD_HHMMSS} をキーに並べる**。
    # 時刻として読めないファイル名は候補から外す（名前を推測で救わない）。
    candidates = []
    for p in MEASURE_ROOT.glob(MEASURE_DIR_PREFIX + "*/measure_*.json"):
        m = _MEASURE_STAMP_RE.match(p.name)
        if not m:
            _warn("measure のファイル名が measure_YYYYMMDD_HHMMSS.json 形式でないので "
                  "候補から外します: {}".format(p))
            continue
        candidates.append((m.group(1), str(p), p))
    if not candidates:
        return None, None
    candidates.sort()
    p = candidates[-1][2]
    try:
        return p, json.loads(p.read_text())
    except Exception:
        return p, None


# ---- teach JSON の取得 -------------------------------------------------------
def _fetch_teach_from_pi(name, dest_path):
    """Pi の teach JSON を scp で取ってくる. 成功時 remote_path を返す."""
    remote = "{}/{}".format(PI_TEACH_DIR, name)
    target = "{}@{}:{}".format(PI_USER, PI_HOST, remote)
    cmd = [
        "scp",
        "-i", PI_KEY_PATH,
        "-o", "ConnectTimeout=8",
        "-o", "StrictHostKeyChecking=no",
        "-o", "BatchMode=yes",
        target, str(dest_path),
    ]
    _log("Pi から教えた記録を取得します: {}".format(target))
    try:
        proc = subprocess.run(cmd, capture_output=True, text=True,
                              timeout=SCP_TIMEOUT_SEC)
    except subprocess.TimeoutExpired:
        _abort_msg("scp が {:.0f}s で応答しません: {}".format(SCP_TIMEOUT_SEC, target))
        sys.exit(1)
    except Exception as e:
        _abort_msg("scp の起動に失敗: {}: {}".format(type(e).__name__, e))
        sys.exit(1)
    if proc.returncode != 0:
        _abort_msg("scp 失敗 (rc={}): {}".format(proc.returncode,
                                                 (proc.stderr or "").strip()))
        sys.exit(1)
    _log("取得しました → {}".format(dest_path))
    return remote


def _sha256(path):
    h = hashlib.sha256()
    with open(str(path), "rb") as f:
        for chunk in iter(lambda: f.read(65536), b""):
            h.update(chunk)
    return h.hexdigest()


# ---- 検証 -------------------------------------------------------------------
def _pose_violation(pose):
    """1 姿勢を「有限値 → JOINT_LIMITS → J4_SAFE」の順に検査する.

    codex R1 C1/C2: NaN は上下限比較をすり抜けるので、必ず有限値を先に見る。
    問題なければ None、あれば理由の文字列を返す。
    """
    if not _is_finite_6(pose):
        return "6 軸の有限な数値でない（NaN / inf / 型不正 / 長さ不正）: {!r}".format(pose)
    viol = wr._check_joint_limits(pose)
    if viol:
        return "JOINT_LIMITS 違反: " + ", ".join(
            "j{}={:.3f} not in [{:.1f}, {:.1f}]".format(ax + 1, v, lo, hi)
            for ax, v, lo, hi in viol)
    j4 = float(pose[4])
    if j4 < J4_SAFE_MIN or j4 > J4_SAFE_MAX:
        return "J4_SAFE 違反: j4={:.3f} not in [{:+.1f}, {:+.1f}]".format(
            j4, J4_SAFE_MIN, J4_SAFE_MAX)
    return None


def _j4_margin(pose):
    """j4 の安全範囲までの余裕（度）。非有限なら None."""
    if not _is_finite_6(pose):
        return None
    j4 = float(pose[4])
    return min(j4 - J4_SAFE_MIN, J4_SAFE_MAX - j4)


def _fk_tip_of(pose):
    """角度の最近傍で、その姿勢の爪先位置を FK テーブルから引く（**読み取りのみ**）.

    本線 fk_lookup._load_fk_table() が fk_scan_motion_based_v2.csv を 1 度だけ
    読んでキャッシュしたものを借りる。CSV には一切書かない。
    IK は解かない（実在する FK 行を選ぶだけ）。
    戻り値: (ee [x, y, z] m, 行 index, 最大軸差 deg)。読めなければ (None, None, None)
    """
    try:
        joints, ees, _safe = fkl._load_fk_table()
        d = np.abs(joints - np.asarray(pose, dtype=float)).max(axis=1)
        i = int(np.argmin(d))
        return [float(v) for v in ees[i]], i, float(d[i])
    except (KeyboardInterrupt, SystemExit):
        raise
    except Exception as e:
        _warn("FK テーブルから爪先位置を引けませんでした（止めません）: {}: {}"
              .format(type(e).__name__, e))
        return None, None, None


def _shoulder_replacement(pose):
    """肩を SHOULDER_MAX_DEG 以内に収めた置き換え姿勢を FK テーブルから選ぶ.

    本線 fk_lookup と同じ考え方で選ぶ:
      - 候補は safe_mask の行だけ（JOINT_LIMITS(hard) ∩ j1 <= J1_SAFE_MAX）
      - **j0 と j5 は教えた値のまま**使うので、候補は j0 が教示値
        ±SHOULDER_FIX_J0_TOL_DEG の行に絞る（j0 を差し替えると爪先が回るため）
      - その中から score = 爪先の 3D 距離[m] + W*(|Δj2|+|Δj3|+|Δj4|) が最小の行
      - 返す姿勢は [教示 j0, 行 j1, 行 j2, 行 j3, 行 j4, 教示 j5]
    IK は解かない。send_coords は使わない。CSV は書き換えない。
    戻り値: (new_pose or None, detail dict)
    """
    detail = {
        "shoulder_max_deg": SHOULDER_MAX_DEG,
        "j0_tol_deg": SHOULDER_FIX_J0_TOL_DEG,
        "angle_penalty_m_per_deg": SHOULDER_FIX_ANGLE_PENALTY,
        "fk_table": fkl.FK_CSV.name,
        "method": "fk_nearest_tip_with_angle_penalty",
        "candidate_mask": ("fk_lookup.safe_mask(JOINT_LIMITS hard ∩ j1<=88)"
                           " ∩ |Δj0|<=tol ∩ J4_SAFE"),
        "j4_safe": [J4_SAFE_MIN, J4_SAFE_MAX],
    }
    tip, tip_row, tip_dang = _fk_tip_of(pose)
    detail["taught_tip_xyz_m"] = tip
    detail["taught_tip_fk_row"] = tip_row
    detail["taught_tip_angle_nn_max_deg"] = (round(tip_dang, 3)
                                             if tip_dang is not None else None)
    if tip is None:
        detail["result"] = "no_fk_table"
        return None, detail
    try:
        joints, ees, safe = fkl._load_fk_table()
        ee_t = np.asarray(tip, dtype=float)
        m = safe.copy()
        m &= np.abs(joints[:, 0] - float(pose[0])) <= SHOULDER_FIX_J0_TOL_DEG
        # codex V5: 本線の safe_mask は JOINT_LIMITS(hard) ∩ j1<=88 だけで、
        # **J4_SAFE を含まない**（fk_lookup.py L255 の設計）。
        # ここで当てておかないと、J4_SAFE 外の行を採って計画段階の姿勢検査で
        # 止まり、「置き換えなければ走れた試行」を止めてしまう。
        m &= (joints[:, 4] >= J4_SAFE_MIN) & (joints[:, 4] <= J4_SAFE_MAX)
        n_cand = int(m.sum())
        detail["n_candidates"] = n_cand
        if n_cand == 0:
            detail["result"] = "no_candidate"
            return None, detail
        idx = np.where(m)[0]
        dist3 = np.linalg.norm(ees[idx] - ee_t, axis=1)
        pen = (np.abs(joints[idx, 2] - float(pose[2]))
               + np.abs(joints[idx, 3] - float(pose[3]))
               + np.abs(joints[idx, 4] - float(pose[4])))
        best = int(idx[int(np.argmin(dist3 + SHOULDER_FIX_ANGLE_PENALTY * pen))])
        row = [float(v) for v in joints[best]]
        new_pose = [float(pose[0]), row[1], row[2], row[3], row[4], float(pose[5])]
        # codex V1: 返すのは「FK テーブルに実在する行そのもの」ではない
        #（j0 と j5 を教示値に差し替えている）。教示値が制限外なら最終姿勢が
        # JOINT_LIMITS / J4_SAFE を外れうるので、**採用前に同じ検査を当てる**。
        # 外れたら置き換えない（教えた姿勢のまま進む・止めない）。
        bad = _pose_violation(new_pose)
        if bad:
            detail["result"] = "replacement_rejected"
            detail["rejected_reason"] = bad
            detail["rejected_angles"] = [round(v, 3) for v in new_pose]
            _warn("置き換え候補が姿勢検査に通らないので採用しません"
                  "（教えた姿勢のまま進みます）: {}".format(bad))
            return None, detail
        detail["fk_row"] = best
        detail["fk_row_angles"] = [round(v, 3) for v in row]
        detail["fk_row_tip_xyz_m"] = [float(v) for v in ees[best]]
        detail["selected_tip_dist_mm"] = round(
            float(np.linalg.norm(ees[best] - ee_t)) * 1000.0, 2)
        # 置き換え後の姿勢（j0/j5 を教示値に戻した形）で爪先を**再評価**する。
        # 候補行の爪先はその行の j0 で計算されているので、選定時の距離とは別物。
        tip2, row2, dang2 = _fk_tip_of(new_pose)
        detail["new_tip_xyz_m"] = tip2
        detail["new_tip_fk_row"] = row2
        detail["new_tip_angle_nn_max_deg"] = (round(dang2, 3)
                                              if dang2 is not None else None)
        detail["tip_delta_mm"] = (
            round(float(np.linalg.norm(np.asarray(tip2) - ee_t)) * 1000.0, 2)
            if tip2 is not None else None)
        detail["tip_delta_xz_mm"] = (
            round(float(math.hypot(tip2[0] - ee_t[0], tip2[2] - ee_t[2]))
                  * 1000.0, 2) if tip2 is not None else None)
        detail["tip_delta_y_mm"] = (
            round(float(tip2[1] - ee_t[1]) * 1000.0, 2)
            if tip2 is not None else None)
        detail["result"] = "replaced"
        return new_pose, detail
    except (KeyboardInterrupt, SystemExit):
        raise
    except Exception as e:
        detail["result"] = "error"
        detail["error"] = "{}: {}".format(type(e).__name__, e)
        _warn("肩の置き換えで例外（止めません・教えた姿勢のまま進みます）: {}"
              .format(detail["error"]))
        return None, detail


def _fix_shoulder_over_max(waypoints):
    """肩が上限を超えた教示姿勢を、計画段階で置き換える（送信前）.

    **waypoints は書き換えず、新しい dict を返す**（元の教示値は記録に残す）。
    置き換えられなかった姿勢はそのまま返す（**止めない**）。
    戻り値: (new_waypoints, records list)
    """
    out = dict((k, list(v)) for k, v in waypoints.items())
    records = []
    for name in wr.PHASE_ORDER:
        pose = list(waypoints[name])
        if not (_is_finite_num(pose[1]) and float(pose[1]) > SHOULDER_MAX_DEG):
            continue
        new_pose, detail = _shoulder_replacement(pose)
        rec = {
            "phase": name,
            "taught_angles": [round(v, 3) for v in pose],
            "taught_shoulder_deg": round(float(pose[1]), 3),
        }
        rec.update(detail)
        if new_pose is None:
            rec["new_angles"] = None
            rec["applied"] = False
            _warn("{} の肩が {:+.2f}°（上限 {:.1f}°）ですが、置き換える姿勢が"
                  "見つかりませんでした（{}）。**教えた姿勢のまま進みます**"
                  .format(name, pose[1], SHOULDER_MAX_DEG,
                          detail.get("result")))
        else:
            out[name] = new_pose
            rec["new_angles"] = [round(v, 3) for v in new_pose]
            rec["new_shoulder_deg"] = round(float(new_pose[1]), 3)
            rec["applied"] = True
        records.append(rec)
    return out, records


def _print_shoulder_fix(records):
    """置き換えの前後を目立つ形で出す（計画表示の一部）."""
    if not records:
        return
    print("")
    print("!" * 78)
    print("{} [WARN] 肩（index 1）が上限 {:.1f}° を超える教示姿勢を "
          "{} 件置き換えました（送るのは置き換え後の姿勢です）。".format(
              LOG_PREFIX, SHOULDER_MAX_DEG, len(records)))
    print("{} [WARN] 理由: 手で教えた PICK 肩 95.53 / 97.03 / 99.22 は 4 回とも"
          "send_angles が捨てられた（80.59 は通った）。".format(LOG_PREFIX))
    print("{} [WARN] 本線は肩 87.74 / 87.00 / 85.00 を送って 647 試行動いている"
          "（命令の最大 88.0）。".format(LOG_PREFIX))
    for r in records:
        print("{}   {}".format(LOG_PREFIX, r["phase"]))
        print("{}     教えた姿勢   = {}  （肩 {:+.2f}）".format(
            LOG_PREFIX, _fmt_angles(r["taught_angles"]),
            r["taught_shoulder_deg"]))
        if r.get("applied"):
            print("{}     置き換え後   = {}  （肩 {:+.2f}）".format(
                LOG_PREFIX, _fmt_angles(r["new_angles"]), r["new_shoulder_deg"]))
            print("{}     爪先の差     = {} mm（xz {} / 上下 {}）  "
                  "FK 行 {} / 候補 {} 件".format(
                      LOG_PREFIX, r.get("tip_delta_mm"),
                      r.get("tip_delta_xz_mm"), r.get("tip_delta_y_mm"),
                      r.get("fk_row"), r.get("n_candidates")))
            print("{}     j0 と j5 は教えた値のまま（{:+.2f} / {:+.2f}）".format(
                LOG_PREFIX, r["taught_angles"][0], r["taught_angles"][5]))
            d = r.get("tip_delta_mm")
            if d is not None and d > SHOULDER_FIX_TIP_WARN_MM:
                print("{}     [WARN] 爪先が {} mm ずれています"
                      "（目安 {:.0f} mm 超）。つかみ損ねる可能性があります"
                      .format(LOG_PREFIX, d, SHOULDER_FIX_TIP_WARN_MM))
        else:
            print("{}     置き換えできませんでした（{}）。"
                  "教えた姿勢のまま送ります".format(
                      LOG_PREFIX, r.get("result")))
    print("{} [WARN] FK テーブル（{}）は読んだだけで、書き換えていません。"
          .format(LOG_PREFIX, fkl.FK_CSV.name))
    print("!" * 78)
    print("")


def _check_all_poses(seq, waypoints):
    """送る姿勢と、その間を通る姿勢を _pose_violation で検査する.

    単発送信方式では送るのは各区間の終点（= waypoint か HOME）だけだが、
    アームは 2 点間を補間して動くので、**その経路上の姿勢も検査する**
    （wpsr._interpolate の凸結合は JOINT_LIMITS / J4_SAFE に対して
    端点検査で足りるが、j4 の余裕の最小値を出すために実際に全点を見る）。
    戻り値: (has_violation, n_checked, min_j4_margin)
    """
    violations = []
    margins = []
    sh_over = []     # 肩 > SHOULDER_MAX_DEG の姿勢（WARN 用・止めない）

    def _check(where, tick, pose):
        reason = _pose_violation(pose)
        if reason:
            violations.append((where, tick, pose, reason))
        m = _j4_margin(pose)
        if m is not None:
            margins.append((m, where, tick, pose))
        # 肩（index 1）が受け付けられない帯に入っていないか（2026-10-19 実測）。
        # 【訂正】以前は index 2 を見ていた。問題の軸は index 1。
        if _is_finite_num(pose[1]) and float(pose[1]) > SHOULDER_MAX_DEG:
            sh_over.append((float(pose[1]), where, tick))

    _check("HOME", 0, HOME_ANGLES)
    for name in wr.PHASE_ORDER:
        _check(name, 0, waypoints[name])

    n_checked = 1 + len(wr.PHASE_ORDER)
    for s in seq:
        if s["kind"] != "move":
            continue
        # 送るのは終点だけだが、アームは 2 点間を補間して動くので経路上も見る
        for k, pose in enumerate(wpsr._interpolate(s["from"], s["to"], s["n_steps"])):
            n_checked += 1
            _check("{} → {}".format(s["from_name"], s["to_name"]), k + 1, pose)

    # 肩が受け付けられない帯に残っている姿勢を、目立つ形で知らせる（**止めない**）。
    # 通常は _fix_shoulder_over_max が計画段階で置き換えているので、ここに出るのは
    # 「置き換えられなかった姿勢」か「補間の経路上だけ超える姿勢」。
    # 姿勢検査の合否より先に出す（別の理由で止まる場合でも見えるように）。
    if sh_over:
        worst = max(sh_over)
        print("")
        print("!" * 78)
        print("{} [WARN] 肩（index 1）が +{:.1f}° を超える姿勢が {} 件残っています"
              "（最大 {:+.2f} / {} 経路上の点 {}）。".format(
                  LOG_PREFIX, SHOULDER_MAX_DEG, len(sh_over),
                  worst[0], worst[1], worst[2]))
        print("{} [WARN] **この姿勢はアームが受け付けず、送っても動きません。**"
              .format(LOG_PREFIX))
        print("{} [WARN] **ブロックを手前に寄せて教え直してください。**"
              .format(LOG_PREFIX))
        print("{} [WARN] 根拠: 手で教えた PICK 肩 95.53 / 97.03 / 99.22 は 4 回とも"
              "無視された / 80.59 は通った。本線の命令の最大は 88.0。"
              .format(LOG_PREFIX))
        print("{} [WARN] 止めはしません。このまま進めると、その区間で"
              "「送り直しても動かない」として中断する見込みです。".format(LOG_PREFIX))
        for v, where, tick in sorted(sh_over, reverse=True)[:5]:
            print("{}          肩={:+.2f}  {}  経路上の点 {}".format(
                LOG_PREFIX, v, where, tick))
        if len(sh_over) > 5:
            print("{}          ... 他 {} 件".format(LOG_PREFIX, len(sh_over) - 5))
        print("!" * 78)
        print("")

    if violations:
        _abort_msg("姿勢検査（有限値 / JOINT_LIMITS / J4_SAFE）違反が {} 件"
                   "（全 {} 姿勢を検査）".format(len(violations), n_checked))
        for where, tick, pose, reason in violations[:10]:
            _log("  {}  経路上の点 {}  {}".format(where, tick, reason))
        if len(violations) > 10:
            _log("  ... 他 {} 件".format(len(violations) - 10))
        return True, n_checked, None

    min_margin = min(margins) if margins else None
    _log("姿勢検査 OK（全 {} 姿勢が 有限値 + JOINT_LIMITS + "
         "J4_SAFE [{:+.1f}, {:+.1f}] 内）".format(
             n_checked, J4_SAFE_MIN, J4_SAFE_MAX))
    if min_margin is not None:
        m, where, tick, pose = min_margin
        if m < J4_MARGIN_WARN_DEG:
            # codex R1 C2 追加分: 余裕が薄い教示データを計画時に知らせる。
            _warn("j4 の余裕が最小 {:.2f}°（< {:.1f}°）です: {} 経路上の点 {} j4={:+.2f}。"
                  "実測が安全範囲の外に振れる可能性があります"
                  "（実測では止めません。試行 JSON に WARN として残します）".format(
                      m, J4_MARGIN_WARN_DEG, where, tick, float(pose[4])))
        else:
            _log("j4 の余裕は最小 {:.2f}°（しきい値 {:.1f}° 以上）".format(
                m, J4_MARGIN_WARN_DEG))
    return False, n_checked, (min_margin[0] if min_margin else None)


def _check_camera_and_block_px(measure_payload, insp):
    """再生前の確認を **画素** で行う（エイイチ判断 2026-10-19）.

    homography を作り直さず、measure が保存した homography と画素位置を基準にする。
      1. マット四隅が measure から大きくずれていたら「カメラが動いた可能性」→ 止める
      2. ブロックの画素位置を measure の画素位置と比べる
         （許容は 5mm 相当の画素数。measure の homography から換算する）
      3. mm の値は記録用に、measure の homography で換算する

    戻り値: (ok, detail dict, reason)
    """
    detail = {"basis": "pixel_with_measure_homography",
              "tol_mm": BLOCK_MOVE_TOL_MM_PX_BASIS,
              "mat_corner_shift_abort_px": MAT_CORNER_SHIFT_ABORT_PX}

    H = (measure_payload or {}).get("homography")
    m_block_px = (measure_payload or {}).get("block_px")
    m_corners = (measure_payload or {}).get("mat_corners_px")
    if not H or not m_block_px:
        detail["fail_kind"] = "measure_format"
        return False, detail, (
            "measure に homography / ブロックの画素位置がありません"
            "（古い形式の measure です）。measure を取り直してください")
    if insp is None:
        detail["fail_kind"] = "no_inspection"
        return False, detail, "再生直前に上カメラを確認できませんでした"

    # 1. カメラが動いていないか（マット四隅のずれ）
    n_corners = insp.get("corners_px")
    if m_corners and n_corners and len(m_corners) == len(n_corners) == 4:
        shifts = [math.hypot(n_corners[i][0] - m_corners[i][0],
                             n_corners[i][1] - m_corners[i][1])
                  for i in range(4)]
        detail["mat_corner_shift_px"] = [round(x, 2) for x in shifts]
        detail["mat_corner_shift_max_px"] = round(max(shifts), 2)
        if max(shifts) > MAT_CORNER_SHIFT_ABORT_PX:
            detail["fail_kind"] = "mat_corner_shift"
            return False, detail, (
                "マット四隅が measure 時から最大 {:.1f}px ずれています"
                "（上限 {:.1f}px・最大 {} 回やり直しても収まらず）。"
                "カメラが動いた可能性があるので止めます。"
                "measure を取り直してください（各隅のずれ: {}）".format(
                    max(shifts), MAT_CORNER_SHIFT_ABORT_PX, MAT_CORNER_RETRY_MAX,
                    ", ".join("{:.1f}".format(x) for x in shifts)))
    else:
        # codex R8 P4: 比べられない = カメラが動いていないことを確認できない。
        # WARN で通すと「確認せずに再生」になるので、照合失敗として扱う。
        detail["mat_corner_shift_px"] = None
        detail["fail_kind"] = "mat_corner_incomparable"
        return False, detail, (
            "マット四隅を比べられませんでした（measure 側 {} / 今回 {}）。"
            "カメラが動いていないことを確認できないので止めます".format(
                "有り" if m_corners else "無し",
                "有り" if n_corners else "無し"))

    # 2. ブロックの画素位置を比べる
    n_block_px = insp.get("block_px")
    if not n_block_px:
        detail["fail_kind"] = "block_undetected"
        return False, detail, (
            "再生直前にブロックを検出できません（画素位置が取れない）。"
            "動いていないかを確認できないので止めます")
    detail["measure_block_px"] = [round(v, 1) for v in m_block_px]
    detail["now_block_px"] = [round(v, 1) for v in n_block_px]
    dpx = math.hypot(n_block_px[0] - m_block_px[0], n_block_px[1] - m_block_px[1])
    detail["block_shift_px"] = round(dpx, 2)

    tol_px = _mm_tol_to_px(H, _mm_from_px(H, m_block_px[0], m_block_px[1]),
                           BLOCK_MOVE_TOL_MM_PX_BASIS)
    if tol_px is None or not math.isfinite(tol_px) or tol_px <= 0:
        detail["fail_kind"] = "tol_px_invalid"
        return False, detail, "許容の画素換算ができませんでした（homography が不正）"
    detail["tol_px"] = round(tol_px, 2)

    # 3. mm は記録用に measure の homography で換算する。
    # codex R9 Q3: 判定は **丸める前の値**で行う（丸めてから比べると
    # 5.004mm が 5.0mm になって許容内に化ける）。丸めは記録用にだけ使う。
    dist_mm_raw = None
    try:
        now_mm = _mm_from_px(H, n_block_px[0], n_block_px[1])
        mea_mm = _mm_from_px(H, m_block_px[0], m_block_px[1])
        dx_raw = (now_mm[0] - mea_mm[0]) * 1000.0
        dz_raw = (now_mm[1] - mea_mm[1]) * 1000.0
        dist_mm_raw = math.hypot(dx_raw, dz_raw)
        detail["now_block_xz_m"] = [now_mm[0], now_mm[1]]
        detail["measure_block_xz_m"] = [mea_mm[0], mea_mm[1]]
        detail["dx_mm"] = round(dx_raw, 2)
        detail["dz_mm"] = round(dz_raw, 2)
        detail["dist_mm"] = round(dist_mm_raw, 2)
    except Exception as e:
        detail["mm_error"] = "{}: {}".format(type(e).__name__, e)

    if dpx > tol_px:
        detail["ok"] = False
        detail["fail_kind"] = "block_moved"
        return False, detail, (
            "ブロックが measure 時から {:.1f}px（≒{:.1f}mm）動いています"
            "（許容 {:.1f}px = {:.1f}mm）。ブロックを戻すか measure を"
            "やり直してください".format(
                dpx, detail.get("dist_mm", float("nan")),
                tol_px, BLOCK_MOVE_TOL_MM_PX_BASIS))

    # codex R8 P4: 画素換算（_mm_tol_to_px）は x 方向と z 方向の厳しい方を採るが、
    # 射影変換では斜め方向がそれより緩くなりうる。**保存した H で換算した実距離**
    # も 5mm 以内であることを必須にする（H は作り直さないので揺れは入らない）。
    if dist_mm_raw is None or not _is_finite_num(dist_mm_raw):
        detail["ok"] = False
        detail["fail_kind"] = "block_move_mm_unknown"
        return False, detail, "ブロックの移動量を mm で計算できませんでした"
    if dist_mm_raw > BLOCK_MOVE_TOL_MM_PX_BASIS:
        detail["ok"] = False
        detail["fail_kind"] = "block_moved"
        return False, detail, (
            "ブロックが measure 時から {:.3f}mm 動いています（許容 {:.1f}mm）。"
            "画素では {:.1f}px で許容内でしたが、斜め方向の換算で超えました".format(
                dist_mm_raw, BLOCK_MOVE_TOL_MM_PX_BASIS, dpx))
    detail["ok"] = True
    return True, detail, None


def _check_block_not_moved(measure_payload, now_res):
    """measure 時と再生直前のブロック位置の差を見る.

    戻り値: (ok, detail dict, reason)。ok=False なら呼び出し側が止める。
    """
    m_block = (measure_payload or {}).get("block_xz_m")
    n_block = now_res.get("block_xz_m")

    def _valid(xz):
        # codex R1 C3: 非有限（NaN / inf）は hypot も NaN になり
        # 「NaN > 5.0 == False」でしきい値をすり抜けるので、ここで弾く。
        return (isinstance(xz, (list, tuple)) and len(xz) == 2
                and _is_finite_num(xz[0]) and _is_finite_num(xz[1]))

    if not _valid(m_block):
        return False, {"tol_mm": BLOCK_MOVE_TOL_MM,
                       "measure_block_xz_m": m_block}, \
            ("measure のブロック位置が無い、または有限な数値でありません（{!r}）。"
             "measure をやり直してください".format(m_block))
    if not _valid(n_block):
        return False, {"tol_mm": BLOCK_MOVE_TOL_MM,
                       "now_block_xz_m": n_block}, \
            ("再生直前のブロック位置が検出できない、または有限な数値でありません（{!r}）。"
             "ブロックが動いていないかを確認できないので止めます".format(n_block))

    dx_mm = (n_block[0] - m_block[0]) * 1000.0
    dz_mm = (n_block[1] - m_block[1]) * 1000.0
    dist_mm = math.hypot(dx_mm, dz_mm)
    detail = {
        "measure_block_xz_m": list(m_block),
        "now_block_xz_m": list(n_block),
        "dx_mm": round(dx_mm, 2),
        "dz_mm": round(dz_mm, 2),
        "dist_mm": round(dist_mm, 2),
        "tol_mm": BLOCK_MOVE_TOL_MM,
    }
    if not math.isfinite(dist_mm):
        # 両座標が有限でも、差の計算で inf が出る極端値を取りこぼさないための保険。
        detail["ok"] = False
        return False, detail, \
            "ブロック位置の差が有限な数値になりません（dx={!r} dz={!r}）".format(dx_mm, dz_mm)
    if dist_mm > BLOCK_MOVE_TOL_MM:
        detail["ok"] = False
        return False, detail, (
            "ブロックが measure 時から {:.1f}mm 動いています（許容 {:.1f}mm・"
            "dx={:+.1f} dz={:+.1f}）。ブロックを戻すか measure をやり直してください".format(
                dist_mm, BLOCK_MOVE_TOL_MM, dx_mm, dz_mm))
    detail["ok"] = True
    return True, detail, None


# ---- 計画表示 ---------------------------------------------------------------
def _plan_summary(seq):
    """単発送信方式の所要時間の見込み.

    各 move 区間は「1 回送信 → 到達確認（最大は区間ごと・本線と同じ算出）」。
    実際の移動時間は速度 20 で max|Δ| に比例するので、
    20°/s を目安にした概算と、到達確認の最大待ちの 2 通りを出す。
    """
    n_moves = 0
    n_grippers = 0
    n_opens = 0
    sum_delta = 0.0
    gripper_sec = 0.0
    for s in seq:
        if s["kind"] == "gripper":
            n_grippers += 1
            if s["action"] == "close":
                # close: daemon 側の事前 sleep は set_gripper_value には無い
                gripper_sec += GRIPPER_DWELL_SEC
            else:
                # open（項目6）: 1 回ごとに daemon 事前 2.0 + 事後相当 2.0 + 待ち 2.0
                n_opens += 1
                gripper_sec += (
                    GRIPPER_OPEN_ATTEMPTS * (DAEMON_SET_GRIPPER_PRE_SLEEP_SEC
                                             + GRIPPER_OPEN_POST_SLEEP_SEC
                                             + GRIPPER_OPEN_DWELL_SEC))
        else:
            n_moves += 1
            sum_delta += float(s["max_delta"])
    # 速度 20 ≒ 20°/s を目安にした移動時間の概算
    move_sec_est = sum_delta / 20.0
    worst_sec = (FIRST_MOVE_SETTLE_SEC + gripper_sec
                 + n_moves * ARRIVAL_TIMEOUT_MIN_SEC)
    total_sec = FIRST_MOVE_SETTLE_SEC + move_sec_est + gripper_sec
    return {
        "n_moves": n_moves,
        "n_grippers": n_grippers,
        "n_opens": n_opens,
        "gripper_open_sends": n_opens * GRIPPER_OPEN_ATTEMPTS,
        "sum_max_delta_deg": round(sum_delta, 2),
        "move_sec_est": round(move_sec_est, 1),
        "gripper_sec": round(gripper_sec, 1),
        "estimated_sec": round(total_sec, 1),
        "worst_case_sec": round(worst_sec, 1),
    }


def _print_plan(current, seq):
    """Pi 版 WPSR の _print_plan と同じ書式で出す（段階A で並べて比べられるように）.

    違いは送信口が daemon_* になる点だけ。姿勢・区間・tick 数は同じになるはず。
    """
    print("=" * 78)
    print("{} 実行プラン（daemon 経由・区間ごとに 1 回送信 → 到達待ち）".format(
        LOG_PREFIX))
    print("=" * 78)

    if current is not None:
        max_d0, _ = wr._max_axis_diff(HOME_ANGLES, current)
        print("  [STEP  1] MOVE → HOME (single send + settle)  speed={}".format(SPEED_FIRST))
        print("            current = {}".format(_fmt_angles(current)))
        print("            to HOME = {}".format(_fmt_angles(HOME_ANGLES)))
        print("            max |Δ| = {:.2f}°  (settle {:.1f}s)".format(
            max_d0, FIRST_MOVE_SETTLE_SEC))
    else:
        print("  [STEP  1] MOVE → HOME (single send + settle)  speed={}".format(SPEED_FIRST))
        print("            current = 未取得（ドライラン・daemon に接続していない）")
        print("            to HOME = {}".format(_fmt_angles(HOME_ANGLES)))

    step_no = 2
    for s in seq:
        if s["kind"] == "gripper":
            if s["action"] == "close":
                cmd_str = "daemon_set_gripper_value({}, {})".format(GRIP_VALUE, GRIPPER_SPEED)
                print("  [STEP {:>2}] GRIPPER {}  {} + sleep({:.1f}s)".format(
                    step_no, s["action"].upper(), cmd_str, GRIPPER_DWELL_SEC))
            else:
                cmd_str = "daemon_set_gripper(0, {}, wait=0)".format(GRIPPER_SPEED)
                # 本線 run_pick_place.py の _do_gripper("open") と同じ待ち方（項目6）
                print("  [STEP {:>2}] GRIPPER {}  {} ×{} 回"
                      "（daemon 事前 {:.1f}s + 事後 {:.1f}s + 待ち {:.1f}s）"
                      "= 計 {:.1f}s".format(
                          step_no, s["action"].upper(), cmd_str,
                          GRIPPER_OPEN_ATTEMPTS,
                          DAEMON_SET_GRIPPER_PRE_SLEEP_SEC,
                          GRIPPER_OPEN_POST_SLEEP_SEC, GRIPPER_OPEN_DWELL_SEC,
                          GRIPPER_OPEN_ATTEMPTS
                          * (DAEMON_SET_GRIPPER_PRE_SLEEP_SEC
                             + GRIPPER_OPEN_POST_SLEEP_SEC
                             + GRIPPER_OPEN_DWELL_SEC)))
                print("            開いたかは確認しません"
                      "（gv を使わず・開始前の y/n も無し。判定は終了後の y/n と動画）")
        else:
            _tol, _src = _arrival_tol_for(s["to_name"])
            print("  [STEP {:>2}] MOVE → {}  segment {} → {}{}  "
                  "tol={}{}".format(
                      step_no, s["to_name"], s["from_name"], s["to_name"],
                      "  [critical]" if s.get("critical") else "",
                      ("{:.1f}°".format(_tol) if _tol is not None
                       else "**未定義（実機では ABORT）**"),
                      ("  [rpp.{}]".format(_src) if _src else "")))
            print("            from    = {}".format(_fmt_angles(s["from"])))
            print("            to      = {}".format(_fmt_angles(s["to"])))
            print("            max |Δ| = {:.2f}°  1 回送信 speed={}  "
                  "(~ {:.1f}s @ 20°/s 目安・到達待ち 最大 {:.1f}s)".format(
                      s["max_delta"], SPEED_SINGLE,
                      float(s["max_delta"]) / 20.0,
                      _arrival_timeout_for(s["max_delta"])))
        step_no += 1

    summary = _plan_summary(seq)
    print("=" * 78)
    print("{} 送信 = {} 回（STEP 1 + move {} 区間）, "
          "想定所要時間 ≈ {:.1f}s（最悪 {:.1f}s）".format(
              LOG_PREFIX, summary["n_moves"] + 1, summary["n_moves"],
              summary["estimated_sec"], summary["worst_case_sec"]))
    print("{}   内訳: 先頭 settle {:.1f}s + 移動 {:.1f}s（max|Δ| 合計 {:.1f}° @ 20°/s 目安）"
          " + gripper {:.1f}s".format(
              LOG_PREFIX, FIRST_MOVE_SETTLE_SEC, summary["move_sec_est"],
              summary["sum_max_delta_deg"], summary["gripper_sec"]))
    print("{}   到達確認: 許容は区間ごと（本線 run_pick_place.py の stage 値）/ "
          "最大は clamp(2.0 + max|Δ|/20*3.0, {:.1f}, 30.0) 秒"
          .format(LOG_PREFIX, ARRIVAL_TIMEOUT_MIN_SEC))
    print("{}   届かないとき: 送信前から {:.1f}° 未満しか動いていなければ "
          "{:.1f}s 待って**同じ目標へ 1 回だけ**再送（本線と同じ）。"
          "それでも届かなければ ABORT（HOME へ自動で戻らない）"
          .format(LOG_PREFIX, RETRY_NO_MOVE_THRESHOLD_DEG, RETRY_PAUSE_SEC))
    print("{}   実測 j4: [{:+.1f}, {:+.1f}] の外は WARN / "
          "[{:+.1f}, {:+.1f}] の外は ABORT".format(
              LOG_PREFIX, J4_SAFE_MIN, J4_SAFE_MAX, J4_HARD_MIN, J4_HARD_MAX))
    print("=" * 78)
    return summary


# ---- recorder 通知 ----------------------------------------------------------
class _Recorder(object):
    """recorder.py への fire-and-forget 通知. --no-record なら何も送らない."""

    def __init__(self, enabled, run_stem):
        self.enabled = bool(enabled)
        self.run_stem = run_stem
        self.actual_sent = 0
        self.send_failures = 0
        self.last_send_error = None
        self.currents = None   # 依頼 121: policy_recorder.CurrentsRecorder（記録するときだけ）
        # 依頼 202: 電流の記録スレッドが読んだ角度も actual 行にする。states.csv の行が時刻順に並ぶよう、
        # start_current_actuals() から close_current_actuals() までは stage / actual / gripper_cmd の通知を
        # 鍵の中で 1 本の列に積み（鍵の中では送らない）、送り手のスレッドが積んだ順に送る
        # （codex R1 必須: 送信が遅れても本体と電流の記録スレッドは待たない）。
        self._state_lock = threading.Lock()
        self._state_cv = threading.Condition(self._state_lock)
        self._last_state_ts = None
        self._queue = None                        # 積んだ通知（列を使っている間だけ list）
        self._sender = None
        self._sender_stop = False
        self._current_actuals_open = False        # start_current_actuals() で開き、trial_end の前に閉じる
        self.actual_sent_current_thread = 0       # 電流の記録スレッドの角度で積んだ actual 行
        self.actual_current_thread_late = 0       # 先に積んだ行より前の時刻だったので積まなかった数
        self.actual_current_thread_errors = 0     # 角度・時刻が数にならず積まなかった数
        self.state_ts_backwards = 0               # 本体の行の時刻が前の行より前だった数（時計が戻ったとき. codex R2）
        self.current_actuals_unsent = 0           # close の期限の時点でまだ送り終えていなかった通知の数（途中の 1 件を含む）
        self.current_actuals_flush_timeout = False
        self.state_rows_sent_by_sender = 0        # 送り手のスレッドが送り終えた通知の数（試行 JSON は trial_end の直後の値）
        self.trial_end_via_sender = False         # 送り手が止まっていたので trial_end も列の最後に積んだ
        self._inflight = 0                        # 送り手が送っている途中の通知の数（0 か 1）
        self._sender_exited = False               # 送り手が列を空にして終わった（鍵の中で立てる）

    def start_current_actuals(self):
        """依頼 202: 電流の記録スレッドの角度を受け付け、通知を送り手のスレッドから送る形にする."""
        with self._state_lock:
            if self._sender is not None:
                return
            self._queue = []
            self._sender_stop = False
            self._sender_exited = False
            self._current_actuals_open = True
            self._sender = threading.Thread(target=self._send_loop, args=(self._queue,), daemon=True,
                                            name="record_state_sender")
        self._sender.start()

    def _send_loop(self, q):
        """積んだ通知を 1 件ずつ取り出して送る（close の期限を過ぎても捨てずに、trial_end まで積んだ順に送り切る）."""
        while True:
            with self._state_cv:
                while not q and not self._sender_stop:
                    self._state_cv.wait()
                if not q:
                    self._sender_exited = True
                    return
                p = q.pop(0)
                self._inflight = 1
            self._send(p)        # 鍵の外で送る（_send は例外を外へ出さない）
            with self._state_cv:
                self._inflight = 0
                self.state_rows_sent_by_sender += 1

    def _send_state(self, payload, state_row=True):
        """依頼 202: states.csv の行になる通知（stage / actual / gripper_cmd）.

        ts が無ければ鍵の中で今の時刻を入れる（rpp.recorder_notify が入れるのと同じ時刻）。
        列を使っている間は鍵の中で積むだけ（recorder.py は届いた順に書くので、積んだ順 = states.csv の順）。
        列を使っていなければ今までどおりその場で送る。
        """
        with self._state_cv:
            if state_row:
                if "ts" not in payload:
                    payload = dict(payload)
                    payload["ts"] = time.time()
                ts = float(payload["ts"])
                if self._last_state_ts is not None and ts < self._last_state_ts:
                    self.state_ts_backwards += 1      # 直さずに数えるだけ（codex R2）
                else:
                    self._last_state_ts = ts
            if self._queue is not None and not self._sender_exited:
                self._queue.append(payload)
                if not state_row:
                    self.trial_end_via_sender = True
                self._state_cv.notify()
                return
        self._send(payload)

    def _send(self, payload):
        """通知は fire-and-forget。**例外を外に出さない**（codex U1）.

        2026-10-19 方針変更: 止めるのは「送り直しても全く動かない」ときだけ。
        recorder への通知（stage / actual / gripper_cmd / trial_start / trial_end）
        が失敗しても再生を止めない。回数と最後のエラーを数えて試行 JSON に残す。
        KeyboardInterrupt / SystemExit は握らない（人の中断はそのまま通す）。
        """
        if not self.enabled:
            return
        try:
            rpp.recorder_notify(payload)
        except (KeyboardInterrupt, SystemExit):
            raise
        except BaseException as e:
            self.send_failures += 1
            self.last_send_error = "{}: {}".format(type(e).__name__, e)
            if self.send_failures <= NONFINITE_WARN_MAX:
                _warn("recorder への通知で例外（記録のみ・再生は続けます）: {}{}"
                      .format(self.last_send_error,
                              "（以降この警告は出しません）"
                              if self.send_failures == NONFINITE_WARN_MAX else ""))

    def trial_start(self):
        self._send({"type": "trial_start", "run_stem": self.run_stem})

    def trial_end(self):
        # 依頼 202: 送り手がまだ送り終えていなければ（close の期限切れ）、trial_end も列の最後に積む。
        # 送り手が積んだ順に送るので、states.csv の行が trial_end を追い越さない（本体は待たない）。
        self._send_state({"type": "trial_end", "run_stem": self.run_stem}, state_row=False)

    def stage(self, name):
        self._send_state({"type": "stage", "stage": name})

    def gripper_cmd(self, state, note, ts=None):
        """build_demo_episodes.py は gripper_cmd の gripper_state を 0/1 で読む.

        ts を渡すとその時刻で記録される（A4-2: 指令が daemon から戻った時刻）。
        open を 2 回送る場合も **1 回目が戻った時刻**で固定する
        （gv を使わないので「どちらの回で実際に開いたか」は記録に無い。
        codex R10 S3 / R10b T3: 1 回目が無視されて 2 回目で開いた試行では、
        ラベルの時刻が「daemon の事前 sleep 2.0 + 事後相当 2.0 + 待ち 2.0
        ≒ 6 秒 + 通信時間」ぶん早くなりうる。動画と attempt_log で確かめる）。
        省略時は rpp.recorder_notify が送信時刻を入れる。
        """
        payload = {"type": "gripper_cmd", "state": int(state), "note": str(note)}
        if ts is not None:
            payload["ts"] = float(ts)
        self._send_state(payload)

    def currents_window(self, sec):
        """依頼 121: これから sec 秒 daemon を使わない（眠る）と電流の記録に知らせる."""
        if self.currents is None:
            return
        try:
            self.currents.window(sec)
        except Exception:
            pass

    def pause_currents(self):
        """依頼 121: 爪の命令を送る前に電流の読み取りを止める（読んでいる途中ならその 1 回を待つ）.

        「爪を動かした直後にシリアルを触らない」（2026-10-19）を電流の記録でも守る。
        記録の失敗で再生を止めない（例外は外へ出さない）。
        """
        if self.currents is None:
            return
        try:
            self.currents.pause()
        except Exception as e:
            _warn("電流の記録を止められませんでした（再生は続けます）: {}: {}".format(
                type(e).__name__, e))

    def resume_currents(self):
        """依頼 121: 爪の待ちが終わったら電流の読み取りを再開する."""
        if self.currents is None:
            return
        try:
            self.currents.resume()
        except Exception as e:
            _warn("電流の記録を再開できませんでした（再生は続けます）: {}: {}".format(
                type(e).__name__, e))

    def actual(self, angles):
        self._send_state({"type": "actual",
                          "angles": [float(x) for x in angles]})
        self.actual_sent += 1

    def actual_from_currents(self, angles, ts):
        """依頼 202: 電流の記録スレッド（read_servo）が読んだ角度を actual 行として送る.

        時刻 ts は角度を読んだ時刻（currents.csv の ts と同じ）。本体の読み（到達・静止の確かめなど。
        note は空のまま）と区別するため note に CURRENT_THREAD_NOTE を入れる。
        先に積んだ行（本体の actual・stage・gripper_cmd）より前の時刻なら積まずに数える
        （states.csv の行を時刻順に保つため。電流スレッドは本体が眠っている間だけ読むので、ふつうは起きない）。
        電流スレッドから呼ばれる。鍵の中では積むだけで送らない（送るのは送り手のスレッド）。例外は外へ出さない。
        start_current_actuals() の前と close_current_actuals() のあとは何もしない。
        """
        try:
            vals = [float(x) for x in angles]
            ts = float(ts)
            if len(vals) != 6 or not all(math.isfinite(v) for v in vals) or not math.isfinite(ts):
                raise ValueError("bad angles")
            with self._state_cv:
                if not self._current_actuals_open or self._queue is None:
                    return
                if self._last_state_ts is not None and ts < self._last_state_ts:
                    self.actual_current_thread_late += 1
                    return
                self._queue.append({"type": "actual", "angles": vals, "ts": ts,
                                    "note": CURRENT_THREAD_NOTE})
                self._last_state_ts = ts
                self.actual_sent_current_thread += 1
                self._state_cv.notify()
        except Exception:
            self.actual_current_thread_errors += 1   # 壊れた値は積まずに数える

    def close_current_actuals(self, timeout=None):
        """依頼 202: 電流の記録スレッドの角度を受け付けるのをやめ、積んだ通知を送り切る（trial_end の前に呼ぶ）.

        送り手のスレッドを待つのは timeout 秒（既定 CURRENT_ACTUALS_FLUSH_SEC）まで。送り切れたら、このあとの
        通知は今までどおりその場で送る。送り切れなければ、まだの数を current_actuals_unsent に残し、
        積んだ分とこのあとの通知（trial_end）は送り手が順に送る（本体は待たない。追い越さない）。
        """
        timeout = CURRENT_ACTUALS_FLUSH_SEC if timeout is None else timeout
        with self._state_cv:
            self._current_actuals_open = False
            sender = self._sender
            if sender is None:
                return
            self._sender_stop = True
            self._state_cv.notify()
        sender.join(timeout=timeout)
        with self._state_cv:
            if not self._sender_exited:
                # 期限切れ: 積んだ分は捨てずに送り手に任せる（このあとの trial_end も列の最後に積む）
                self.current_actuals_flush_timeout = True
                self.current_actuals_unsent = len(self._queue or []) + self._inflight
            else:
                self._queue = None


_STAGE_SAFE_RE = re.compile(r"[^A-Z0-9]+")


def _stage_name(to_name):
    """'APPROACH_PICK (lift)' → 'APPROACH_PICK_LIFT'（states.csv の stage 列用）."""
    return _STAGE_SAFE_RE.sub("_", to_name.upper()).strip("_")


_ACTIVE_CURRENTS = []   # 依頼 121: 始めた電流の記録（cmd_run の最後に必ず止める）


def _sleep_open(recorder, sec):
    """time.sleep(sec) と同じ（**待つ長さは変えない**）。依頼 121: 眠る前に、この間は daemon を
    使わないと電流の記録に知らせる。電流はこの間だけ読むので、本体の daemon とのやりとりは
    電流の記録を待たない（到達・静止の確かめの回数と時刻が変わらない。codex R1・R2 必須）。
    """
    recorder.currents_window(sec)
    time.sleep(sec)


def _halt_currents():
    """依頼 121: 始めた電流の記録をすべて止める（socket を閉じる前に呼ぶ）.

    stop() が途中で中断されていても、止める印を立ててスレッドの終わりを待つ。
    1 本あたり待つのは最大 STOP_JOIN_SEC + READ_SERVO_TIMEOUT_SEC。待っている間の Ctrl-C は
    1 回だけ受け流してもう一度同じだけ待つ（2 回目の Ctrl-C はそのまま通す）。
    スレッドが終わったと分かるまで _ACTIVE_CURRENTS から外さない（codex R2 推奨）。
    """
    for cur in list(_ACTIVE_CURRENTS):
        cur.stop_flag = True
        bound = _prec.STOP_JOIN_SEC + _prec.READ_SERVO_TIMEOUT_SEC
        try:
            cur.t.join(timeout=bound)
        except KeyboardInterrupt:
            _warn("電流の記録スレッドの終わりを待っています（もう一度 Ctrl-C で待たずに進みます）")
            cur.t.join(timeout=bound)
        try:
            if getattr(cur, "meta", None) is None:
                cur.stop()
        except Exception as e:
            _warn("電流の記録を閉じるときに例外（続行）: {}: {}".format(type(e).__name__, e))
        if cur.t.is_alive():
            _warn("電流の記録スレッドが止まりません（socket を閉じて終えます）")
        else:
            _ACTIVE_CURRENTS.remove(cur)


def _start_currents(run_dir, power=None, angle_sink=None):
    """依頼 121: 爪と肩の電流の記録スレッドを始める（失敗しても再生は止めない → None）.

    読み方・列・頻度は実機推論の記録（依頼 116）と同じ部品（policy_recorder.CurrentsRecorder）。
    daemon のソケットを本体と共用するので、先に install_daemon_lock() で
    1 回のやりとりごとに鍵をかける（送る中身・タイムアウトは変えない）。
    states.csv（recorder.py が書く）の列と行は変えない。
    依頼 156: power（省略時は cmd_run が --power から入れた POWER。2A など）があれば、爪の位置と電圧の番地も読む
    （policy_recorder.PowerRegsRecorder → recording/power_regs.csv。読み始めてよい時の決め方は同じ）。
    依頼 202: angle_sink（_Recorder.actual_from_currents）を渡すと、read_servo で読めた角度を
    states.csv の actual 行（note = CURRENT_THREAD_NOTE）にも送る。読む命令・読む時機は変えない。
    """
    power = POWER if power is None else power
    try:
        _prec.install_daemon_lock()
        cls = _prec.PowerRegsRecorder if power else _prec.CurrentsRecorder
        cur = cls(run_dir, _prec.daemon_read_servo)
        cur.angle_sink = angle_sink
        _ACTIVE_CURRENTS.append(cur)
        cur.start()
    except Exception as e:
        _warn("電流の記録を始められませんでした（再生と映像の記録はそのまま続けます）: "
              "{}: {}".format(type(e).__name__, e))
        return None
    _log("爪と肩の電流を 1 秒 {:.0f} 回で記録します（爪を動かしている間は読みません）→ {}".format(
        _prec.RECORD_FPS, cur.dir / "currents.csv"))
    if power:
        _log("電源 {}: 爪の位置（番地 7:56/57）と電圧（2:62・7:62）も読みます（生の値・未確認の番地）→ {}".format(
            power, cur.dir / "power_regs.csv"))
    return cur


# ---- 実測角の読み取り --------------------------------------------------------
class _ActualSampler(object):
    """実測 6 軸を読んで recorder へ流す. 制御判断には使わない（観測）."""

    def __init__(self, recorder):
        self.recorder = recorder
        self.attempted = 0
        self.ok = 0
        self.last = None
        self.t_first = None
        self.t_last = None
        # codex R1 C1 / C2: 観測の異常は制御を止めずに数えて試行 JSON に残す
        self.attempted_once = 0   # 記録用（get_angles_once）の試行数
        self.ok_once = 0          # 同・成功数（1 回で読める率の実測）
        self.nonfinite_reads = 0
        self.j4_out_of_range = []        # 値のサンプル（上限あり）
        self.j4_out_of_range_count = 0    # 総数
        self.j4_hard_violation = None

    def read(self, once=True, timeout_override=None):
        """1 回読む. 成功時 6 要素 list、失敗時 None.

        once=True（既定・**記録用**）: daemon cmd `get_angles_once` を使う。
          daemon 側でリトライしないので、読めないときも最悪 1 回分で返る。
          既存 get_angles は 12 回リトライ（最悪 ≈3.7s）で、0.1s 周期の
          ストリーミング中に使うと送信まで止まる（2026-10-19 実測）。
        once=False（**区間終端の到達確認**）: 既存 get_angles（リトライ有り）を使う。
          アームが止まりかけているので読めるはずで、確実に読めた方が良い。

        codex R1 C1: 非有限値（NaN / inf）が 1 つでも混じる読み取りは **失敗扱い**。
        NaN は比較が全部 False になるため、そのまま到達判定に渡すと
        未到達を「到達」と誤判定する（max() が NaN を飛ばして 0.0 を返す）。
        """
        self.attempted += 1
        if once:
            self.attempted_once += 1
        try:
            if once and USE_FAST_DAEMON_CMDS:
                _to = (DAEMON_GET_ONCE_TIMEOUT_SEC if timeout_override is None
                       else min(DAEMON_GET_ONCE_TIMEOUT_SEC,
                                float(timeout_override)))
                a = daemon_get_angles_once(timeout_override=_to)
            else:
                # codex U2: 呼び出し側が残り時間を渡したら、それ以上は待たない
                _to = (DAEMON_GET_TIMEOUT_SEC if timeout_override is None
                       else min(DAEMON_GET_TIMEOUT_SEC, float(timeout_override)))
                a = daemon_get_angles(timeout_override=_to)
        except Exception as e:
            _warn("角度読み取りで例外（観測のみ・制御は続行）: {}: {}".format(
                type(e).__name__, e))
            return None
        if not _is_valid_angles(a):
            if a is not None and not _is_finite_6(a):
                self.nonfinite_reads += 1
                # 到達確認のポーリング中に連発するとログが埋まるので回数を絞る
                # （総数は actual_sampling.nonfinite_reads に必ず残る）。
                if self.nonfinite_reads <= NONFINITE_WARN_MAX:
                    _warn("実測角に有限でない値が入っていたので、この読み取りは失敗扱いに"
                          "します（到達とみなしません）: {!r}{}".format(
                              a, "（以降この警告は出しません）"
                                 if self.nonfinite_reads == NONFINITE_WARN_MAX else ""))
            return None
        vals = [float(x) for x in a]
        self.ok += 1
        if once:
            self.ok_once += 1
        self.last = vals
        # 実測 j4 が安全範囲外に読めても ABORT はしない。
        # 2026-10-19（方針変更・項目3）: **ハード帯（±98 / +68）も WARN だけ**にした。
        # 止めるのは「送り直しても全く動かない」ときだけという方針に合わせる。
        # 逸脱は数えて試行 JSON に残す（j4_hard_violation / j4_out_of_range）。
        j4 = vals[4]
        if j4 < J4_HARD_MIN or j4 > J4_HARD_MAX:
            if self.j4_hard_violation is None:
                self.j4_hard_violation = round(j4, 3)
                _warn("実測 j4 = {:+.2f} が [{:+.1f}, {:+.1f}] の外です。"
                      "**止めません**（WARN のみ・試行 JSON に残します）"
                      "（実測角 = {}）".format(
                          j4, J4_HARD_MIN, J4_HARD_MAX, _fmt_angles(vals)))
            self.j4_hard_violation_count = getattr(
                self, "j4_hard_violation_count", 0) + 1
        if j4 < J4_SAFE_MIN or j4 > J4_SAFE_MAX:
            self.j4_out_of_range_count += 1
            if len(self.j4_out_of_range) < J4_SAMPLE_MAX:
                self.j4_out_of_range.append(round(j4, 3))
            if self.j4_out_of_range_count <= NONFINITE_WARN_MAX:
                _warn("実測 j4 = {:+.2f} が J4_SAFE [{:+.1f}, {:+.1f}] の外です"
                      "（記録のみ・送信は止めません）{}".format(
                          j4, J4_SAFE_MIN, J4_SAFE_MAX,
                          "（以降この警告は出しません）"
                          if self.j4_out_of_range_count == NONFINITE_WARN_MAX else ""))
        now = time.time()
        if self.t_first is None:
            self.t_first = now
        self.t_last = now
        self.recorder.actual(vals)
        return vals

    def sleep_with_reads(self, duration_sec):
        """duration_sec 待つ間も実測角を読む.

        **実際に time.sleep した合計が duration_sec を下回らない**ように数える
        （codex R1 A7/D7）。読み取りにかかった時間は差し引かず、待ち時間の上に乗る。
        壁時計の経過が duration_sec より長くなるのは可（規約「time.sleep を
        削除・短縮しない」に合わせるため、短くなる方向だけを禁じる）。

        settle / gripper dwell の間にも state を残すため、爪の開閉前後が
        学習データで欠けないようにする。
        """
        if duration_sec <= 0:
            return
        remaining_sleep = float(duration_sec)
        # 読み取り周期の管理だけ monotonic を使う（時計の巻き戻しに影響されないため）。
        next_read_at = time.monotonic()
        while remaining_sleep > 0:
            if time.monotonic() >= next_read_at:
                self.read()   # ここでかかった時間は remaining_sleep から引かない
                next_read_at = time.monotonic() + (1.0 / ACTUAL_READ_TARGET_HZ)
                continue
            slice_sec = min(SLEEP_SLICE_SEC, remaining_sleep)
            # 依頼 121: 次に読むまで（か待ちの終わりまで）daemon を使わないと電流の記録に知らせる
            self.recorder.currents_window(
                min(remaining_sleep, max(0.0, next_read_at - time.monotonic())))
            time.sleep(slice_sec)
            remaining_sleep -= slice_sec

    def summary(self, motion_elapsed_sec):
        span = None
        if self.t_first is not None and self.t_last is not None:
            span = self.t_last - self.t_first
        hz = (self.ok / span) if (span and span > 0) else None
        return {
            "reads_attempted": self.attempted,
            "reads_ok": self.ok,
            "read_span_sec": (round(span, 3) if span is not None else None),
            "hz": (round(hz, 3) if hz is not None else None),
            "target_hz": ACTUAL_READ_TARGET_HZ,
            "read_every_n_ticks": ACTUAL_READ_EVERY_N_TICKS,
            "read_timeout_sec": DAEMON_GET_TIMEOUT_SEC,
            # LOG352: 記録用の読み取り（get_angles_once）の実測
            "once_cmd_used": bool(USE_FAST_DAEMON_CMDS),
            "reads_once_attempted": self.attempted_once,
            "reads_once_ok": self.ok_once,
            "reads_once_hit_rate": (round(self.ok_once / self.attempted_once, 3)
                                    if self.attempted_once else None),
            "motion_elapsed_sec": round(motion_elapsed_sec, 3),
            # codex R1 C1: 非有限値で失敗扱いにした読み取り回数
            "nonfinite_reads": self.nonfinite_reads,
            # codex R1 C2: 実測 j4 が J4_SAFE 外に読めた値（ABORT はしない・記録のみ）
            "j4_out_of_range_count": self.j4_out_of_range_count,
            "j4_out_of_range_values": self.j4_out_of_range[:20],
            # 実測 j4 の扱い（エイイチ判断）: J4_SAFE 外は WARN、
            # J4_SAFE ± J4_HARD_MARGIN_DEG の外は ABORT
            "j4_warn_band": [J4_SAFE_MIN, J4_SAFE_MAX],
            "j4_abort_band": [J4_HARD_MIN, J4_HARD_MAX],
            "j4_hard_violation": self.j4_hard_violation,
        }


# ---- 所要時間の統計（A4-3）--------------------------------------------------
def _stats(values, label):
    """秒のリストから n / mean / max / p50 / p99 を出す.

    A4-3（エイイチ判断 2026-10-19）: Pi の daemon は send_angles のたびに
    角度を読み直すので、0.1s 周期が守れず Pi 直より遅くなる可能性がある。
    それを数字で見るために、1 tick の send_angles 往復時間と tick 全体の周期を
    別々に残す。p99 は nearest-rank（並べて ceil(0.99*n) 番目）。
    """
    # codex R3 G2: 失敗 tick は None が入る。統計は成功分だけで出す
    # （失敗は timeout 上限に張り付くので混ぜると平均が歪む）。
    vals = [v for v in (values or []) if _is_finite_num(v)]
    if not vals:
        return {"label": label, "n": 0, "n_missing": len(values or []),
                "mean_sec": None, "max_sec": None,
                "p50_sec": None, "p99_sec": None}
    vs = sorted(float(v) for v in vals)
    n = len(vs)

    def _pick(q):
        idx = int(math.ceil(q * n)) - 1
        return vs[max(0, min(n - 1, idx))]

    return {
        "label": label,
        "n": n,
        "n_missing": len(values or []) - n,
        "mean_sec": round(sum(vs) / n, 4),
        "max_sec": round(vs[-1], 4),
        "p50_sec": round(_pick(0.50), 4),
        "p99_sec": round(_pick(0.99), 4),
    }


# ---- 到達確認 ---------------------------------------------------------------
def _hold_until_arrival(target, label, sampler, tol_deg, timeout_sec):
    """target を最後に送ったあと tol_deg 内に入るまでポーリングする.

    2026-10-19（項目1）: 許容差とタイムアウトを **区間ごとに引数で受ける**。
    許容差は本線 run_pick_place.py の stage 値、タイムアウトは本線と同じ
    wr._compute_arrival_timeout の算出。ポーリング間隔は jss.POLL_SLEEP = 0.3。
    読んだ実測角は recorder にも流す（sampler 経由）。
    戻り値: (arrived, last_max_d, last_actual)
    """
    t0 = time.time()
    last_max = None
    last_actual = None
    while time.time() - t0 < timeout_sec:
        # 到達確認はリトライ有りの既存 get_angles を使う（once=False）。
        # アームが止まりかけなので読めるはずで、確実に読めた方が良い。
        actual = sampler.read(once=False)   # 非有限を含む読み取りは None（C1）
        if actual is None:
            _sleep_open(sampler.recorder, ARRIVAL_POLL_SLEEP)
            continue
        last_actual = actual
        # codex R1 C1: wr._max_axis_diff は NaN を飛ばして偽の 0.0 を返しうるので
        # 有限値を保証する版を使う。None（計算不能）は「到達とみなさない」。
        max_d = _max_axis_diff_finite(actual, target)
        if max_d is None:
            _warn("到達差を有限な数値として計算できませんでした（到達とみなしません）: "
                  "actual={!r} target={!r}".format(actual, target))
            _sleep_open(sampler.recorder, ARRIVAL_POLL_SLEEP)
            continue
        last_max = max_d
        if max_d <= tol_deg:
            _log("arrival OK ({}, max|Δ|={:.2f}° <= tol {:.1f}°)".format(
                label, max_d, tol_deg))
            return True, max_d, last_actual
        _sleep_open(sampler.recorder, ARRIVAL_POLL_SLEEP)
    _warn("arrival timeout ({}, max|Δ|={}° > tol {:.1f}°, {:.1f}s)".format(
        label,
        ("{:.2f}".format(last_max) if last_max is not None else "None"),
        tol_deg, timeout_sec))
    return False, last_max, last_actual


def _wait_until_still(sampler, target, tol_deg, label):
    """爪を動かす前に、アームが止まっていることを確認する（codex R11 A1）.

    成立条件（両方）:
      (a) target から tol_deg 内にいる（target が None なら (a) は省略）
      (b) 連続する実測の差が GRIPPER_STILL_TOL_DEG 以下

    これを GRIPPER_STILL_READS 回続けて満たしたら True。
    読めない回・有限でない回はカウントをリセットする（成立とみなさない）。
    期限（GRIPPER_STILL_TIMEOUT_SEC）内に満たせなければ False。
    **この関数はアームへ何も送らない**（読み取りだけ）。
    戻り値: (still, detail dict)
    """
    # codex R11b T3: 期限は monotonic で見て、**読み取りの後にも**確認する
    # （読み取り自体に時間がかかるので、前だけ見ると期限を超えて成功しうる）。
    t0 = time.monotonic()
    prev = None
    run = 0
    reads = 0
    reads_ok = 0
    worst_step = None      # 連続差の最大（記録用）
    last_to_target = None
    timed_out = False
    while True:
        if time.monotonic() - t0 >= GRIPPER_STILL_TIMEOUT_SEC:
            timed_out = True
            break
        # codex U2: 読み取りにも残り時間を渡して、8 秒を本当の上限にする
        remaining = GRIPPER_STILL_TIMEOUT_SEC - (time.monotonic() - t0)
        if remaining <= 0.05:
            timed_out = True
            break
        reads += 1
        actual = sampler.read(once=False, timeout_override=remaining)
        if time.monotonic() - t0 >= GRIPPER_STILL_TIMEOUT_SEC:
            # 読み取り中に期限を超えた。この回の値では成立させない
            timed_out = True
            break
        if actual is None:
            prev = None
            run = 0
            _sleep_open(sampler.recorder, ARRIVAL_POLL_SLEEP)
            continue
        reads_ok += 1
        # 項目2: target が None なら「止まっているか」だけを見る
        ok_target = True
        if target is not None and tol_deg is not None:
            d = _max_axis_diff_finite(actual, target)
            last_to_target = d
            ok_target = (d is not None and d <= tol_deg)
        step = _max_axis_diff_finite(actual, prev) if prev is not None else None
        if step is not None and (worst_step is None or step > worst_step):
            worst_step = step
        ok_still = (step is not None and step <= GRIPPER_STILL_TOL_DEG)
        prev = actual
        if ok_target and ok_still:
            run += 1
            if run >= GRIPPER_STILL_READS:
                detail = {
                    "still": True, "reads": reads, "reads_ok": reads_ok,
                    "consecutive_ok": run,
                    "elapsed_sec": round(time.monotonic() - t0, 3),
                    "max_step_deg": (round(worst_step, 3)
                                     if worst_step is not None else None),
                    "to_target_deg": (round(last_to_target, 3)
                                      if last_to_target is not None else None),
                    "tol_deg": tol_deg,
                    "still_tol_deg": GRIPPER_STILL_TOL_DEG,
                    "required_reads": GRIPPER_STILL_READS,
                }
                _log("静止確認 OK（{}: 連続 {} 回・連続差 最大 {}° <= {:.1f}°"
                     "）".format(
                         label, run,
                         ("{:.2f}".format(worst_step)
                          if worst_step is not None else "None"),
                         GRIPPER_STILL_TOL_DEG))
                return True, detail
        else:
            run = 0
        # 残り時間を超えて眠らない（8 秒を本当の上限にする）
        _left = GRIPPER_STILL_TIMEOUT_SEC - (time.monotonic() - t0)
        if _left <= 0:
            timed_out = True
            break
        _sleep_open(sampler.recorder, min(ARRIVAL_POLL_SLEEP, _left))

    detail = {
        "still": False, "reads": reads, "reads_ok": reads_ok,
        "consecutive_ok": run,
        "timed_out": bool(timed_out),
        "elapsed_sec": round(time.monotonic() - t0, 3),
        "max_step_deg": (round(worst_step, 3) if worst_step is not None else None),
        "to_target_deg": (round(last_to_target, 3)
                          if last_to_target is not None else None),
        "tol_deg": tol_deg,
        "still_tol_deg": GRIPPER_STILL_TOL_DEG,
        "required_reads": GRIPPER_STILL_READS,
        "timeout_sec": GRIPPER_STILL_TIMEOUT_SEC,
    }
    return False, detail


# ---- 再生本体 ---------------------------------------------------------------
class _Aborted(Exception):
    def __init__(self, stage, reason):
        Exception.__init__(self, "{}: {}".format(stage, reason))
        self.stage = stage
        self.reason = reason


# ---- LOG352: daemon の新 cmd のラッパ ---------------------------------------
def daemon_send_angles_fast(angles, speed, timeout_override=None):
    """送信後に角度を読み直さない send_angles（daemon cmd: send_angles_fast）.

    visual_servo_pick._daemon_request を借りるので、既存ヘルパと同じ 1 本の
    TCP 接続を共有する（socket を二重に張らない）。
    """
    return _daemon_request({
        "cmd": "send_angles_fast",
        "angles": list(angles),
        "speed": int(speed),
    }, timeout_override=timeout_override)


def daemon_get_angles_once(timeout_override=DAEMON_GET_ONCE_TIMEOUT_SEC):
    """1 回だけ角度を読む（daemon cmd: get_angles_once）. 読めなければ None.

    daemon 側はリトライしないので、最悪でも _mc.get_angles() 1 回分で返る。
    「読めない」は異常ではないので daemon は ok=True / angles=None を返す。
    """
    try:
        resp = _daemon_request({"cmd": "get_angles_once"},
                               timeout_override=timeout_override)
    except RuntimeError as e:
        print("  [WARN] daemon_get_angles_once failed: {}".format(e))
        return None
    a = resp.get("angles")
    if not a or len(a) != 6:
        return None
    return a


def _require_fast_cmds():
    """Pi の daemon が新 cmd を持っているかを、アームを動かさずに確かめる.

    send_angles_fast には **わざと長さ不正の angles** を渡す。
    新 daemon は "angles must be length 6" を返し（_mc には触らない）、
    旧 daemon は "unknown cmd: send_angles_fast" を返すので区別できる。
    未配備なら 1 コマンドも送らずに ABORT する（157 tick ぶん失敗し続けさせない）。
    """
    try:
        _daemon_request({"cmd": "send_angles_fast", "angles": [0, 0, 0],
                         "speed": 20},
                        timeout_override=DAEMON_SEND_TIMEOUT_SEC)
        _abort_msg("daemon が長さ不正の angles を受け付けました。"
                   "送信ガードが期待どおりでないので止めます")
        sys.exit(1)
    except RuntimeError as e:
        msg = str(e)
        if "unknown cmd" in msg:
            _abort_msg("Pi の daemon が send_angles_fast を持っていません。"
                       "新しい mecharm_daemon.py を Pi に配備してから実行してください"
                       "（応答: {}）".format(msg))
            sys.exit(1)
        if "length 6" not in msg:
            _abort_msg("daemon の send_angles_fast の応答が想定外です: {}".format(msg))
            sys.exit(1)
    # get_angles_once も確認（こちらは副作用が無いのでそのまま呼ぶ）
    try:
        _daemon_request({"cmd": "get_angles_once"},
                        timeout_override=DAEMON_GET_ONCE_TIMEOUT_SEC)
    except RuntimeError as e:
        if "unknown cmd" in str(e):
            _abort_msg("Pi の daemon が get_angles_once を持っていません。"
                       "新しい mecharm_daemon.py を Pi に配備してください"
                       "（応答: {}）".format(e))
            sys.exit(1)
        _warn("get_angles_once の確認で例外（記録の読み取りだけ影響）: {}".format(e))
    _log("daemon の新 cmd 確認 OK（send_angles_fast / get_angles_once）")


def _send_angles_guarded(pose, speed, where, recorder=None):
    """送信直前の backstop（codex R1 C2）.

    送る姿勢ごとに「有限値 → JOINT_LIMITS → J4_SAFE」を確かめ、
    外れていたら **送らずに** ABORT する。クランプして黙って姿勢を変えることはしない
    （教示姿勢を勝手に書き換えない）。
    計画時に全姿勢を検査済みでも、ここが最後の砦として全送信を通る。
    依頼 213: SEND_WAIT_ON が "mac" なら、送ったあとの SEND_WAIT_SEC をここ（Mac 側）で待ってから返る。
    recorder を渡すとその間を電流の記録スレッドに知らせる（読める）。
    """
    reason = _pose_violation(pose)
    if reason is not None:
        raise _Aborted(where, "送信直前の姿勢検査で弾きました（送信していません）: "
                              + reason)
    _LAST_SEND["daemon_rtt_sec"] = None
    if USE_FAST_DAEMON_CMDS:
        # LOG352: 読み直し無しの送信（Pi 直と同じ送り方）
        return daemon_send_angles_fast(
            pose, speed, timeout_override=DAEMON_SEND_TIMEOUT_SEC)
    # 項目3 / D7: 本線 j0_sweep_scan._send_and_wait_arrival は
    #   daemon_send_angles(target, speed=SPEED, wait=SEND_WAIT_SEC)
    # で送る（timeout_override は渡さない）。テンプレートの固定事項
    # 「SENDWAIT = 1.0（DAY266: 短縮しない）」にも合う。
    #
    # codex R11 B5: timeout_override は渡さない。visual_servo_pick._daemon_request は
    #   timeout = (wait + 3.0) if wait > 0 else DAEMON_TIMEOUT_SEC
    #   deadline = min(timeout, timeout_override)
    # なので、wait=1.0 のとき override を渡しても **4.0s より長くはできない**
    #（override は短縮しかできない）。渡すと「6.0s にした」という意図と
    # 実際（4.0s）が食い違うだけなので渡さず、**本線と同じ 4.0s** にする。
    # daemon の最悪応答は wait 1.0 + 読み直し 12 回 ≒3.7s + 通信時間なので、
    # 4.0s を超えることがある。超えるとソケットが切れて例外 → その場で ABORT。
    # codex R11b T6: この ABORT は **後続の送信と爪の操作をやめる**だけで、
    # **実機の停止ではない**。daemon は応答の前に関節指令を送り終えているので、
    # アームは目標まで動き続けうる。止めるのは電源を切ることだけ
    #（2026-10-18 からの運用）。周囲の空間を空けて監視下で走らせること。
    # 送信回数は増やさない。待つのは daemon 側の 1 回だけ。
    if SEND_WAIT_ON == "daemon":
        t0 = time.monotonic()
        try:
            return daemon_send_angles(pose, speed, wait=SEND_WAIT_SEC)
        finally:
            _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0   # 例外でも残す（codex 2 回目 R2）
    # 依頼 213（"mac"）: daemon は wait=0 で、書き込みのあと 1 回だけ角度を読んで（read_angles_once_for_log）
    # すぐ返す。返ってから Mac 側で同じ SEND_WAIT_SEC 待つので、到達の確かめが始まる時刻は今までと同じ
    # （書き込み → 1.0 秒 → 読み 1 回 → 返事、が 書き込み → 読み 1 回 → 返事 → 1.0 秒 になる）。
    # 送信は install_daemon_lock の鍵の中の 1 回のやりとりなので、電流の記録スレッドの読みは
    # 送信の途中に割り込まない（返事が来て鍵が外れてから、眠りを知らされた間だけ読む）。
    # 送信が例外でも 1.0 秒は待つ（届いたかは分からないので、待ちを縮めずに到達の確かめへ進む）。
    t0 = time.monotonic()
    try:
        resp = daemon_send_angles(pose, speed, wait=0,
                                  timeout_override=SEND_SOCKET_TIMEOUT_SEC)
    except Exception:
        _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0
        _send_wait_on_mac(recorder)
        raise
    _LAST_SEND["daemon_rtt_sec"] = time.monotonic() - t0
    _send_wait_on_mac(recorder)
    return resp


def _send_wait_on_mac(recorder):
    """依頼 213: 送ったあとの SEND_WAIT_SEC（1.0 秒）を Mac 側で待つ。**長さは縮めない**（time.sleep と同じ）."""
    if recorder is not None:
        _sleep_open(recorder, SEND_WAIT_SEC)
    else:
        time.sleep(SEND_WAIT_SEC)


def _note_send_failure(streak, label, detail, exc):
    """send_angles の失敗を数えて WARN する（**止めない**）.

    2026-10-19 方針変更: 送信の例外では ABORT しない。連続回数は記録のために
    数えるだけで、止めるかどうかは到達確認（送り直しても動かないか）で決める。
    戻り値は互換のため残しているが、呼び出し側は使っていない。
    成功時のリセットは呼び出し側で `streak["consecutive_send_failures"] = 0`。
    """
    streak["total_send_failures"] += 1
    streak["consecutive_send_failures"] += 1
    streak["max_consecutive_send_failures"] = max(
        streak["max_consecutive_send_failures"],
        streak["consecutive_send_failures"])
    _warn("send_angles 例外 {} ({}): {}: {}（連続 {} 回目・**止めません**）".format(
        detail, label, type(exc).__name__, exc,
        streak["consecutive_send_failures"]))
    return streak["consecutive_send_failures"] >= SEND_FAIL_ABORT_STREAK


def _replay(seq, recorder, log_record):
    """Pi 配備版 WPSR と同じ 11 step を daemon 経由で流す.

    Pi 配備版との対応:
      - 到達確認タイムアウトは **critical 区間（次が爪の開閉）だけ ABORT**。
        それ以外は WARN のみで続行（配備版 L359-365 と同じ）。
      - 単発の send 例外は WARN のみで続行（配備版 L350-352 と同じ）。
      - 2026-10-19 方針変更（エイイチ指示）: **止めるのは「送り直しても全く
        動かない」ときだけ**。送信の例外・到達タイムアウト・爪の失敗・
        実測 j4 の逸脱・静止確認の失敗は、すべて WARN にして先へ進む。
      - ABORT 後はアームへ追加コマンドを送らない（自動 HOME 復帰はしない）。
    """
    segments_log = []
    send_rtts = []        # 区間ごとの send_angles 1 回の daemon 往復時間
    sampler = _ActualSampler(recorder)
    t_motion_start = time.monotonic()
    # 連続 send 失敗数は区間をまたいで数える（通信断は区間の境目では切れない）
    streak = {"consecutive_send_failures": 0, "max_consecutive_send_failures": 0,
              "total_send_failures": 0}

    try:
        # ---- 項目2（2026-10-19・調べた結果として **残す**）--------------------
        # 「本線は set_fresh_mode を呼ばない」という前回の報告は**誤り**だった。
        # 確かめたこと:
        #   - run_pick_place.py L3976 で daemon_set_fresh_mode(1) を呼び、
        #     L5047 / L5059（finally）で (0) に戻している
        #     → 19 本 = 647 試行は **fresh_mode = 1 で動いている**
        #   - j0_sweep_scan.py L709 も (1)、L978 で (0)
        #   - Pi の daemon（無改変・5b7bacf1）は起動時に fresh_mode を設定しない。
        #     init_mycobot() は MyCobot(...) と角度読みだけ。終了時（SIGTERM /
        #     main の finally）にだけ _mc.set_fresh_mode(0) を送る。
        #     _fresh_mode_state = 0 は daemon 内の記録用の変数にすぎない
        #   - 実ログの裏づけ: Pi の daemon.log.pre_LOG352（本線を動かしていた頃）に
        #     set_fresh_mode(1) が 3 件、(0) が 5 件
        # → **削除すると本線と違う状態になる**ので、(1) と finally の (0) を
        #   そのまま残す（本線と同じ明示設定）。
        # codex U1: ここで例外が出ても止めない（WARN のみ）。
        # visual_servo_pick.daemon_set_fresh_mode は RuntimeError を握って
        # False を返すが、それ以外の例外は上がりうる。
        try:
            daemon_set_fresh_mode(1)
            _log("daemon_set_fresh_mode(1)（最新コマンド優先・本線 "
                 "run_pick_place.py L3976 と同じ。647 試行の実績はこの状態）")
        except (KeyboardInterrupt, SystemExit):
            raise
        except BaseException as e:
            _warn("daemon_set_fresh_mode(1) で例外（止めません）: {}: {}".format(
                type(e).__name__, e))
            log_record["fresh_mode_set_error"] = "{}: {}".format(
                type(e).__name__, e)

        # ---- STEP 1: HOME へ単発送信 + settle ----
        recorder.stage("HOME_START")
        print("[STEP  1] MOVE → HOME  daemon_send_angles(HOME, {})".format(SPEED_FIRST))
        # codex R8 O2/O3: 先頭 HOME も **送信失敗はその場で ABORT** し、
        # settle のあとに **到達確認**を行い、区間記録を 1 件残す。
        # 以前は送信失敗しても settle するだけで次へ進み、到達確認も無かったので、
        # どこにいるか分からないまま爪を開いて APPROACH_PICK へ送っていた。
        # cmd_run が測った current → HOME の max|Δ|（記録用）
        _first_max_delta = _max_axis_diff_finite(
            HOME_ANGLES, log_record.get("current_angles_before_replay"))
        # 項目1: 先頭 HOME の許容差は本線の TOL_HOME（表から引く）
        first_warnings = []
        first_tol, first_tol_src = _arrival_tol_for("HOME")
        if first_tol is None:
            first_tol = float(rpp.TOL_PICK_PLACE)
            first_tol_src = "未定義 → TOL_PICK_PLACE で代用"
            _warn("先頭 HOME の到達許容差が決まっていません。"
                  "{:.1f}° で続けます（止めません）".format(first_tol))
            first_warnings.append("arrival_tol_undefined")
        first_timeout = _arrival_timeout_for(_first_max_delta)
        print("[STEP  1] 到達待ち 許容 {:.1f}°[{}] / 最大 {:.1f}s".format(
            first_tol, first_tol_src, first_timeout))
        t_first_seg = time.monotonic()   # 区間全体（読み直しを含む）
        t_first_send = None              # 送信の計時は送信直前に始める（R11b T5）
        first_rtt = None
        first_rtt_retry = None
        first_daemon_rtt = None          # 依頼 213: daemon とのやりとりだけの往復（Mac 側の待ちを含まない）
        first_daemon_rtt_retry = None
        first_arrived = None
        first_max = None
        first_actual = None
        first_aborted_at = None
        first_pre_actual = None
        first_send_attempted = False     # 送信を試みたか（R11b T5）
        first_retried = False
        first_retry_arrived = None
        first_retry_reason = None
        first_retry_move_after_deg = None
        first_retry_move_deg = None
        phase = "pre_read"
        try:
            # 項目3 + codex R11 A6: 再送の可否判定に送信前の姿勢が要る。
            # cmd_run が読んだ current_angles_before_replay は、そのあとに
            # カメラ確認と Enter 待ちを挟むので「送信直前」ではない
            #（Enter 待ちは人次第でいくらでも伸びる）。**ここで読み直す**。
            # 読めなければ送らずに ABORT（本線も「fallback はしない」）。
            first_pre_actual = sampler.read(once=False)
            if first_pre_actual is None:
                _warn("先頭移動の送信前の実測角が取れませんでした。"
                      "動いたかを判定できませんが**止めません**")
                first_warnings.append("pre_read_failed")
            # 読み直した姿勢で max|Δ| とタイムアウトを作り直す
            if first_pre_actual is not None:
                _first_max_delta = _max_axis_diff_finite(
                    HOME_ANGLES, first_pre_actual)
            first_timeout = _arrival_timeout_for(_first_max_delta)
            _log("先頭移動: 送信直前の実測で max|Δ| = {}° / 到達待ち 最大 {:.1f}s"
                 .format(("{:.2f}".format(_first_max_delta)
                          if _first_max_delta is not None else "None"),
                         first_timeout))
            # codex R11b T5: 読み直しで max|Δ| が増えていたら、ここでも警告する
            # （Enter 前の大移動 WARN は読み直し前の姿勢で出しているため）。
            if (_first_max_delta is not None
                    and _first_max_delta > FIRST_MOVE_WARN_DEG):
                _warn("送信直前の実測では current → HOME が {:.2f}°（> {:.1f}°）"
                      "あります。周囲に手・物が無いか、もう一度目で確かめてください"
                      .format(_first_max_delta, FIRST_MOVE_WARN_DEG))
                log_record["first_move_warn_after_reread_deg"] = round(
                    _first_max_delta, 3)
            phase = "send"
            first_send_attempted = True
            try:
                t_first_send = time.monotonic()
                _send_angles_guarded(HOME_ANGLES, SPEED_FIRST, "step1_HOME_START",
                                     recorder=recorder)
                first_daemon_rtt = _LAST_SEND["daemon_rtt_sec"]
                first_rtt = time.monotonic() - t_first_send
                log_record["first_move_send_rtt_sec"] = round(first_rtt, 4)
                streak["consecutive_send_failures"] = 0
            except _Aborted:
                first_aborted_at = "send_backstop"
                raise
            except Exception as e:
                # 2026-10-19 方針変更: 送信の例外でも止めない
                first_daemon_rtt = _LAST_SEND["daemon_rtt_sec"]   # 依頼 213 codex R2: 失敗でも残す
                _note_send_failure(streak, "先頭移動 (current → HOME)",
                                   "step1_HOME_START", e)
                _warn("先頭移動の send_angles が失敗しました。止めません。"
                      "到達確認で動いたかを見ます: {}: {}".format(
                          type(e).__name__, e))
                first_warnings.append("send_failed")

            print("[STEP  1] {:.1f}s 待ちます（settle）".format(
                FIRST_MOVE_SETTLE_SEC))
            sampler.sleep_with_reads(FIRST_MOVE_SETTLE_SEC)

            phase = "arrival_wait"
            first_arrived, first_max, first_actual = _hold_until_arrival(
                HOME_ANGLES, "current → HOME", sampler, first_tol, first_timeout)

            if not first_arrived:
                # 2026-10-19 方針変更: move 区間とまったく同じ扱い。
                #   1.0° 以上動いた → WARN で先へ進む
                #   1.0° 未満       → 5s 待って同じ HOME へ 1 回再送
                #     再送後 1.0° 以上 → WARN で先へ進む
                #     再送後も 1.0° 未満 → **ABORT（唯一）**
                phase = "arrival_decide"
                if first_pre_actual is not None and first_actual is not None:
                    first_retry_move_deg, _mlabel, _unexp = jss._retry_progress(
                        first_pre_actual, first_actual, first_tol)
                else:
                    first_retry_move_deg, _mlabel = None, "pre→last max|Δ|"
                _moved_ok = (first_retry_move_deg is not None
                             and _is_finite_num(first_retry_move_deg)
                             and first_retry_move_deg >= RETRY_NO_MOVE_THRESHOLD_DEG)
                _unknown = (first_retry_move_deg is None
                            or not _is_finite_num(first_retry_move_deg))

                if _moved_ok:
                    first_retry_reason = ("{} {:.2f}° >= {:.1f}°（動いているので"
                                          "再送しない）".format(
                                              _mlabel, first_retry_move_deg,
                                              RETRY_NO_MOVE_THRESHOLD_DEG))
                    _warn("先頭 HOME の到達確認タイムアウト（max|Δ|={}° > "
                          "tol {:.1f}°）ですが {}。**止めずに再生を始めます**"
                          .format(("{:.2f}".format(first_max)
                                   if first_max is not None else "None"),
                                  first_tol, first_retry_reason))
                    first_warnings.append("arrival_timeout_but_moved")
                elif _unknown:
                    first_retry_reason = "移動量が分からない（実測が無い）"
                    _warn("先頭 HOME の到達確認タイムアウト。{}ので再送の判定が"
                          "できません。**止めずに再生を始めます**".format(
                              first_retry_reason))
                    first_warnings.append("arrival_timeout_move_unknown")
                else:
                    first_retried = True
                    first_retry_reason = (
                        "{} {:.2f}° < {:.1f}°（送信無視疑い）".format(
                            _mlabel, first_retry_move_deg,
                            RETRY_NO_MOVE_THRESHOLD_DEG))
                    phase = "retry_pause"
                    _warn("先頭 HOME の送信が無視されたとみなします（{}）。"
                          "{:.1f}s 待って**同じ HOME へ**再送します".format(
                              first_retry_reason, RETRY_PAUSE_SEC))
                    first_warnings.append("retry_sent")
                    # 依頼 121: 復帰待ちの間は電流の記録もシリアルを触らない（codex R1 必須 2）
                    recorder.pause_currents()
                    time.sleep(RETRY_PAUSE_SEC)
                    recorder.resume_currents()

                    phase = "retry_pre_read"
                    _pre_retry = sampler.read(once=False)
                    if _pre_retry is None:
                        _warn("再送の前の実測角が取れませんでした。"
                              "1 回目の基準姿勢で判定します（止めません）")
                        first_warnings.append("retry_pre_read_failed")
                        _retry_base = (first_actual if first_actual is not None
                                       else first_pre_actual)
                    else:
                        _retry_base = _pre_retry
                    _timeout_retry = _arrival_timeout_for(
                        _max_axis_diff_finite(HOME_ANGLES, _retry_base)
                        if _retry_base is not None else _first_max_delta)

                    phase = "retry_send"
                    _t_retry = time.monotonic()
                    try:
                        _send_angles_guarded(HOME_ANGLES, SPEED_FIRST,
                                             "step1_HOME_START_retry",
                                             recorder=recorder)
                        first_daemon_rtt_retry = _LAST_SEND["daemon_rtt_sec"]
                        first_rtt_retry = time.monotonic() - _t_retry
                        _log("再送 OK（daemon 往復 {:.3f}s）".format(first_rtt_retry))
                    except _Aborted:
                        # 送信直前の姿勢検査（項目5 と同じもの）だけは止める
                        first_aborted_at = "retry_send_backstop"
                        raise
                    except Exception as e:
                        first_daemon_rtt_retry = _LAST_SEND["daemon_rtt_sec"]
                        _note_send_failure(streak, "先頭移動 (current → HOME)",
                                           "step1_HOME_START_retry", e)
                        _warn("先頭移動の再送 send_angles が失敗しました。"
                              "止めません: {}: {}".format(type(e).__name__, e))
                        first_warnings.append("retry_send_failed")

                    print("[STEP  1] {:.1f}s 待ちます（再送後の settle）".format(
                        FIRST_MOVE_SETTLE_SEC))
                    sampler.sleep_with_reads(FIRST_MOVE_SETTLE_SEC)

                    phase = "retry_arrival_wait"
                    (first_retry_arrived, first_max,
                     first_actual) = _hold_until_arrival(
                        HOME_ANGLES, "current → HOME（再送）", sampler,
                        first_tol, _timeout_retry)
                    first_arrived = bool(first_retry_arrived)

                    # ---- 唯一の ABORT: 再送しても全く動かない ----
                    _m2 = None
                    if _retry_base is not None and first_actual is not None:
                        _m2, _rl, _ru = jss._retry_progress(
                            _retry_base, first_actual, first_tol)
                    first_retry_move_after_deg = _m2
                    if first_retry_arrived:
                        _log("再送で HOME に到達しました")
                    elif (_m2 is not None and _is_finite_num(_m2)
                            and _m2 >= RETRY_NO_MOVE_THRESHOLD_DEG):
                        _warn("再送でも許容差に入りませんでしたが {:.2f}° "
                              "動いています（>= {:.1f}°）。"
                              "**止めずに再生を始めます**".format(
                                  _m2, RETRY_NO_MOVE_THRESHOLD_DEG))
                        first_warnings.append("retry_timeout_but_moved")
                    elif _m2 is None or not _is_finite_num(_m2):
                        _warn("再送のあとの移動量が分かりません。"
                              "**止めずに再生を始めます**")
                        first_warnings.append("retry_move_unknown")
                    else:
                        first_aborted_at = "retry_no_move"
                        raise _Aborted(
                            "step1_HOME_START",
                            "**送り直してもアームが動きません**"
                            "（再送後の移動 {:.2f}° < {:.1f}° / HOME まで {}°）。"
                            "これが唯一の中断条件です。これ以降は何も送りません"
                            .format(_m2, RETRY_NO_MOVE_THRESHOLD_DEG,
                                    ("{:.2f}".format(first_max)
                                     if first_max is not None else "None")))
        except BaseException:
            if first_aborted_at is None:
                first_aborted_at = phase
            raise
        finally:
            segments_log.append({
                "step": 1, "kind": "move",
                "from_name": "current", "to_name": "HOME",
                "stage": "HOME_START",
                "critical": False,
                "max_delta_deg": (round(_first_max_delta, 3)
                                  if _first_max_delta is not None else None),
                "arrival_ok": (None if first_arrived is None
                               else bool(first_arrived)),
                "arrival_max_delta_deg": (round(float(first_max), 3)
                                          if first_max is not None else None),
                "arrival_actual": first_actual,
                # codex R11b T5: 送信を試みていない（送信前の読みで止まった）場合は
                # 失敗 0 件。試みて例外になった場合だけ 1 件。
                "send_failures": (0 if (first_rtt is not None
                                        or not first_send_attempted) else 1),
                "send_attempted": bool(first_send_attempted),
                "send_rtt_sec": (round(first_rtt, 4)
                                 if first_rtt is not None else None),
                "segment_elapsed_sec": round(time.monotonic() - t_first_seg, 3),
                "aborted_at": first_aborted_at,
                "speed": SPEED_FIRST,
                "arrival_tol_deg": first_tol,
                "arrival_tol_source": first_tol_src,
                "arrival_timeout_sec": round(float(first_timeout), 2),
                "pre_actual": first_pre_actual,
                "pre_max_delta_deg": (round(_first_max_delta, 3)
                                      if _first_max_delta is not None else None),
                "retried": bool(first_retried),
                "retry_arrived": (None if first_retry_arrived is None
                                  else bool(first_retry_arrived)),
                "retry_reason": first_retry_reason,
                "retry_move_deg": (round(float(first_retry_move_deg), 3)
                                   if first_retry_move_deg is not None
                                   and _is_finite_num(first_retry_move_deg)
                                   else None),
                "retry_move_after_deg": (
                    round(float(first_retry_move_after_deg), 3)
                    if first_retry_move_after_deg is not None
                    and _is_finite_num(first_retry_move_after_deg) else None),
                "retry_no_move_threshold_deg": RETRY_NO_MOVE_THRESHOLD_DEG,
                "retry_pause_sec": (RETRY_PAUSE_SEC if first_retried else None),
                "warnings": list(first_warnings),
                "send_rtt_retry_sec": (round(first_rtt_retry, 4)
                                       if first_rtt_retry is not None else None),
                # 依頼 213: 送信のあとの 1.0 秒をどこで待ったか と daemon とのやりとりだけの往復
                "send_wait_on": SEND_WAIT_ON,
                "send_daemon_rtt_sec": (round(first_daemon_rtt, 4)
                                        if first_daemon_rtt is not None else None),
                "send_daemon_rtt_retry_sec": (round(first_daemon_rtt_retry, 4)
                                              if first_daemon_rtt_retry is not None else None),
            })

        # codex R11 A1: 爪の直前の静止確認に「直前の move の目標と許容差」を使う。
        # STEP 1 のあとは HOME が直前の目標。
        last_move_target = list(HOME_ANGLES)
        last_move_tol = first_tol

        # ---- STEP 2 以降 ----
        step_no = 2
        for s in seq:
            if s["kind"] == "gripper":
                is_close = (s["action"] == "close")
                state = 1 if is_close else 0
                if is_close:
                    cmd_str = "daemon_set_gripper_value({}, {})".format(
                        GRIP_VALUE, GRIPPER_SPEED)
                else:
                    cmd_str = "daemon_set_gripper(0, {}, wait=0)".format(GRIPPER_SPEED)
                print("[STEP {:>2}] GRIPPER {} → {}".format(
                    step_no, s["action"].upper(), cmd_str))
                # A4-2（エイイチ判断 2026-10-19）: recorder への通知は
                # **daemon 呼び出しが戻った直後**に送る（閉じる・開く両方）。
                # 開く経路の daemon ハンドラは set_gripper_state() の手前で
                # time.sleep(2) する（DAY175 対策・daemon は変えない）ので、
                # 呼び出しの前に通知すると「実際に爪へ命令が出る約2秒前」に
                # state が切り替わった記録になる。build_demo_episodes.py は
                # gripper_cmd の時刻で state を 0↔1 にするため、約2秒ぶんの
                # フレームが誤ラベルになる。wait=0 なので
                # 「戻った時刻 ≒ 実際に爪へ命令が出た時刻」。
                # 例外時も通知する（イベント自体を落とすと state が切り替わらない
                # 記録になるため）。失敗は note と試行 JSON に残す。
                # ---- 爪の開閉 ----
                # 2026-10-19（エイイチ判断・方針変更 + 項目4・項目6）:
                # get_gripper_value を使った開放確認をやめ、**本線
                # run_pick_place.py の _do_gripper と同じ形**にする。
                #   open:  daemon_set_gripper(0) 1 回 → 2.0s（daemon 事後相当）→ 2.0s
                #   close: set_gripper_value を 1 回 + 2.0s（本線 GRIPPER_SLEEP）
                # 理由: 段階C で PLACE の open のあと gv が -1 を返し、その直後から
                # アームが send_angles を受け付けなくなった（書き込みは通り
                # elapsed 10.5ms、get_angles も読めるのにモーターが動かない。
                # daemon を再起動するまで gv は -1 のままだった）。
                # T1/T2 で「静止中の gv 読み取りは壊さない」ことは確認したが、
                # 爪が動いている最中の読み取りが引き金の可能性が残る。
                # stage9 の形は 647 試行で詰まっていない実績がある。
                # **開いたかは人が目で見て y/n で答える**（自動確認はしない）。
                # ---- codex R11 A1 + 2026-10-19 方針変更（項目2）------------
                # 爪を動かす前に **止まるまで最大 8 秒待つ**。
                # 止まらなくても **ABORT はしない**。WARN を残して爪を動かす。
                # **目標との差は見ない**（止まっているかだけを見る）。
                # 到達確認は「1 回でも tol 内に読めたら到達」なので、通過中でも
                # 到達になりうる（段階C 1回目はその状態で close していた）。
                # ここで待つことで、動いている最中に爪を動かす確率を下げる。
                still_ok, still_detail = _wait_until_still(
                    sampler, None, None,
                    "step{}_{}".format(step_no, s["action"]))
                if not still_ok:
                    _warn("爪を動かす前の静止確認が取れませんでした"
                          "（連続差 最大 {}° / 連続 {} 回 < 必要 {} 回 / "
                          "{:.1f}s）。**止めずに爪を動かします**（WARN のみ）"
                          .format((("{:.2f}".format(still_detail["max_step_deg"]))
                                   if still_detail.get("max_step_deg") is not None
                                   else "None"),
                                  still_detail.get("consecutive_ok"),
                                  GRIPPER_STILL_READS, GRIPPER_STILL_TIMEOUT_SEC))

                is_open_cmd = not is_close
                attempts = GRIPPER_OPEN_ATTEMPTS if is_open_cmd else 1
                cmd_error = None
                attempt_log = []
                notify_ts = None
                last_rtt = None

                # 依頼 121: 爪の命令から待ち（dwell）が終わるまで、電流の記録スレッドも
                # daemon を読まない（下の「読み取りを挟まない」と同じ扱い）。
                # 読んでいる途中ならその 1 回（約 0.1 秒）が終わってから爪の命令を送る。
                # 再生の中で電流の記録を止めるのは、ここと再送の前の待ち（RETRY_PAUSE_SEC）の 2 か所。
                recorder.pause_currents()
                for attempt in range(1, attempts + 1):
                    t_cmd_send = time.monotonic()
                    t_cmd_wall = time.time()
                    try:
                        if is_close:
                            daemon_set_gripper_value(GRIP_VALUE, GRIPPER_SPEED)
                        else:
                            daemon_set_gripper(0, GRIPPER_SPEED, wait=0)
                        cmd_error = None
                    except Exception as e:
                        cmd_error = "{}: {}".format(type(e).__name__, e)
                    last_rtt = time.monotonic() - t_cmd_send
                    if cmd_error is not None:
                        break
                    # recorder へ出す時刻は **1 回目の指令時刻**で固定する
                    # （2 回目は同じ open を念のため送るだけ。state は変わらない）
                    if notify_ts is None:
                        notify_ts = t_cmd_wall + last_rtt
                    attempt_log.append({
                        "attempt": attempt,
                        "cmd_returned_ts": t_cmd_wall + last_rtt,
                        "daemon_roundtrip_sec": round(last_rtt, 4),
                    })
                    _log("gripper {} ({}/{} 回目) の daemon 往復 = {:.3f}s".format(
                        s["action"], attempt, attempts, last_rtt))
                    if is_open_cmd:
                        # codex R10 S2: 本線がヘルパ既定 wait=2 で得ている
                        # 事後 sleep の相当分。**読み取りを挟まない**で待つ
                        # （爪を動かした直後にシリアルを触らない）。
                        print("[STEP {:>2}] open 後 {:.1f}s 待ちます"
                              "（本線の daemon wait=2 相当・読み取りなし）"
                              .format(step_no, GRIPPER_OPEN_POST_SLEEP_SEC))
                        time.sleep(GRIPPER_OPEN_POST_SLEEP_SEC)
                    dwell = (GRIPPER_OPEN_DWELL_SEC if is_open_cmd
                             else GRIPPER_DWELL_SEC)
                    # codex R11b T4: ここも **読み取りを挟まない**。
                    # 本線 _do_gripper は time.sleep(GRIPPER_SLEEP) だけで、
                    # 爪の指令の直後に get_angles を出さない。close のあとに
                    # 読んでいたのは本線との差で、方針（爪を動かした直後に
                    # シリアルを触らない）にも反していた。
                    # 実測角の 2.0s の欠測は受け入れる（動画は止まらない）。
                    print("[STEP {:>2}] gripper dwell {}s（読み取りなし）".format(
                        step_no, dwell))
                    time.sleep(dwell)
                # 待ちが終わってから再開する。待ちの途中の例外（Ctrl-C など）では再開しない
                # （止めたまま、cmd_run の finally で記録を閉じる。codex R1 必須 1）。
                # 爪の命令が失敗して break したときは待たずに先へ進む（本体も読み始める）ので再開する。
                recorder.resume_currents()

                seg_rec = {
                    "step": step_no, "kind": "gripper", "action": s["action"],
                    "daemon_roundtrip_sec": (round(last_rtt, 4)
                                             if last_rtt is not None else None),
                    "error": cmd_error,
                    "attempts": len(attempt_log),
                    "attempt_log": attempt_log,
                    "method": ("open_once_no_readback" if is_open_cmd
                               else "set_gripper_value_once"),
                    "pre_sleep_sec": 0.0,
                    "post_sleep_sec": (GRIPPER_OPEN_POST_SLEEP_SEC if is_open_cmd
                                       else 0.0),
                    # codex R10b T3: daemon の事前 sleep(2) は set_gripper
                    #（open）ハンドラにだけある。close の set_gripper_value には無い。
                    "daemon_pre_sleep_sec": (DAEMON_SET_GRIPPER_PRE_SLEEP_SEC
                                             if is_open_cmd else 0.0),
                    "dwell_sec": (GRIPPER_OPEN_DWELL_SEC if is_open_cmd
                                  else GRIPPER_DWELL_SEC),
                    "verified_by": "human_y_n",
                    "still_check": still_detail,
                }

                if cmd_error is not None:
                    # 2026-10-19 方針変更: 爪の指令が失敗しても **止めない**。
                    # 爪が開いたか閉じたかは分からないが、WARN と試行 JSON に残して
                    # 先へ進む（最終判定は人の y/n と動画）。
                    _warn("{} が失敗しました（{}）。爪の状態は分かりませんが"
                          "**止めません**（WARN のみ）".format(cmd_str, cmd_error))
                    log_record.setdefault("gripper_errors", []).append({
                        "step": step_no, "action": s["action"], "error": cmd_error,
                    })

                seg_rec["state"] = state
                segments_log.append(seg_rec)
                # recorder への通知は「1 回目の指令が戻った時刻」で出す（A4-2）
                recorder.gripper_cmd(
                    state, "step{}_{}".format(step_no, s["action"]), ts=notify_ts)
                log_record["gripper_last_state"] = state
                step_no += 1
                continue

            # ---- move: 区間ごとに 1 回だけ送って到達を待つ ----
            # 2026-10-19（エイイチ判断）: 0.1 秒ごとの補間送信をやめた。
            # 19 本（run_pick_place.py）と同じ「1 回送って到達を待つ」動かし方。
            # 移動中の実測角は _hold_until_arrival のポーリング
            # （ARRIVAL_POLL_SLEEP = 0.3s → 約 3.3Hz）で読んで recorder へ流れる。
            #
            # codex R7 N2/N3（単発送信にしたことで新たに必要になった扱い）:
            #   補間ストリーミングでは「未到達」＝経路上で少し遅れている、だったので
            #   critical 以外は WARN で続行してよかった。
            #   単発送信では「未到達」＝**中継姿勢に着いていない**ので、そこから
            #   次の目標を送ると、どこにいるか分からない姿勢から関節空間の直線で
            #   移動することになり、ブロックや箱に当たりうる。
            #   よって **critical でなくても到達できなければ ABORT** する
            #   （Pi 配備版の「critical だけ ABORT」から意図的に変えた）。
            #   送信失敗（1 区間 1 指令なので未送達の可能性）も、同じ到達確認で
            #   捕まえて止める。自動再送はしない。
            label = "{} → {}".format(s["from_name"], s["to_name"])
            # 許容差は区間ごと（本線の stage 値）。
            # 2026-10-19 方針変更: 知らない区間名でも止めない。本線のいちばん緩い
            # 値（TOL_PICK_PLACE = 3.0）で続け、WARN と試行 JSON に残す。
            seg_warnings = []
            tol_deg, tol_src = _arrival_tol_for(s["to_name"])
            if tol_deg is None:
                tol_deg = float(rpp.TOL_PICK_PLACE)
                tol_src = "未定義 → TOL_PICK_PLACE で代用"
                _warn("区間名 {!r} の到達許容差が決まっていません。"
                      "本線のいちばん緩い値 {:.1f}° で続けます（止めません）"
                      .format(s["to_name"], tol_deg))
                seg_warnings.append("arrival_tol_undefined")
            timeout_sec = _arrival_timeout_for(s["max_delta"])
            print("[STEP {:>2}] MOVE → {}  segment {}  "
                  "(1 回送信 speed={} → 到達待ち 許容 {:.1f}°[{}] / 最大 {:.1f}s){}".format(
                      step_no, s["to_name"], label, SPEED_SINGLE,
                      tol_deg, tol_src, timeout_sec,
                      "  [critical]" if s.get("critical") else ""))
            recorder.stage(_stage_name(s["to_name"]))

            send_failures = 0
            send_rtt_sec = None
            send_rtt_retry_sec = None
            send_daemon_rtt_sec = None        # 依頼 213
            send_daemon_rtt_retry_sec = None
            arrived = None
            last_max = None
            last_actual = None
            aborted_at = None
            retried = False
            retry_arrived = None
            retry_reason = None
            retry_move_deg = None
            retry_move_after_deg = None
            pre_actual = None
            pre_max = None
            t_seg = time.monotonic()
            # codex R7 N6: 送信と到達待ちを **1 つの try/finally** で囲み、
            # どこで抜けても区間の記録を必ず 1 件残す
            # （以前は到達待ち中の Ctrl-C / 実測 j4 ハード帯 ABORT で記録が欠けた）。
            phase = "pre_read"
            try:
                # 再送の可否は「送信前姿勢 → 到達待ちで最後に読めた姿勢」の
                # 移動量で決める。2026-10-19 方針変更: 読めなくても止めない。
                # その場合は「動いたか不明」として扱い、再送もしないで次へ進む。
                pre_actual = sampler.read(once=False)
                if pre_actual is None:
                    _warn("送信前の実測角が取れませんでした（区間 {}）。"
                          "動いたかどうかを判定できないので、この区間は"
                          "「不明」として続けます（止めません）".format(label))
                    seg_warnings.append("pre_read_failed")
                pre_max = (_max_axis_diff_finite(s["to"], pre_actual)
                           if pre_actual is not None else None)
                timeout_sec = _arrival_timeout_for(
                    pre_max if pre_max is not None else s["max_delta"])

                phase = "send"
                t_send = time.monotonic()
                try:
                    _send_angles_guarded(
                        s["to"], SPEED_SINGLE,
                        "step{}_{}".format(step_no, _stage_name(s["to_name"])),
                        recorder=recorder)
                    send_daemon_rtt_sec = _LAST_SEND["daemon_rtt_sec"]
                    send_rtt_sec = time.monotonic() - t_send
                    send_rtts.append(send_rtt_sec)
                    streak["consecutive_send_failures"] = 0
                    _log("送信 OK（daemon 往復 {:.3f}s）".format(send_rtt_sec))
                except _Aborted:
                    # 送信直前の姿勢検査（backstop）で弾いた分はそのまま止める
                    aborted_at = "send_backstop"
                    raise
                except Exception as e:
                    # 2026-10-19 方針変更: 送信の例外でも止めない。
                    # 指令が届いたかは分からないが、**動いたかどうかは到達確認で
                    # 分かる**ので、そこで判断する（届いていなければ
                    # 「1.0° 未満 → 再送 → それでも 1.0° 未満」で ABORT になる）。
                    send_failures += 1
                    send_daemon_rtt_sec = _LAST_SEND["daemon_rtt_sec"]   # 依頼 213 codex R2: 失敗でも残す
                    _note_send_failure(streak, label, "単発送信", e)
                    _warn("send_angles が失敗しました（区間 {}）。止めません。"
                          "到達確認で動いたかを見ます: {}: {}".format(
                              label, type(e).__name__, e))
                    seg_warnings.append("send_failed")

                phase = "arrival_wait"
                arrived, last_max, last_actual = _hold_until_arrival(
                    s["to"], label, sampler, tol_deg, timeout_sec)

                if not arrived:
                    # ---- 2026-10-19 方針変更（エイイチ指示）------------------
                    # 止めるのは「**送り直しても全く動かない**」ときだけ。
                    #   1.0° 以上動いている        → WARN で次の区間へ
                    #   1.0° 未満                  → 5s 待って同じ目標へ 1 回再送
                    #     再送後に 1.0° 以上動いた → WARN で次の区間へ
                    #     再送後も 1.0° 未満       → **ABORT（唯一の ABORT）**
                    #   移動量が分からない          → WARN で次の区間へ
                    # 送るのは s["to"] だけ。HOME や別の姿勢へは絶対に送らない。
                    phase = "arrival_decide"
                    move_label = "pre→last max|Δ|"
                    if pre_actual is not None and last_actual is not None:
                        retry_move_deg, move_label, _unexp = (
                            jss._retry_progress(pre_actual, last_actual, tol_deg))
                    else:
                        retry_move_deg = None

                    moved_ok = (retry_move_deg is not None
                                and _is_finite_num(retry_move_deg)
                                and retry_move_deg >= RETRY_NO_MOVE_THRESHOLD_DEG)
                    move_unknown = (retry_move_deg is None
                                    or not _is_finite_num(retry_move_deg))

                    if moved_ok:
                        retry_reason = ("{} {:.2f}° >= {:.1f}°（動いているので"
                                        "再送しない）".format(
                                            move_label, retry_move_deg,
                                            RETRY_NO_MOVE_THRESHOLD_DEG))
                        _warn("到達確認タイムアウト（max|Δ|={}° > tol {:.1f}°）"
                              "ですが {}。**止めずに次の区間へ進みます**".format(
                                  ("{:.2f}".format(last_max)
                                   if last_max is not None else "None"),
                                  tol_deg, retry_reason))
                        seg_warnings.append("arrival_timeout_but_moved")
                    elif move_unknown:
                        retry_reason = ("移動量が分からない（送信前または到達待ちの"
                                        "実測が無い）")
                        _warn("到達確認タイムアウト（max|Δ|={}° > tol {:.1f}°）。"
                              "{}ので再送の判定ができません。"
                              "**止めずに次の区間へ進みます**".format(
                                  ("{:.2f}".format(last_max)
                                   if last_max is not None else "None"),
                                  tol_deg, retry_reason))
                        seg_warnings.append("arrival_timeout_move_unknown")
                    else:
                        # ---- 1.0° 未満しか動いていない → 再送（1 回だけ）----
                        retried = True
                        retry_reason = ("{} {:.2f}° < {:.1f}°（送信無視疑い）".format(
                            move_label, retry_move_deg,
                            RETRY_NO_MOVE_THRESHOLD_DEG))
                        phase = "retry_pause"
                        _warn("1 回目の送信が無視されたとみなします（{}）。"
                              "{:.1f}s 待って**同じ目標へ**再送します".format(
                                  retry_reason, RETRY_PAUSE_SEC))
                        seg_warnings.append("retry_sent")
                        # 復帰待ちの間はシリアルを触らない（本線と同じ）
                        # 依頼 121: 電流の記録も止める（codex R1 必須 2）
                        recorder.pause_currents()
                        time.sleep(RETRY_PAUSE_SEC)
                        recorder.resume_currents()

                        phase = "retry_pre_read"
                        pre_actual_retry = sampler.read(once=False)
                        if pre_actual_retry is None:
                            # 読めなくても止めない。1 回目の送信前姿勢（または
                            # 到達待ちの最後の姿勢）を再送の基準に使う。
                            _warn("再送の前の実測角が取れませんでした。"
                                  "1 回目の基準姿勢で判定します（止めません）")
                            seg_warnings.append("retry_pre_read_failed")
                            retry_base = (last_actual if last_actual is not None
                                          else pre_actual)
                        else:
                            retry_base = pre_actual_retry
                        pre_max_retry = (_max_axis_diff_finite(s["to"], retry_base)
                                         if retry_base is not None else None)
                        timeout_retry_sec = _arrival_timeout_for(
                            pre_max_retry if pre_max_retry is not None
                            else s["max_delta"])

                        phase = "retry_send"
                        t_send_retry = time.monotonic()
                        try:
                            _send_angles_guarded(
                                s["to"], SPEED_SINGLE,
                                "step{}_{}_retry".format(
                                    step_no, _stage_name(s["to_name"])),
                                recorder=recorder)
                            send_daemon_rtt_retry_sec = _LAST_SEND["daemon_rtt_sec"]
                            send_rtt_retry_sec = time.monotonic() - t_send_retry
                            _log("再送 OK（daemon 往復 {:.3f}s）".format(
                                send_rtt_retry_sec))
                        except _Aborted:
                            # 送信直前の姿勢検査（JOINT_LIMITS / J4_SAFE）だけは
                            # そのまま止める（項目5 の検証と同じもの）
                            aborted_at = "retry_send_backstop"
                            raise
                        except Exception as e:
                            send_failures += 1
                            send_daemon_rtt_retry_sec = _LAST_SEND["daemon_rtt_sec"]
                            _note_send_failure(streak, label, "再送", e)
                            _warn("再送の send_angles が失敗しました。止めません。"
                                  "動いたかを到達確認で見ます: {}: {}".format(
                                      type(e).__name__, e))
                            seg_warnings.append("retry_send_failed")

                        phase = "retry_arrival_wait"
                        retry_arrived, last_max, last_actual = _hold_until_arrival(
                            s["to"], label + "（再送）", sampler,
                            tol_deg, timeout_retry_sec)
                        arrived = bool(retry_arrived)

                        # ---- 唯一の ABORT: 再送しても全く動かない ----
                        retry2_move = None
                        if retry_base is not None and last_actual is not None:
                            retry2_move, _rlabel, _ru = jss._retry_progress(
                                retry_base, last_actual, tol_deg)
                        retry_move_after_deg = retry2_move
                        if retry_arrived:
                            _log("再送で到達しました（{}）".format(label))
                        elif (retry2_move is not None
                                and _is_finite_num(retry2_move)
                                and retry2_move >= RETRY_NO_MOVE_THRESHOLD_DEG):
                            _warn("再送でも許容差に入りませんでしたが "
                                  "{:.2f}° 動いています（>= {:.1f}°）。"
                                  "**止めずに次の区間へ進みます**".format(
                                      retry2_move, RETRY_NO_MOVE_THRESHOLD_DEG))
                            seg_warnings.append("retry_timeout_but_moved")
                        elif retry2_move is None or not _is_finite_num(retry2_move):
                            _warn("再送のあとの移動量が分かりません"
                                  "（基準または実測が無い）。"
                                  "**止めずに次の区間へ進みます**")
                            seg_warnings.append("retry_move_unknown")
                        else:
                            aborted_at = "retry_no_move"
                            raise _Aborted(
                                "step{}_{}".format(
                                    step_no, _stage_name(s["to_name"])),
                                "**送り直してもアームが動きません**"
                                "（再送後の移動 {:.2f}° < {:.1f}° / "
                                "目標まで {}°）。これが唯一の中断条件です。"
                                "これ以降は何も送りません"
                                "（HOME へは自動で戻りません）".format(
                                    retry2_move, RETRY_NO_MOVE_THRESHOLD_DEG,
                                    ("{:.2f}".format(last_max)
                                     if last_max is not None else "None")))

                # pose_used（台帳 37-50）用: PICK / PLACE で最後に読めた実測角。
                # 到達していなくても「実際にそこで爪を動かした姿勢」を残す。
                if s["to_name"] == "PICK" and last_actual is not None:
                    log_record.setdefault("pose_used", {})["pick_used"] = list(
                        last_actual)
                if s["to_name"] == "PLACE" and last_actual is not None:
                    log_record.setdefault("pose_used", {})["place_used"] = list(
                        last_actual)
            except BaseException:
                # 到達待ちの最中に外から来た中断（Ctrl-C / 実測 j4 のハード帯 ABORT など）
                # codex R8 O3: 送信中の中断を arrival_wait と書かない
                if aborted_at is None:
                    aborted_at = phase
                raise
            finally:
                segments_log.append({
                    "step": step_no, "kind": "move",
                    "from_name": s["from_name"], "to_name": s["to_name"],
                    "stage": _stage_name(s["to_name"]),
                    "critical": bool(s.get("critical")),
                    "max_delta_deg": round(float(s["max_delta"]), 3),
                    "arrival_ok": (None if arrived is None else bool(arrived)),
                    "arrival_max_delta_deg": (round(float(last_max), 3)
                                              if last_max is not None else None),
                    "arrival_actual": last_actual,
                    "send_failures": send_failures,
                    # 単発送信方式: 送信 1 回の daemon 往復時間と、
                    # 送信から到達確認が終わる（または中断する）までの実時間
                    "send_rtt_sec": (round(send_rtt_sec, 4)
                                     if send_rtt_sec is not None else None),
                    "segment_elapsed_sec": round(time.monotonic() - t_seg, 3),
                    "aborted_at": aborted_at,
                    # ---- 項目1: この区間に使った許容差とタイムアウト ----
                    "arrival_tol_deg": tol_deg,
                    "arrival_tol_source": tol_src,
                    "arrival_timeout_sec": round(float(timeout_sec), 2),
                    "pre_actual": pre_actual,
                    "pre_max_delta_deg": (round(float(pre_max), 3)
                                          if pre_max is not None else None),
                    # ---- 項目5: 送り直したか / 送り直しで届いたか ----
                    "retried": bool(retried),
                    "retry_arrived": (None if retry_arrived is None
                                      else bool(retry_arrived)),
                    "retry_reason": retry_reason,
                    "retry_move_deg": (round(float(retry_move_deg), 3)
                                       if retry_move_deg is not None
                                       and _is_finite_num(retry_move_deg) else None),
                    "retry_move_after_deg": (round(float(retry_move_after_deg), 3)
                                             if retry_move_after_deg is not None
                                             and _is_finite_num(retry_move_after_deg)
                                             else None),
                    "retry_no_move_threshold_deg": RETRY_NO_MOVE_THRESHOLD_DEG,
                    "retry_pause_sec": (RETRY_PAUSE_SEC if retried else None),
                    "send_rtt_retry_sec": (round(send_rtt_retry_sec, 4)
                                           if send_rtt_retry_sec is not None
                                           else None),
                    # 依頼 213: send_rtt_sec は どちらの待ち方でも 1.0 秒の待ちを含む（送信の始めから
                    # 到達の確かめに入るまで）。send_daemon_rtt_sec は daemon とのやりとりだけ
                    # （"mac" では待ちを含まない・"daemon" では含む）
                    "send_wait_on": SEND_WAIT_ON,
                    "send_daemon_rtt_sec": (round(send_daemon_rtt_sec, 4)
                                            if send_daemon_rtt_sec is not None else None),
                    "send_daemon_rtt_retry_sec": (round(send_daemon_rtt_retry_sec, 4)
                                                  if send_daemon_rtt_retry_sec is not None
                                                  else None),
                    # 2026-10-19 方針変更: 止めずに続けた事象はここに残す
                    "warnings": list(seg_warnings),
                })
            # codex R11 A1: 次の爪の静止確認が使う「直前の目標と許容差」
            last_move_target = list(s["to"])
            last_move_tol = tol_deg
            step_no += 1

        result = "completed"
        abort = None
    except _Aborted as e:
        _abort_msg("{}（これ以降の送信を停止します。HOME 復帰等は人が判断してください）"
                   .format(e.reason))
        result = "aborted"
        abort = {"stage": e.stage, "reason": e.reason}
    except KeyboardInterrupt:
        print("")
        _warn("Ctrl-C 検出。新しい送信を停止します（mc.stop は呼びません）")
        result = "interrupted"
        abort = {"stage": "keyboard_interrupt", "reason": "Ctrl-C"}
    except BaseException as e:
        # codex R1 D5: 予期しない例外も必ず「中断」として確定させる。
        # ここで握らないと試行 JSON・台帳に 1 行も残らず、追記義務
        # （data_manifest_rules 6節）を破る。再送はしない（自動 HOME 復帰もしない）。
        print("")
        _abort_msg("予期しない例外で中断します（これ以降の送信を停止します。"
                   "HOME 復帰等は人が判断してください）: {}: {}".format(
                       type(e).__name__, e))
        result = "error"
        abort = {"stage": "unexpected_exception",
                 "reason": "{}: {}".format(type(e).__name__, e),
                 "traceback": traceback.format_exc()}
    finally:
        # set_fresh_mode(0) は正常・異常いずれでも実行（Pi 配備版と同じ try/finally）
        try:
            daemon_set_fresh_mode(0)
            _log("daemon_set_fresh_mode(0) 完了")
        except Exception as e:
            _warn("daemon_set_fresh_mode(0) で例外: {}: {}".format(type(e).__name__, e))

    motion_elapsed = time.monotonic() - t_motion_start
    # A4-3: 1 tick の send_angles 往復時間と tick 全体の周期を、平均・最大・
    # p50・p99 で残す（daemon が送信のたびに角度を読み直すので、0.1s 周期が
    # 守れず Pi 直より遅くなる可能性がある。それを数字で見るため）。
    # 単発送信方式の計測。1 試行の送信は STEP 1 + 9 区間 = 10 回なので
    # 生の配列をそのまま残す（集約値だけでは遅い区間が追えない）。
    seg_elapsed = [sg.get("segment_elapsed_sec") for sg in segments_log
                   if sg.get("kind") == "move"]
    timing = {
        "mode": "single_send_per_segment",
        "speed": SPEED_SINGLE,
        "send_timeout_sec": DAEMON_SEND_TIMEOUT_SEC,
        # 項目1: 許容差は区間ごと（区間の記録に arrival_tol_deg がある）
        "arrival_tol_by_segment": {k: v[0]
                                   for k, v in ARRIVAL_TOL_BY_SEGMENT.items()},
        "arrival_tol_source": "run_pick_place.py の stage 値",
        "arrival_timeout_min_sec": ARRIVAL_TIMEOUT_MIN_SEC,
        "arrival_timeout_rule": "clamp(2.0 + max|Δ|/20*3.0, 8.0, 30.0)",
        # 項目3: silent_drop 対処
        "retry_policy": "本線 j0_sweep_scan._send_and_wait_arrival と同じ"
                        "（同じ目標へ 1 回だけ・ループしない・HOME へ戻さない）",
        "retry_pause_sec": RETRY_PAUSE_SEC,
        "retry_no_move_threshold_deg": RETRY_NO_MOVE_THRESHOLD_DEG,
        "arrival_poll_sleep_sec": ARRIVAL_POLL_SLEEP,
        # codex R7 N6: ここは **move 区間の成功送信数**（先頭 HOME は含まない）。
        # 先頭 HOME は first_move_send_rtt_sec に別枠で入る。
        "move_sends_ok": len(send_rtts),
        "first_move_send_included": False,
        "send_rtt_raw_sec": [round(t, 4) for t in send_rtts],
        "send_rtt": _stats(send_rtts, "send_rtt"),
        # 送信から到達確認が終わるまで（区間ごと・move 区間のみ）
        "segment_elapsed_raw_sec": [x for x in seg_elapsed],
        "segment_elapsed": _stats(seg_elapsed, "segment_elapsed"),
    }
    log_record["segments"] = segments_log
    log_record["actual_sampling"] = sampler.summary(motion_elapsed)
    log_record["tick_timing"] = timing
    log_record["send_failures"] = {
        "total": streak["total_send_failures"],
        "max_consecutive": streak["max_consecutive_send_failures"],
        "abort_streak": SEND_FAIL_ABORT_STREAK,
    }
    log_record["result"] = result
    log_record["abort"] = abort
    if sampler.last is not None:
        log_record["final_actual"] = list(sampler.last)
    if sampler.nonfinite_reads:
        _warn("実測角の読み取り {} 回が有限でない値のため失敗扱いになりました"
              "（試行 JSON の actual_sampling.nonfinite_reads）".format(
                  sampler.nonfinite_reads))
    if sampler.j4_out_of_range_count:
        _warn("実測 j4 が J4_SAFE 外に読めた回数 = {}（最初の数件: {}）。"
              "送信は止めていません（記録のみ・エイイチ判断。"
              "[{:+.1f}, {:+.1f}] の外なら ABORT する）".format(
                  sampler.j4_out_of_range_count, sampler.j4_out_of_range[:5],
                  J4_HARD_MIN, J4_HARD_MAX))

    hz = log_record["actual_sampling"]["hz"]
    _log("実測角の読み取り: ok {}/{} 回, 実効 {} Hz（目標 {} Hz 以上）".format(
        sampler.ok, sampler.attempted,
        ("{:.2f}".format(hz) if hz is not None else "計測不能"),
        ACTUAL_READ_TARGET_HZ))
    if hz is not None and hz < ACTUAL_READ_TARGET_HZ:
        _warn("実測角の読み取りが目標 {} Hz を下回りました（{:.2f} Hz）。"
              "学習データの state 密度が 19 本より薄くなります".format(
                  ACTUAL_READ_TARGET_HZ, hz))
    sr = timing["send_rtt"]
    se = timing["segment_elapsed"]
    if sr["n"]:
        _log("send_angles の daemon 往復 ({} 回): 平均 {:.3f}s / p50 {:.3f}s / "
             "最大 {:.3f}s（timeout {:.1f}s）".format(
                 sr["n"], sr["mean_sec"], sr["p50_sec"], sr["max_sec"],
                 DAEMON_SEND_TIMEOUT_SEC))
        if sr["max_sec"] is not None and sr["max_sec"] > 1.0:
            _warn("送信 1 回に {:.3f}s かかった回があります。daemon は送信後に"
                  "角度を読み直すので、動き始めで読めないと伸びます"
                  "（単発送信なので試行全体への影響は小さい）".format(sr["max_sec"]))
    if se["n"]:
        _log("区間の所要（送信→到達確認まで）({} 区間): 平均 {:.2f}s / "
             "最大 {:.2f}s".format(se["n"], se["mean_sec"], se["max_sec"]))

    return result


# ---- 人の判定 ---------------------------------------------------------------
def _ask_yes_no(question, max_attempts=ASK_MAX_ATTEMPTS):
    """y/n を聞く. EOF / Ctrl-C / 回数超過は 'unknown' を返す（勝手に yes にしない）.

    回数上限を置くのは、stdin が EOF を返さず空文字を返し続ける環境
    （リダイレクト・非対話シェル等）で無限ループにならないようにするため。
    """
    for _ in range(max_attempts):
        try:
            ans = input("{} {} (y/n) > ".format(LOG_PREFIX, question)).strip().lower()
        except (EOFError, KeyboardInterrupt):
            print("")
            _warn("入力が得られませんでした → unknown")
            return "unknown"
        if ans in ("y", "yes"):
            return "yes"
        if ans in ("n", "no"):
            return "no"
        print("{} y か n で答えてください".format(LOG_PREFIX))
    _warn("{} 回入力しても y/n になりませんでした → unknown".format(max_attempts))
    return "unknown"


# ---- 台帳追記 ---------------------------------------------------------------
def _ledger_mm(xz, idx):
    if not (isinstance(xz, (list, tuple)) and len(xz) > idx
            and isinstance(xz[idx], (int, float))):
        return ""
    return "{:.1f}".format(xz[idx] * 1000.0)


def _read_recording_meta(run_dir):
    """recorder が書く recording_meta.json を有界待ちで読む（本線と同値の 2.0s / 100ms）.

    後からの reconcile はしない（台帳は追記のみ・data_manifest 7節）。
    """
    path = ""
    fps = ""
    frames = ""
    status = "none"
    try:
        meta_path = Path(run_dir) / "recording" / "recording_meta.json"
        deadline = time.monotonic() + RECORDING_META_POLL_TIMEOUT_SEC
        while not meta_path.is_file():
            remaining = deadline - time.monotonic()
            if remaining <= 0:
                break
            time.sleep(min(RECORDING_META_POLL_INTERVAL_SEC, remaining))
        if meta_path.is_file():
            meta = json.loads(meta_path.read_text())
            path = str(meta.get("recording_path") or "recording/")
            f = meta.get("effective_fps")
            fps = "{:.3f}".format(f) if isinstance(f, (int, float)) else ""
            fc = meta.get("frames_captured")
            if not isinstance(fc, int):
                fc = meta.get("frame_count")
            frames = str(fc) if isinstance(fc, int) else ""
            status = str(meta.get("status") or "none")
    except Exception:
        status = "none"
    return path, fps, frames, status


def _append_ledger(log_record, run_stem, run_dir, recording_enabled):
    """試行 1 行を台帳へ追記する（完走でも中断でも・data_manifest 6節）.

    fire-and-forget: 例外で制御を止めない。ローカルへ fallback しない。
    列定義は run_pick_place.LEDGER_COLS を import して使い、実体ヘッダと
    一致しなければ追記しない（本線と同じ追記ガード）。
    """
    try:
        # codex R1 D6: 追記の直前にマウントを再確認（ローカルへ fallback しない）
        if not _ensure_ssd_mounted():
            print("[LEDGER] KIOXIA 未マウント or 書込不可 → CSV スキップ "
                  "（ローカルへは書きません）")
            return

        judgments = log_record.get("judgments") or {}
        pick_v = (judgments.get("pick") or {}).get("human_verdict") or ""
        place_v = (judgments.get("place") or {}).get("human_verdict") or ""

        before = log_record.get("positions_before_replay") or {}
        measure = log_record.get("measure") or {}
        block_xz = before.get("block_xz_m") or measure.get("block_xz_m")
        box_xz = before.get("box_xz_m") or measure.get("box_xz_m")

        final = log_record.get("final_actual") or [None] * 6

        def _fmt_j(v):
            return ("{:.2f}".format(v) if isinstance(v, (int, float)) else "")

        pose_used = log_record.get("pose_used") or {}
        pick_used = pose_used.get("pick_used") or [None] * 6
        place_used = pose_used.get("place_used") or [None] * 6

        teach_name = (log_record.get("teach") or {}).get("file") or ""
        pick_pose_id = "teach:{}:PICK".format(teach_name) if teach_name else ""
        place_pose_id = "teach:{}:PLACE".format(teach_name) if teach_name else ""

        grip = log_record.get("gripper_last_state")

        # 指示どおり、埋めるのは下の列だけ。それ以外は空欄のまま。
        # 例外は recording_* 4 列（79-82）。recording_status は
        # data_manifest 2節で「空欄不可」と定義されており、かつこのスクリプトは
        # 連続記録を残すこと自体が目的なので埋める（--no-record 時は "none"）。
        row = dict((c, "") for c in LEDGER_COLS)
        row.update({
            "timestamp":     log_record.get("ended_at") or datetime.now().isoformat(),
            "run_stem":      run_stem,
            "trial_no":      rpp._ledger_extract_trial_no(run_stem),
            "pick_verdict":  pick_v,
            "place_verdict": place_v,
            "block_pick_x":  _ledger_mm(block_xz, 0),
            "block_pick_z":  _ledger_mm(block_xz, 1),
            "box_x":         _ledger_mm(box_xz, 0),
            "box_z":         _ledger_mm(box_xz, 1),
            "final_j1":      _fmt_j(final[0]),
            "final_j2":      _fmt_j(final[1]),
            "final_j3":      _fmt_j(final[2]),
            "final_j4":      _fmt_j(final[3]),
            "final_j5":      _fmt_j(final[4]),
            "final_j6":      _fmt_j(final[5]),
            "gripper":       ("" if grip is None else str(grip)),
            "run_dir":       str(run_dir),
            "pick_pose_id":  pick_pose_id,
            "pick_used_j0":  _fmt_j(pick_used[0]),
            "pick_used_j1":  _fmt_j(pick_used[1]),
            "pick_used_j2":  _fmt_j(pick_used[2]),
            "pick_used_j3":  _fmt_j(pick_used[3]),
            "pick_used_j4":  _fmt_j(pick_used[4]),
            "pick_used_j5":  _fmt_j(pick_used[5]),
            "place_pose_id": place_pose_id,
            "place_used_j0": _fmt_j(place_used[0]),
            "place_used_j1": _fmt_j(place_used[1]),
            "place_used_j2": _fmt_j(place_used[2]),
            "place_used_j3": _fmt_j(place_used[3]),
            "place_used_j4": _fmt_j(place_used[4]),
            "place_used_j5": _fmt_j(place_used[5]),
        })

        if recording_enabled:
            rec_path, rec_fps, rec_frames, rec_status = _read_recording_meta(run_dir)
        else:
            rec_path, rec_fps, rec_frames, rec_status = "", "", "", "none"
        row.update({
            "recording_path":   rec_path,
            "recording_fps":    rec_fps,
            "recording_frames": rec_frames,
            "recording_status": rec_status,
        })
        log_record["recording_meta_read"] = {
            "recording_path": rec_path, "recording_fps": rec_fps,
            "recording_frames": rec_frames, "recording_status": rec_status,
        }

        is_new = not LEDGER_CSV_PATH.exists()
        if not is_new:
            # 本線と同じ追記ガード: 既存ヘッダと LEDGER_COLS が不一致なら追記しない
            # （不一致のまま append すると列ズレで台帳が汚れる）。
            try:
                with open(str(LEDGER_CSV_PATH), "r", newline="") as fchk:
                    existing_header = next(csv.reader(fchk), None)
            except Exception as e:
                print("[LEDGER] [WARN] ヘッダ読込失敗 ({}). append スキップ".format(e))
                return
            if existing_header != LEDGER_COLS:
                print("[LEDGER] [ABORT-APPEND] 既存ヘッダ ({} 列) と LEDGER_COLS "
                      "({} 列) が不一致 → append スキップ".format(
                          len(existing_header) if existing_header is not None else 0,
                          len(LEDGER_COLS)))
                return
        with open(str(LEDGER_CSV_PATH), "a", newline="") as f:
            w = csv.DictWriter(f, fieldnames=LEDGER_COLS)
            if is_new:
                w.writeheader()
            w.writerow(row)
        print("[LEDGER] append OK: {} (pick={} place={} recording_status={})".format(
            LEDGER_CSV_PATH, pick_v or "''", place_v or "''", rec_status))
    except Exception as e:
        print("[LEDGER] [WARN] append 例外（制御は継続）: {}: {}".format(
            type(e).__name__, e))


# ---- run --------------------------------------------------------------------
def _print_open_gripper_hint():
    print("")
    print("{} [SAFETY] 動きを止めるときはアームの電源を切ること"
          "（Ctrl-C では進行中の移動は止まりません）。".format(LOG_PREFIX))
    print("{} 爪が閉じたままの場合は、次の1行で開きます:".format(LOG_PREFIX))
    print("  python3 -c \"import sys;sys.path.insert(0,'{}');"
          "from visual_servo_pick import daemon_set_gripper;"
          "daemon_set_gripper(0)\"".format(_SCRIPTS_ROOT))


def cmd_run(args):
    global GRIP_VALUE, POWER, SEND_WAIT_ON
    _note_tol_source()

    # 依頼 152: 爪の閉じる値（計画の表示・再生の閉じる指令・試行 JSON がすべてこの値を使う）
    if getattr(args, "grip_value", None) is not None:
        if not (GRIP_VALUE_RANGE[0] <= args.grip_value <= GRIP_VALUE_RANGE[1]):
            _abort_msg("--grip-value は {}〜{} の整数で指定してください（指定: {}）".format(
                GRIP_VALUE_RANGE[0], GRIP_VALUE_RANGE[1], args.grip_value))
            sys.exit(2)
        GRIP_VALUE = int(args.grip_value)
    _log("爪の閉じる値: {}{}".format(
        GRIP_VALUE, "（--grip-value）" if getattr(args, "grip_value", None) is not None else "（既定）"))

    # 依頼 156: 電源の種類（試行 JSON の power に残し、爪の位置と電圧も読む）。動き・待ち時間は変えない
    power = getattr(args, "power", None)
    if power is not None:
        if not POWER_RE.match(power):
            _abort_msg("--power は 2A・5A のように 数字 + A で指定してください（指定: {}）".format(power))
            sys.exit(2)
        if getattr(args, "no_currents", False) or getattr(args, "no_record", False):
            _abort_msg("--power は電流の記録と一緒に使います（--no-currents・--no-record とは併用できません）")
            sys.exit(2)
        _log("電源: {}（--power。試行 JSON の power に残し、爪の位置と電圧も読みます）".format(power))
    POWER = power

    # 依頼 213: 送信のあとの 1.0 秒をどこで待つか（長さは変えない）
    send_wait_on = getattr(args, "send_wait_on", None) or "mac"
    if send_wait_on not in SEND_WAIT_ON_CHOICES:
        _abort_msg("--send-wait-on は {} のどれかで指定してください（指定: {}）".format(
            " / ".join(SEND_WAIT_ON_CHOICES), send_wait_on))
        sys.exit(2)
    SEND_WAIT_ON = send_wait_on
    _log("送信のあとの {:.1f}s の待ち: {}".format(
        SEND_WAIT_SEC,
        "Mac 側（daemon へは wait=0。この間も電流の記録スレッドが読みます）" if SEND_WAIT_ON == "mac"
        else "daemon 側（依頼 202 までと同じ。この間は読みません）"))

    if args.teach_local and args.execute:
        _abort_msg("--teach-local は ドライラン専用です（--execute とは併用できません）。"
                   "実機では Pi から取得した教えた記録だけを再生します")
        sys.exit(2)

    teach_name = os.path.basename(args.teach)
    if teach_name != args.teach or not teach_name:
        _abort_msg("--teach はファイル名だけを渡してください（ディレクトリは "
                   "{} 固定）: {!r}".format(PI_TEACH_DIR, args.teach))
        sys.exit(2)

    dry_run = not args.execute
    recording_enabled = (not args.no_record) and args.execute
    _log("モード: {}{}".format(
        "ドライラン（アームに何も送りません）" if dry_run else "EXECUTE（実機）",
        "" if dry_run else ("  記録: {}".format("ON" if recording_enabled else "OFF (--no-record)"))))

    # KIOXIA は実機 run で必須（JSON・台帳・recorder の置き場）。
    # ドライランでも measure を読むので存在は要る。
    _require_ssd("teach 記録のコピー・試行 JSON・台帳の保存")

    staging_dir = Path(tempfile.mkdtemp(prefix="record_teach_demo_"))
    staged_teach = staging_dir / teach_name
    remote_path = None
    try:
        # ---- 1. 教えた記録の取得 ----
        if args.teach_local:
            src = Path(args.teach_local).expanduser()
            if not src.is_file():
                _abort_msg("--teach-local が見つかりません: {}".format(src))
                sys.exit(1)
            shutil.copy2(str(src), str(staged_teach))
            remote_path = "(local) {}".format(src)
            _warn("ドライラン用に Pi ではなくローカルの教えた記録を読みました: {}".format(src))
        else:
            remote_path = _fetch_teach_from_pi(teach_name, staged_teach)

        teach_sha = _sha256(staged_teach)

        # ---- 2. 姿勢の取り出しと計画（Pi と同じ実装を使う）----
        payload, angles, phases, gripper_events, count = wr._load_teach(str(staged_teach))
        waypoints, sources = wr._extract_waypoints(angles, phases, gripper_events, count)

        _log("入力ファイル: {}".format(teach_name))
        _log("抽出ウェイポイント:")
        for name in wr.PHASE_ORDER:
            print("  {:<14} <- {}".format(name, sources[name]))
            print("  {:<14}    angles = {}".format("", _fmt_angles(waypoints[name])))

        # ---- 肩が上限を超えた姿勢の置き換え（計画段階・送信前）--------------
        # 手で教えた PICK は肩 95.53〜99.22° で、send_angles が 4 回とも捨てられた。
        # 本線は肩 87.74 / 87.00 / 85.00 を送って 647 試行動いている。
        # 爪先位置を保ったまま肩を上限内に収めた姿勢に差し替える。
        # FK テーブルは読むだけ。IK は解かない。send_coords は使わない。
        waypoints_taught = dict((k, list(v)) for k, v in waypoints.items())
        waypoints, shoulder_fix_records = _fix_shoulder_over_max(waypoints)
        _print_shoulder_fix(shoulder_fix_records)
        shoulder_fix_payload = {
            "shoulder_max_deg": SHOULDER_MAX_DEG,
            "shoulder_max_source": "fk_lookup.J1_SAFE_MAX（本線）",
            "j0_tol_deg": SHOULDER_FIX_J0_TOL_DEG,
            "angle_penalty_m_per_deg": SHOULDER_FIX_ANGLE_PENALTY,
            "tip_warn_mm": SHOULDER_FIX_TIP_WARN_MM,
            "fk_table": fkl.FK_CSV.name,
            "fk_table_readonly": True,
            "n_replaced": sum(1 for r in shoulder_fix_records if r.get("applied")),
            "records": shoulder_fix_records,
            "waypoints_taught": dict((k, [round(x, 3) for x in v])
                                     for k, v in waypoints_taught.items()),
            "waypoints_sent": dict((k, [round(x, 3) for x in v])
                                   for k, v in waypoints.items()),
        }

        seq = wpsr._build_segments(waypoints)

        if wpsr._validate_endpoints(seq):
            _abort_msg("JOINT_LIMITS 違反があるため送信せず終了します")
            sys.exit(1)
        _log("JOINT_LIMITS 一括検証 OK（全 endpoint + HOME）")

        bad, n_poses_checked, min_j4_margin = _check_all_poses(seq, waypoints)
        if bad:
            _abort_msg("姿勢検査に通らないため動かす前に終了します")
            sys.exit(1)

        # ---- 3. 計画表示 ----
        # 指示書の手順どおり、計画は位置照合より前に出す（ドライランで Pi 版の
        # 出力と並べて比べられるようにする。カメラ・daemon が無くても出る）。
        current = None
        if not dry_run:
            get_daemon_socket()
            if USE_FAST_DAEMON_CMDS:
                # LOG352: 新 cmd が Pi に配備されているかを、アームを動かさずに確認
                _require_fast_cmds()
            _log("現在姿勢を daemon から取得します")
            for _ in range(5):
                a = daemon_get_angles(timeout_override=DAEMON_GET_TIMEOUT_SEC)
                if _is_valid_angles(a):   # 非有限値も弾く（C1）
                    current = [float(x) for x in a]
                    break
                time.sleep(0.2)
            if current is None:
                _abort_msg("現在姿勢が取得できません（daemon を確認してください）")
                sys.exit(1)
            _log("現在姿勢 = {}".format(_fmt_angles(current)))
            # 2026-10-19 方針変更（項目3）: 実測 j4 の範囲外は **WARN のみ**。
            # ハード帯（±98 / +68）でも止めない。
            if not (J4_HARD_MIN <= current[4] <= J4_HARD_MAX):
                _warn("現在の実測 j4 = {:+.2f} が [{:+.1f}, {:+.1f}] の外です。"
                      "**止めません**（WARN のみ・試行 JSON に残します）"
                      .format(current[4], J4_HARD_MIN, J4_HARD_MAX))
            if not (J4_SAFE_MIN <= current[4] <= J4_SAFE_MAX):
                _warn("現在の実測 j4 = {:+.2f} が J4_SAFE [{:+.1f}, {:+.1f}] の外です"
                      "（指令値の制限なので続行します・記録のみ）".format(
                          current[4], J4_SAFE_MIN, J4_SAFE_MAX))

        summary = _print_plan(current, seq)

        # ---- 4. ブロックが動いていないかの確認 ----
        # 2026-10-19 方針変更（項目4・エイイチ指示）: **ここでは止めない**。
        # カメラのずれ・ブロック検出の失敗・120mm ガード・homography の不良は
        # すべて WARN にして再生を続ける。位置が取れなければ試行 JSON に null。
        # 位置確認はつかみ損ねを防ぐための補助で、アームの安全装置ではない。
        prerun_warnings = []

        def _gate(reason):
            _warn("{}（**止めません**・WARN のみ）".format(reason))
            prerun_warnings.append(str(reason)[:200])
            return False

        move_detail = {"tol_mm": BLOCK_MOVE_TOL_MM}
        now_res = {}
        measure_path, measure_payload = _find_latest_measure(args.measure)
        if measure_path is None:
            _gate("measure が見つかりません（{}/{}*/measure_*.json）。"
                  "先に `record_teach_demo.py measure` を実行してください".format(
                      MEASURE_ROOT, MEASURE_DIR_PREFIX))
        elif measure_payload is None:
            _gate("measure が読めません（壊れている可能性）: {}".format(measure_path))
        else:
            _log("measure: {}".format(measure_path))
            _log("  measured_at = {}  block = {}  box = {}".format(
                measure_payload.get("measured_at"),
                _fmt_xz_mm(measure_payload.get("block_xz_m")),
                _fmt_xz_mm(measure_payload.get("box_xz_m"))))
            m_ts = measure_payload.get("measured_at_ts")
            if isinstance(m_ts, (int, float)):
                age = time.time() - float(m_ts)
                if age > MEASURE_STALE_WARN_SEC:
                    _warn("measure が {:.0f} 分前のものです。ブロックを置き直したなら "
                          "measure をやり直してください".format(age / 60.0))

            # 再生前の確認は **homography を作り直さず画素で比べる**
            # （エイイチ判断 2026-10-19）。四隅の検出が揺れると同じ画素が別の mm に
            # なるため、mm で比べると 37mm の外れ値を掴む。
            now_stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
            _log("再生直前: 上カメラでマット四隅とブロック画素を確認します")
            H_meas = (measure_payload or {}).get("homography")
            # 四隅が measure と合わなければ最大 3 回やり直す（エイイチ判断）。
            # 1 回でも 15px 以内に収まれば、その回の値で続ける。
            insp, corner_attempts = _inspect_until_corners_match(
                (measure_payload or {}).get("mat_corners_px"), "再生直前")

            # 記録用の mm は measure の homography で換算する
            now_res = {}
            if insp and insp.get("block_px") and H_meas:
                bx, bz = _mm_from_px(H_meas, insp["block_px"][0],
                                     insp["block_px"][1])
                now_res["block_xz_m"] = [bx, bz]
            now_res["box_xz_m"] = (measure_payload or {}).get("box_xz_m")
            now_res["n_captures"] = (insp.get("n_frames") if insp else None)
            _log("再生直前: block = {}（measure の homography で換算）".format(
                _fmt_xz_mm(now_res.get("block_xz_m"))))

            check_img = Path(measure_path).parent / "prerun_{}.jpg".format(now_stamp)
            check_img_ok = _save_check_image(
                check_img, "pre-run {}".format(now_stamp),
                now_res.get("block_xz_m"), now_res.get("box_xz_m"),
                H=H_meas, frame=(insp.get("frame") if insp else None))
            now_res["check_image"] = str(check_img) if check_img_ok else None

            # 根元に近すぎるものはブロックとして扱わない
            too_close = _block_too_close_to_base(now_res.get("block_xz_m"))
            if too_close:
                if check_img_ok:
                    _log("何を拾ったかは確認画像で見てください: {}".format(check_img))
                _gate(too_close)

            ok, move_detail, move_reason = _check_camera_and_block_px(
                measure_payload, insp)
            move_detail["mat_corner_attempts"] = corner_attempts
            move_detail["mat_corner_retry_max"] = MAT_CORNER_RETRY_MAX
            # 2026-10-19（エイイチ判断・項目8）: **ブロックのずれが 5mm を
            # 超えても止めない**。差を表示して試行 JSON に残すだけにする。
            # 位置確認はつかみ損ねを防ぐための補助で、アームの安全装置ではない。
            # カメラが動いた / 検出できない / homography が壊れている場合は、
            # 「measure と比べられない」= 位置確認が成立しないので従来どおり止める。
            block_moved_only = (move_detail.get("fail_kind") == "block_moved")
            if not ok and block_moved_only:
                move_detail["gate"] = "warn_only"
                _warn("ブロックが measure 時から動いています（止めません・項目8）: {}"
                      .format(move_reason))
                _warn("  画素の差 {:.1f}px（許容 {:.1f}px = {:.1f}mm）/ "
                      "mm 換算の差 {:.2f}mm".format(
                          move_detail.get("block_shift_px", float("nan")),
                          move_detail.get("tol_px", float("nan")),
                          BLOCK_MOVE_TOL_MM_PX_BASIS,
                          move_detail.get("dist_mm", float("nan"))))
                _warn("  つかみ損ねる可能性があります。"
                      "気になるなら Enter の前に measure を取り直してください")
            if not ok and not block_moved_only:
                # codex R9 R3: 止める前に各回の記録を残す（原因分析に要る）
                move_detail["reject_json"] = _save_reject_json(
                    Path(measure_path).parent, now_stamp, "prerun",
                    move_reason or "", {
                        "measure_path": str(measure_path),
                        "measure_mat_corners_px":
                            (measure_payload or {}).get("mat_corners_px"),
                        "measure_block_px":
                            (measure_payload or {}).get("block_px"),
                        "check_image": now_res.get("check_image"),
                        "detail": move_detail,
                    })
            if not ok and not block_moved_only:
                _gate(move_reason)
            elif ok:
                _log("ブロック位置の照合 OK（画素の差 {:.1f}px <= 許容 {:.1f}px "
                     "= {:.1f}mm / mm 換算の差 {:.2f}mm）".format(
                         move_detail.get("block_shift_px", float("nan")),
                         move_detail.get("tol_px", float("nan")),
                         BLOCK_MOVE_TOL_MM_PX_BASIS,
                         move_detail.get("dist_mm", float("nan"))))
                if move_detail.get("mat_corner_shift_max_px") is not None:
                    _log("マット四隅のずれ 最大 {:.1f}px（上限 {:.1f}px・"
                         "やり直し {} 回）".format(
                             move_detail["mat_corner_shift_max_px"],
                             MAT_CORNER_SHIFT_ABORT_PX, len(corner_attempts)))

        if dry_run:
            _log("ドライランなので、ここで終了します（アームには何も送っていません）")
            _log("Pi 版と比べる点: 抽出ウェイポイント 4 姿勢と各区間の max|Δ| は同じ。"
                 "送り方は単発（Pi 版は 0.1s 補間ストリーミング）なので "
                 "n_steps / ticks は比較対象外。送信 {} 回 / 想定所要 ≈ {:.1f}s".format(
                     summary["n_moves"] + 1, summary["estimated_sec"]))
            return 0

        print("{} [SAFETY] 再生中はグリッパーが実機で開閉します。爪に指を挟まないこと。"
              .format(LOG_PREFIX))
        # codex R3 C4: Ctrl-C は「新しい送信をやめる」だけで、すでに daemon へ
        # 渡った命令や進行中の移動は止まらない。実機の停止手段は電源を切ること
        # （2026-10-18 からの運用）。
        print("{} [SAFETY] 止めるときはアームの電源を切ること。"
              "Ctrl-C は新しい送信をやめるだけで、進行中の移動は止まりません。"
              .format(LOG_PREFIX))
        # 2026-10-19（エイイチ判断・項目7）: 開始前の「爪は開いているか」の y/n は
        # 削除した（本線 run_pick_place.py にも同じ確認は無く、STEP 2 の open が
        # PICK の前に必ず入る）。開いたかの自動確認はしない（gv は使わない）ので、
        # 判定は終了後の人の y/n と動画だけになる。
        print("{} [SAFETY] 開いたかの自動確認はしません（gv は使わない）。"
              "STEP 2 で open を送ってから PICK へ降ります。".format(LOG_PREFIX))
        print("{} 周囲に手・物が無いか確認してください。OK なら Enter で開始".format(LOG_PREFIX))
        try:
            input()
        except (EOFError, KeyboardInterrupt):
            print("")
            _abort_msg("開始前に中止しました（アームには何も送っていません）")
            sys.exit(1)

        first_max_diff = _max_axis_diff_finite(HOME_ANGLES, current)
        if first_max_diff is None:
            _abort_msg("現在姿勢と HOME の差を有限な数値として計算できません: "
                       "current={!r}".format(current))
            sys.exit(1)
        if first_max_diff > FIRST_MOVE_WARN_DEG:
            _warn("先頭移動の最大軸差 {:.2f}° > {:.1f}°。急加速の可能性があります".format(
                first_max_diff, FIRST_MOVE_WARN_DEG))
            print("{} 続行するには 'yes' と入力してください（それ以外は中止）".format(LOG_PREFIX))
            try:
                ans = input().strip()
            except (EOFError, KeyboardInterrupt):
                print("")
                ans = ""
            if ans != "yes":
                _abort_msg("中止しました（差分が大きいため・アームには何も送っていません）")
                sys.exit(1)

        # ---- 5. 試行フォルダの用意（データと対のコード原則）----
        # codex R1 D6: 保存の直前にマウントを再確認する（Enter 待ちの間に
        # SSD が外れていたら、ローカル側にディレクトリを作らせず失敗させる）。
        _require_mount_now("試行フォルダの作成")
        # codex R1 B4: 競合しない採番（mkdir(exist_ok=False) → 取れなければ次の番号）
        run_dir, run_stem = _next_run_dir()
        log_path = run_dir / (run_stem + ".json")
        teach_dir = run_dir / "teach"
        teach_dir.mkdir(parents=True, exist_ok=True)
        teach_copy = teach_dir / teach_name
        shutil.copy2(str(staged_teach), str(teach_copy))
        _log("試行フォルダ: {}".format(run_dir))
        _log("教えた記録のコピー: {}".format(teach_copy))

        log_record = {
            "script": _THIS.name,
            "source": "teach_replay",
            "run_stem": run_stem,
            "run_dir": str(run_dir),
            "started_at": datetime.now().isoformat(),
            "recording_enabled": recording_enabled,
            # 依頼 152: 爪の閉じる値（--grip-value。付けなければ 20）
            "grip_value": GRIP_VALUE,
            # 依頼 156: 電源の種類（--power。付けなければ None）
            "power": POWER,
            # 依頼 213: 送信のあとの 1.0 秒をどこで待ったか（mac = Mac 側・電流の記録スレッドが読める /
            # daemon = 依頼 202 までと同じ）
            "send_wait_on": SEND_WAIT_ON,
            # 肩が上限を超えた姿勢の置き換え（計画段階で決まったもの）
            "shoulder_fix": shoulder_fix_payload,
            "teach": {
                "file": teach_name,
                "remote_path": remote_path,
                "local_copy": str(teach_copy),
                "sha256": teach_sha,
                "schema_version": payload.get("schema_version"),
                "n_angles": count,
                # 置き換え後（= 実際に送る）姿勢。教えた値は shoulder_fix に残す
                "waypoints": dict((k, list(v)) for k, v in waypoints.items()),
                "waypoints_taught": dict((k, list(v))
                                         for k, v in waypoints_taught.items()),
                "waypoint_sources": dict(sources),
            },
            "measure": {
                "path": str(measure_path),
                "measured_at": measure_payload.get("measured_at"),
                "block_xz_m": measure_payload.get("block_xz_m"),
                "box_xz_m": measure_payload.get("box_xz_m"),
                "detect_score": measure_payload.get("detect_score"),
            },
            "positions_before_replay": {
                "block_xz_m": now_res.get("block_xz_m"),
                "box_xz_m": now_res.get("box_xz_m"),
                "detect_score": now_res.get("detect_score"),
                "n_captures": now_res.get("n_captures"),
                "resolved_top_index": now_res.get("resolved_top_index"),
                "check_image": now_res.get("check_image"),
                "block_dist_from_base_mm": (
                    round(_dist_from_base_mm(now_res.get("block_xz_m")), 1)
                    if _dist_from_base_mm(now_res.get("block_xz_m")) is not None
                    else None),
                "block_min_dist_from_base_mm": BLOCK_MIN_DIST_FROM_BASE_MM,
            },
            "block_move_check": move_detail,
            # 2026-10-19 方針変更（項目4）: 再生前の確認で止めずに続けた事象
            "prerun_warnings": list(prerun_warnings),
            # グリッパー確認の実施状況（後から、どう確認した試行か分かるように）
            "gripper_check": {
                # gv（get_gripper_value）は使わない。2026-10-19 の段階C で
                # 読み取りの直後にシリアル同期が壊れた疑いがあるため外した。
                "method": "open_twice_no_readback",
                "verified_by": "human_y_n",
                # 自動確認をしないので True/False は付けない（None のまま）。
                # build_demo_episodes.py が除外するのは verified が False の試行だけ
                # なので、None なら除外されない。実際に開いたかは人の judgments
                #（pick / place）と動画で判断する。
                "verified": None,
                # 項目7: 開始前の目視 y/n は削除した（本線と同じく確認しない）
                "first_open_visual_confirmed": None,
                # codex R10 S3: open を 2 回送るが、recorder へ出す時刻は
                # 1 回目で固定している（どちらで開いたかは記録に無い）。
                "label_time_assumes_first_attempt": True,
                "open_pre_sleep_sec": 0.0,
                "open_dwell_sec": GRIPPER_OPEN_DWELL_SEC,
                "open_attempts": GRIPPER_OPEN_ATTEMPTS,
            },
            "current_angles_before_replay": list(current),
            "plan": dict(summary, segments=[
                {
                    "kind": s["kind"],
                    "action": s.get("action"),
                    "from_name": s.get("from_name"),
                    "to_name": s.get("to_name"),
                    "critical": bool(s.get("critical")),
                    "max_delta_deg": (round(float(s["max_delta"]), 3)
                                      if "max_delta" in s else None),
                }
                for s in seq
            ]),
            "constants": {
                "HOME_ANGLES": list(HOME_ANGLES),
                "SPEED_FIRST": SPEED_FIRST,
                "SPEED_PLAY": SPEED_PLAY,
                "FIRST_MOVE_SETTLE_SEC": FIRST_MOVE_SETTLE_SEC,
                "SPEED_SINGLE": SPEED_SINGLE,
                "motion_mode": "single_send_per_segment",
                "GRIP_VALUE": GRIP_VALUE,
                "GRIPPER_SPEED": GRIPPER_SPEED,
                "GRIPPER_DWELL_SEC": GRIPPER_DWELL_SEC,
                "ARRIVAL_TOL_BY_SEGMENT": {k: v[0] for k, v
                                           in ARRIVAL_TOL_BY_SEGMENT.items()},
                "ARRIVAL_TIMEOUT_MIN_SEC": ARRIVAL_TIMEOUT_MIN_SEC,
                "RETRY_PAUSE_SEC": RETRY_PAUSE_SEC,
                "RETRY_NO_MOVE_THRESHOLD_DEG": RETRY_NO_MOVE_THRESHOLD_DEG,
                "SEND_FAIL_ABORT_STREAK": SEND_FAIL_ABORT_STREAK,
                "J4_SAFE": [J4_SAFE_MIN, J4_SAFE_MAX],
                "poses_checked": n_poses_checked,
                "min_j4_margin_deg": (round(min_j4_margin, 3)
                                      if min_j4_margin is not None else None),
                "J4_MARGIN_WARN_DEG": J4_MARGIN_WARN_DEG,
                "J4_HARD_ABORT_BAND": [J4_HARD_MIN, J4_HARD_MAX],
                "BLOCK_MOVE_TOL_MM": BLOCK_MOVE_TOL_MM,
                "DAEMON_SEND_TIMEOUT_SEC": DAEMON_SEND_TIMEOUT_SEC,
                "DAEMON_GET_TIMEOUT_SEC": DAEMON_GET_TIMEOUT_SEC,
                "USE_FAST_DAEMON_CMDS": USE_FAST_DAEMON_CMDS,
                "DAEMON_GET_ONCE_TIMEOUT_SEC": DAEMON_GET_ONCE_TIMEOUT_SEC,
                "GRIPPER_OPEN_ATTEMPTS": GRIPPER_OPEN_ATTEMPTS,
                "GRIPPER_OPEN_POST_SLEEP_SEC": GRIPPER_OPEN_POST_SLEEP_SEC,
                "GRIPPER_OPEN_DWELL_SEC": GRIPPER_OPEN_DWELL_SEC,
                "DAEMON_SET_GRIPPER_PRE_SLEEP_SEC": DAEMON_SET_GRIPPER_PRE_SLEEP_SEC,
                "gripper_open_check": "none (gv unused / no pre-start y/n)",
                "SEND_WAIT_SEC": SEND_WAIT_SEC,
                "send_wait_on": SEND_WAIT_ON,
                "send_socket_timeout_sec": SEND_SOCKET_TIMEOUT_SEC,
                "send_socket_timeout_source": (
                    "visual_servo_pick._daemon_request: wait>0 なら wait+3.0。"
                    "timeout_override は短縮しかできないので渡していない"
                    if SEND_WAIT_ON == "daemon" else
                    "依頼 213: daemon へは wait=0 で送り、SEND_WAIT_SEC は Mac 側で待つ。"
                    "期限は daemon で待っていたときと同じ wait+3.0 を timeout_override で渡す"),
                "GRIPPER_STILL_TOL_DEG": GRIPPER_STILL_TOL_DEG,
                "GRIPPER_STILL_READS": GRIPPER_STILL_READS,
                "GRIPPER_STILL_TIMEOUT_SEC": GRIPPER_STILL_TIMEOUT_SEC,
                "J4_HARD_MARGIN_DEG": J4_HARD_MARGIN_DEG,
                "arrival_tol_source": (
                    "run_pick_place.py の stage 値を import"
                    "（TOL_HOME 2.5 / TOL_APPROACH 2.0 / TOL_PICK_PLACE 3.0 / "
                    "TOL_APPROACH_PLACE 3.0 / TOL_LIFT_BACK 3.0）。"
                    "一律 8.0（Pi 配備版 waypoint_streaming_replay.py L69）は"
                    "2026-10-19 にやめた"),
            },
        }

        # ---- 6. 記録開始 → 再生 ----
        # codex R1 D5: ここから先は「何が起きても試行 JSON と台帳に 1 行残す」。
        # trial_start 直後の待ち時間中の Ctrl-C も同じ扱いに含める。
        #
        # codex R2 D5: 試行フォルダを確保したあとの全処理を **外側の try/finally** で
        # 囲み、その finally から「試行 JSON の保存」と「台帳 1 行」をそれぞれ独立に
        # 1 回だけ試みる。trial_end() 中の Ctrl-C や、表示処理の例外でも保存に到達する。
        result = "error"
        hint_shown = False
        recorder = None
        saved = {"json": False, "ledger": False}

        def _finalize():
            """試行 JSON と台帳を、それぞれ独立に 1 回だけ保存する.

            二重記帳しないよう `saved` フラグで番をする（この関数は finally から
            1 回呼ばれるだけだが、将来の経路追加でも二重にならないようにする）。
            """
            if not saved["json"]:
                saved["json"] = True   # 試行済みを先に立てる（再入しない）
                try:
                    _require_mount_now("試行 JSON の保存")   # D6
                    _atomic_write_json(log_path, log_record)
                    saved["json"] = "ok"
                    _log("試行 JSON: {}".format(log_path))
                except BaseException as e:
                    _warn("試行 JSON の保存に失敗しました（台帳追記は続けます）: "
                          "{}: {}".format(type(e).__name__, e))
            if not saved["ledger"]:
                saved["ledger"] = True
                try:
                    # 台帳は完走でも中断でも 1 行（data_manifest_rules 6節）
                    _append_ledger(log_record, run_stem, run_dir, recording_enabled)
                    saved["ledger"] = "ok"
                except BaseException as e:
                    print("[LEDGER] [WARN] append 呼び出しで例外: {}: {}".format(
                        type(e).__name__, e))

        try:
            try:
                recorder = _Recorder(recording_enabled, run_stem)
                recorder.trial_start()
                if recording_enabled:
                    _log("recorder に trial_start を送りました。{:.1f}s 待ってから"
                         "動き出します（mp4 writer が開くまで）".format(
                             RECORDER_START_SETTLE_SEC))
                    # 読み取りはまだ始めない（recorder の writer が開くのを待つだけ）
                    time.sleep(RECORDER_START_SETTLE_SEC)
                    # 依頼 121: 爪と肩の電流を {run}/recording/currents.csv に残す（再生の間ずっと）
                    if not getattr(args, "no_currents", False):
                        recorder.start_current_actuals()   # 依頼 202
                        recorder.currents = _start_currents(
                            run_dir, angle_sink=recorder.actual_from_currents)
                    else:
                        _log("--no-currents: 爪と肩の電流は残しません")

                result = _replay(seq, recorder, log_record)
            except BaseException as e:
                # _replay は自前で全例外を握るので、ここに来るのは trial_start や
                # settle 中の Ctrl-C / 例外。中断として確定させる。
                print("")
                _abort_msg("再生の開始前後で中断しました: {}: {}".format(
                    type(e).__name__, e))
                result = ("interrupted" if isinstance(e, KeyboardInterrupt)
                          else "error")
                log_record["result"] = result
                log_record["abort"] = {
                    "stage": "before_replay",
                    "reason": "{}: {}".format(type(e).__name__, e),
                    "traceback": traceback.format_exc(),
                }
            finally:
                log_record.setdefault("result", result)
                log_record["ended_at"] = datetime.now().isoformat()
                # codex R1 C5 / R2 C5: 爪を開く1行は、非完走のときの
                # **いちばん最初**に出す（人の判定入力・保存より前）。
                # trial_end より先に出す（trial_end 中の Ctrl-C でも案内が残る）。
                # gripper の指令が失敗した場合は、その場で ABORT する作りに
                # 変えた（2026-10-19）ので result が completed にならない。
                if log_record.get("result") != "completed":
                    _print_open_gripper_hint()
                    hint_shown = True
                if recorder is not None and recorder.currents is not None:
                    # 依頼 121: trial_end より先に止める（電流の記録は再生の間だけ）。
                    # ここで止まりきらなくても、cmd_run の最後の finally（socket を閉じる前）で
                    # もう一度スレッドの終わりを待つ（_halt_currents）。
                    try:
                        log_record["currents"] = recorder.currents.stop()
                    except BaseException as e:
                        _warn("電流の記録を閉じるときに例外（続行）: {}: {}".format(
                            type(e).__name__, e))
                if recorder is not None:
                    try:
                        recorder.close_current_actuals()   # 依頼 202: 積んだ通知を trial_end の前に送り切る
                    except BaseException as e:
                        _warn("電流の記録の角度の通知を閉じるときに例外（続行）: {}: {}".format(
                            type(e).__name__, e))
                    log_record["recorder_actual_events_sent"] = recorder.actual_sent
                    # 依頼 202: 電流の記録スレッドの角度で送った actual 行（上の数には入れない）
                    log_record["recorder_actual_events_sent_current_thread"] = (
                        recorder.actual_sent_current_thread)
                    log_record["recorder_actual_current_thread_late"] = (
                        recorder.actual_current_thread_late)
                    log_record["recorder_actual_current_thread_errors"] = (
                        recorder.actual_current_thread_errors)
                    log_record["recorder_state_ts_backwards"] = recorder.state_ts_backwards
                    log_record["recorder_current_actuals_unsent"] = recorder.current_actuals_unsent
                    log_record["recorder_current_actuals_flush_timeout"] = (
                        recorder.current_actuals_flush_timeout)
                    log_record["recorder_send_failures"] = recorder.send_failures
                    log_record["recorder_last_send_error"] = (
                        recorder.last_send_error)
                    try:
                        recorder.trial_end()
                    except BaseException as e:
                        _warn("recorder への trial_end 通知で例外（続行）: {}: {}".format(
                            type(e).__name__, e))
                    # 依頼 202: 送り手が送り終えた数（trial_end の直後の値）と、trial_end も送り手に任せたか（ふつうは False）
                    log_record["recorder_state_rows_sent_by_sender"] = recorder.state_rows_sent_by_sender
                    log_record["recorder_trial_end_via_sender"] = recorder.trial_end_via_sender

            result = log_record.get("result") or result
            print("=" * 78)
            _log("再生 {}".format({"completed": "完了",
                                   "aborted": "中断（ABORT）",
                                   "interrupted": "中断（Ctrl-C）",
                                   "error": "中断（予期しない例外）"}.get(result, result)))
            print("=" * 78)

            # ---- 7. 人の判定（完走でも中断でも聞く。中断も台帳に残す）----
            try:
                if result != "completed":
                    _warn("中断した試行です。判定は見たとおりに答えてください"
                          "（学習には pick/place どちらも yes の試行だけが使われます）")
                pick_verdict = _ask_yes_no("つかめたか")
                place_verdict = _ask_yes_no("箱に入ったか")
            except BaseException as e:
                _warn("判定の入力中に中断しました（unknown として記録します）: "
                      "{}: {}".format(type(e).__name__, e))
                pick_verdict = place_verdict = "unknown"
            log_record["judgments"] = {
                "pick": {"human_verdict": pick_verdict, "verdict": pick_verdict,
                         "source": "human"},
                "place": {"human_verdict": place_verdict, "verdict": place_verdict,
                          "source": "human"},
            }
        finally:
            # ---- 8〜9. 試行 JSON → 台帳（何が起きても 1 回だけ）----
            _finalize()

        if saved["json"] != "ok":
            _warn("試行 JSON が保存できていません。build_demo_episodes.py は "
                  "この試行を missing として飛ばします")
        _log("次に進むには: python3 {}/pi05/build_demo_episodes.py --run {} "
             "--dataset mecharm_redblock_box_teach_v1".format(_SCRIPTS_ROOT, run_stem))
        if not hint_shown and result != "completed":
            _print_open_gripper_hint()
        ok = (result == "completed")
        return 0 if ok else 1

    finally:
        _halt_currents()   # 依頼 121: 電流の記録スレッドを止めてから socket を閉じる
        try:
            close_daemon_socket()
        except Exception:
            pass
        shutil.rmtree(str(staging_dir), ignore_errors=True)


# ---- CLI --------------------------------------------------------------------
def main():
    ap = argparse.ArgumentParser(
        description="手で教えた動きを daemon 経由で再生しながら π0.5 のお手本として記録する")
    sub = ap.add_subparsers(dest="cmd")

    ap_m = sub.add_parser("measure", help="上カメラでブロックと箱の位置を測って保存する")
    ap_m.add_argument("--repeat", type=int, default=MEASURE_REPEAT_DEFAULT,
                      help="検出回数（中央値集約・既定 {}）".format(MEASURE_REPEAT_DEFAULT))
    ap_m.add_argument("--interval", type=float, default=MEASURE_INTERVAL_DEFAULT,
                      help="検出間隔 秒（既定 {}）".format(MEASURE_INTERVAL_DEFAULT))
    ap_m.add_argument("--log", type=int, default=None,
                      help="保存先を teach_poses_LOG{NNN}/ にする（例: --log 352）。"
                           "省略時は teach_poses_{YYYYMMDD}/（LOG 番号を推測しない）")
    ap_m.set_defaults(func=cmd_measure)

    ap_r = sub.add_parser("run", help="教えた記録を再生しながら記録する")
    ap_r.add_argument("--teach", required=True,
                      help="Pi の {}/ にある teach JSON のファイル名".format(PI_TEACH_DIR))
    ap_r.add_argument("--execute", action="store_true",
                      help="実機に送る（未指定はドライラン＝計画表示のみ）")
    ap_r.add_argument("--no-record", action="store_true",
                      help="recorder に通知しない（段階C の検証用）")
    ap_r.add_argument("--no-currents", action="store_true",
                      help="爪と肩の電流（currents.csv。依頼 121）を残さない。"
                           "電流の読み取りを一切せず、依頼 121 の前と同じ動きになる")
    ap_r.add_argument("--measure", default=None,
                      help="使う measure JSON を明示する（既定は直近を自動で探す）")
    ap_r.add_argument("--grip-value", type=int, default=None,
                      help="爪の閉じる値（既定 {}。試行 JSON の grip_value に残す。依頼 152）".format(GRIP_VALUE))
    ap_r.add_argument("--power", default=None,
                      help="電源の種類（例 2A / 5A。試行 JSON の power に残し、爪の位置と電圧の番地も "
                           "recording/power_regs.csv に読む。動き・待ち時間は変えない。依頼 156。pi05/POWER_AB.md）")
    ap_r.add_argument("--send-wait-on", choices=SEND_WAIT_ON_CHOICES, default="mac",
                      help="送信のあとの {:.1f} 秒をどこで待つか（既定 mac: daemon へは wait=0 で送り Mac 側で待つ。"
                           "この間も電流の記録スレッドが角度を読む。daemon: 依頼 202 までと同じ）。"
                           "待つ長さは変えない。試行 JSON の send_wait_on に残す。依頼 213".format(SEND_WAIT_SEC))
    ap_r.add_argument("--repeat", type=int, default=MEASURE_REPEAT_DEFAULT,
                      help="再生直前の検出回数（既定 {}）".format(MEASURE_REPEAT_DEFAULT))
    ap_r.add_argument("--interval", type=float, default=MEASURE_INTERVAL_DEFAULT,
                      help="再生直前の検出間隔 秒（既定 {}）".format(MEASURE_INTERVAL_DEFAULT))
    ap_r.add_argument("--teach-local", default=None,
                      help="【ドライラン専用】Pi ではなくローカルの teach JSON を読む "
                           "（Pi 不在でも計画を比べられるようにするため。--execute とは併用不可）")
    ap_r.set_defaults(func=cmd_run)

    args = ap.parse_args()
    if not getattr(args, "func", None):
        ap.print_help()
        return 2
    try:
        return args.func(args)
    except KeyboardInterrupt:
        print("")
        _warn("Ctrl-C で終了しました")
        _print_open_gripper_hint()
        return 130


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