#!/usr/bin/env python3
"""policy_recorder.py — 実機推論 (run_policy_client.py --execute) の間, 上カメラと実測角を 1 秒 10 コマで残す (2026-10-29).

お手本 (record_teach_demo.py + recorder.py --top) と同じ形で書くので,
build_demo_episodes.build_one() がそのまま読める:
  {run_dir}/recording/states.csv          recorder.py と同じ列. actual / gripper_cmd / stage 行.
                                          stage 列 = その時の状態 (model / rewind / correct / home / wait)
  {run_dir}/recording/top.mp4             上カメラ (元の大きさ. 10 コマ/秒)
  {run_dir}/recording/top_frame_log.csv   frame_idx, capture_ts (+ mode 列. 1 コマごとの状態)
  {run_dir}/recording/top_meta.json       コマ数・実効 fps など

アームへは何も送らない (関節角を読むだけ). daemon のソケットは 1 本を本体と共用するので,
install_daemon_lock() で 1 回のやりとりごとに鍵をかける (読み書きが混ざらないように).

2026-10-31 依頼 46: 送っている間の角度の抜けについて
  daemon は send_angles を受けると, 送ったあとに角度を読み直す (動いている間は読めずにやり直す) まで
  返事をしない (約 1.6 秒. 爪は約 3.6 秒). daemon は 1 本のつながりを 1 件ずつ処理するので, その間は
  記録スレッドも角度を読めない. 腕はこの間に動き終わる (Log17 の記録で 1.5〜3.6 秒の抜け).
  記録の側で直せるのは次の 2 つだけ (送る指令・到着の判定・待ち時間は変えない):
    - 本体が読んだ角度 (到着の確かめ・送る直前の読み) も actual 行に残す (note=main)
    - daemon とのやりとり 1 回ごとの始まりと長さを残す (send_angles 行 / rpc 行). 送った目標と時刻が
      分かるので, あとから build_correction_episodes.py --gap-fill speed20 で抜けを埋められる
  抜けそのものをなくすには daemon の送ったあとの読み直しを変える必要がある (決めるのはエイイチ).

2026-11-07 依頼 116: 爪と肩の電流も残す
  記録スレッドは get_angles の代わりに daemon の read_servo (角度 + サーボの番地. 読むだけ) を 1 回呼ぶ
  (daemon への問い合わせの回数は今と同じ). states.csv の列と行は変えない (actual 行は今までどおり).
  同じ時刻の電流は {run_dir}/recording/currents.csv に書く (列は CURRENTS_COLS. ts は states.csv の actual 行と同じ値.
  角度を読んだ時刻 = 返事が着いた時刻 - daemon の中で角度のあとにかかった時間. rpc_ms は鍵の待ちも含む 1 回の長さ).
    番地 69 が下位・70 が上位. 上位の 128 が向き, 残りが大きさ:
      符号付きの値 = (70 & 128 なら -1) × (69 + 256 × (70 & 127))
    爪 (7) の 69/70 と肩 (2) の 69 は毎回読む. 肩の 70 は SHOULDER_HI_EVERY 回に 1 回だけ読む
    (11-07 の測り (diag/servo_current_20261107/180051_pad) で 番地 1 つ 約 20 ms・角度 約 18 ms.
     4 つを毎回読むと 1 回 約 105 ms で 10 Hz に収まらない. 3 つなら 約 85 ms).
    読まなかった番地・読めなかった番地は空. 肩の符号付きの値は 69 と 70 を同じ回に読めたときだけ書く.
  読めなかった回 (例外・返事なし) は値を空にして続ける. daemon に read_servo がなければ (unknown cmd)
  その run は get_angles に戻して角度だけ残す (currents.csv は列名だけ).
  daemon の時間の約 85% を記録が使うので, 同じプロセスのパッド・本体の命令は鍵を 平均 35 ms・最大 約 110 ms
  (読み取り 1 回分) 待つ. 読んだあとは YIELD_SEC 鍵を手放す (すぐ取り直さない. 先に通す保証ではない).
  read_servo が答えないときは READ_SERVO_TIMEOUT_SEC で切ってつなぎ直す (その間の stop は 約 2 秒待つ).
"""
import csv
import json
import math
import threading
import time
from pathlib import Path

import cv2

import visual_servo_pick as vsp

RECORD_FPS = 10.0
FRAME_INTERVAL = 1.0 / RECORD_FPS
ANGLE_INTERVAL = 1.0 / RECORD_FPS
FOURCC = "avc1"                 # recorder.py と同じ
STATES_COLS = ["ts", "event_type", "stage",
               "j0", "j1", "j2", "j3", "j4", "j5",
               "speed", "gripper_state",
               "ts_send", "ts_arrival", "attempt", "arrived",
               "note"]          # recorder.py の STATES_COLS と同じ
FRAME_LOG_COLS = ["frame_idx", "capture_ts", "mode"]
FLUSH_INTERVAL = 1.0
YIELD_SEC = 0.005               # 記録スレッドが読んだあと, 次を読むまでに少なくともあける時間 (依頼 116)
STOP_JOIN_SEC = 5.0             # stop() でスレッドの終わりを待つ上限
FRAME_WAIT_SEC = 2.0            # 本体が画像を求めたとき, 新しいコマを待つ上限
# 依頼 116: 電流 (サーボの番地 69 下位 / 70 上位). 7 = 爪, 2 = 肩
CURRENT_REGS = [(7, 69), (7, 70), (2, 69), (2, 70)]   # currents.csv の生の値の列の順
EVERY_REGS = [(7, 69), (7, 70), (2, 69)]              # 毎回読む
SHOULDER_HI_REG = (2, 70)
SHOULDER_HI_EVERY = 5           # 肩の上位 (向き) はこの回数に 1 回 (1 秒 2 回. 毎回読むと 10 Hz に収まらない)
CURRENTS_COLS = ["ts", "rpc_ms", "pi_ms",
                 "r7_69", "r7_70", "r2_69", "r2_70",
                 "gripper_current", "shoulder_current", "note"]


_rpc_observers = []             # daemon とのやりとり 1 回ごとに呼ぶ関数 (記録用. 依頼 46)


def add_rpc_observer(fn):
    if fn not in _rpc_observers:
        _rpc_observers.append(fn)


def remove_rpc_observer(fn):
    if fn in _rpc_observers:
        _rpc_observers.remove(fn)


def _notify_rpc(cmd_obj, resp, t0, t1, err):
    for fn in list(_rpc_observers):
        try:
            fn(cmd_obj, resp, t0, t1, err)
        except Exception:
            pass                # 記録の失敗で本体を止めない


def install_daemon_lock():
    """visual_servo_pick._daemon_request を「1 回のやりとりごとに鍵をかける」版に差し替える.

    daemon_get_angles / daemon_send_angles / daemon_set_gripper* はどれも
    モジュールの _daemon_request を呼ぶので, ここを差し替えれば全部に効く.
    記録スレッドの get_angles と本体の send_angles が同じソケットで混ざらないようにする.
    中身 (送る内容・タイムアウト) は変えない. 2 回呼んでも 1 回だけ差し替える.
    依頼 46: やりとりが終わったら (鍵を外してから) add_rpc_observer で足した関数に
    (送った dict, 返事 or None, 始めた時刻, 終わった時刻, 例外 or None) を渡す. 返事と例外はそのまま返す.
    再接続のときの入れ子の ping (外のやりとりの鍵の中) は渡さない (codex 推奨).
    観察は同じスレッドで呼ぶ (states.csv に 1 行書くだけ. 本体へ返るのがその分だけ遅れる).
    """
    if getattr(vsp._daemon_request, "_policy_locked", False):
        return
    orig = vsp._daemon_request
    lock = threading.RLock()
    depth = threading.local()       # 再接続の中の ping (鍵の中で呼ばれる入れ子) は観察に渡さない

    def _locked(*a, **k):
        cmd_obj = a[0] if a else k.get("cmd_obj")
        resp, err, t0, t1 = None, None, None, None
        nested = getattr(depth, "n", 0) > 0
        try:
            depth.n = getattr(depth, "n", 0) + 1
            with lock:
                t0 = time.time()
                try:
                    resp = orig(*a, **k)
                finally:
                    t1 = time.time()
        except BaseException as e:
            err = e
            raise
        finally:
            depth.n -= 1
            if _rpc_observers and t0 is not None and not nested:
                _notify_rpc(cmd_obj, resp, t0, t1, err)
        return resp
    _locked._policy_locked = True
    _locked._orig = orig
    vsp._daemon_request = _locked


class _ServoCurrents:
    """read_servo で角度と爪・肩の番地を 1 回読み, currents.csv に 1 行書く部品 (依頼 116).
    実機推論の記録 (PolicyRecorder) と教えた動きの再生の記録 (CurrentsRecorder. 依頼 121) の両方が使う
    (読み方を 2 か所に書かない). 使う側が持つもの: read_servo_fn・read_angles_fn・currents_mode・n_reads・
    read_ms・n_currents・n_currents_fail・currents_write_errors・cur_f・cur_w・csv_lock・_valid()・_maybe_flush()."""

    NO_READ_SERVO_MSG = "電流は残さず, 角度だけ記録します"

    def _servo_cmd(self):
        regs = list(EVERY_REGS)
        if self.n_reads % SHOULDER_HI_EVERY == 0:
            regs.append(SHOULDER_HI_REG)
        return {"cmd": "read_servo", "angles": True, "currents": False, "status": False,
                "gripper": False, "regs": [list(r) for r in regs], "raw": False}

    def _read_once(self):
        """角度を 1 回読む. read_servo のときは同じ回の番地を currents.csv に書く.
        戻り値は (角度 or None, 角度を読んだ時刻).
        read_servo の daemon は角度を先に読み, そのあと番地を読む. 返事が着いた時刻のままだと角度の時刻が
        番地の分 (60〜80 ms) 遅れるので, daemon の中で角度より後にかかった時間 (total_ms - angles の ms) を引く
        (帰りの通信の分 約 2 ms は引かない. codex R1 必須 1). 引けないとき (返事の形が違う) は着いた時刻."""
        if self.currents_mode != "read_servo":
            return self._read_angles_only()
        cmd_obj = self._servo_cmd()
        self.n_reads += 1
        t0 = time.time()
        resp, err = None, None
        try:
            resp = self.read_servo_fn(cmd_obj)
        except Exception as e:
            err = e
        t1 = time.time()
        if err is not None and "unknown cmd" in str(err):
            # daemon が古い (read_servo がない): この run は角度だけにする
            self.currents_mode = "get_angles_fallback"
            print(f"[RECORD] [WARN] daemon に read_servo がありません. {self.NO_READ_SERVO_MSG} "
                  "(tools/daemon_add_read_servo.sh)")
            return self._read_angles_only()
        self.read_ms.append((t1 - t0) * 1000.0)
        a, a_note, ts = None, "", t1
        if err is None and isinstance(resp, dict):
            it = (resp.get("items") or {}).get("angles") or {}
            v = it.get("value")
            try:
                if v is not None and self._valid(v):
                    a = [float(x) for x in v]
            except Exception:
                a = None
            a_note = "" if a is not None else ("angles=no_reply" if not v else "angles=invalid")
            try:
                after = (float(resp["total_ms"]) - float(it["ms"])) / 1000.0
                if 0.0 <= after <= t1 - t0:
                    ts = t1 - after
            except (KeyError, TypeError, ValueError):
                pass
        try:
            self._current_row(ts, t0, t1, cmd_obj, resp, err, a_note)
        except Exception as e:
            # 電流の行が書けなくても角度の記録は続ける (codex R1 必須 2). 数を top_meta.json に残す
            self.currents_write_errors += 1
            if self.currents_write_errors <= 3:
                print(f"[RECORD] [WARN] currents.csv に書けません: {type(e).__name__}: {e}")
        return a, ts

    def _read_angles_only(self):
        try:
            a = self.read_angles_fn()
        except Exception:
            a = None
        return a, time.time()

    def _current_row(self, ts, t0, t1, cmd_obj, resp, err, a_note):
        asked = {tuple(r) for r in cmd_obj["regs"]}
        got, notes = {}, []
        if err is not None:
            notes.append(f"error={type(err).__name__}: {str(err)[:80]}")
        elif not isinstance(resp, dict):
            notes.append("error=bad_response")
        else:
            for r in resp.get("regs") or []:
                try:
                    sid, addr, val = int(r[0]), int(r[1]), r[2]
                except Exception:
                    continue
                if (sid, addr) not in asked:
                    continue
                if isinstance(val, int) and not isinstance(val, bool) and 0 <= val <= 255:
                    got[(sid, addr)] = val
            missing = [f"{s}:{a}" for s, a in CURRENT_REGS if (s, a) in asked and (s, a) not in got]
            if missing:
                notes.append("no_reply=" + ",".join(missing))
            if a_note:
                notes.append(a_note)
        pi_ms = resp.get("total_ms") if err is None and isinstance(resp, dict) else None
        row = [f"{ts:.6f}", f"{(t1 - t0) * 1000.0:.1f}",
               f"{float(pi_ms):.1f}" if isinstance(pi_ms, (int, float)) else ""]
        row += ["" if got.get(r) is None else str(got[r]) for r in CURRENT_REGS]
        g = signed_current(got.get((7, 69)), got.get((7, 70)))
        sh = signed_current(got.get((2, 69)), got.get((2, 70)))
        row += ["" if g is None else str(g), "" if sh is None else str(sh), " ".join(notes)]
        if any((s, a) in asked and (s, a) not in got for s, a in CURRENT_REGS) or err is not None:
            self.n_currents_fail += 1
        with self.csv_lock:
            if self.cur_f.closed:
                return
            self.cur_w.writerow(row)
            self.n_currents += 1
            self._maybe_flush(ts)


class PolicyRecorder(_ServoCurrents):
    """カメラを読むスレッドと関節角を読むスレッドを持つ. 本体は grab() で最新のコマを受け取る."""

    def __init__(self, run_dir, cap, read_angles_fn, to_model_fn, angles_ok=None, read_servo_fn=None):
        """read_servo_fn: daemon に read_servo の dict を送って返事の dict を返す関数 (依頼 116).
        None なら今までどおり read_angles_fn で角度だけ読む (currents.csv は列名だけ)."""
        self.dir = Path(run_dir) / "recording"
        self.angles_ok = angles_ok      # 本体が読んだ角度を残すときの確かめ (None なら 6 個の有限な数)
        self.dir.mkdir(parents=True, exist_ok=True)
        self.cap = cap
        self.read_angles_fn = read_angles_fn
        self.read_servo_fn = read_servo_fn
        self.currents_mode = "read_servo" if read_servo_fn is not None else "off"
        self.n_reads = 0                # 記録スレッドの読み取りの回数 (肩の上位を読む回を決める)
        self.n_currents = 0             # currents.csv の行の数
        self.n_currents_fail = 0        # 1 つでも番地が読めなかった行の数
        self.read_ms = []               # read_servo 1 回の長さ (ms)
        self.currents_write_errors = 0  # currents.csv に書けなかった回数 (角度の記録は続ける)
        self.to_model_fn = to_model_fn
        self.mode = "model"
        self.stop_flag = False
        self.stopped_cleanly = None     # stop() のあと: スレッドが止まって書き切れたか
        self.csv_lock = threading.Lock()
        self.frame_cv = threading.Condition()
        self.latest = None              # (frame, capture_ts)
        self.writer = None
        self.size = None
        self.n_frames = 0
        self.n_actual = 0
        self.n_actual_fail = 0
        self.n_actual_main = 0          # 本体が読んだ角度を残した数 (依頼 46)
        self.n_sends = 0                # send_angles のやりとりを残した数
        self.write_errors = 0           # mp4 を開けない / 書けなかった回数 (codex R2 A5)
        self.started = time.time()
        self.last_flush = self.started
        self.states_f = (self.dir / "states.csv").open("w", newline="")
        self.states_w = csv.writer(self.states_f)
        self.states_w.writerow(STATES_COLS)
        self.flog_f = (self.dir / "top_frame_log.csv").open("w", newline="")
        self.flog_w = csv.writer(self.flog_f)
        self.flog_w.writerow(FRAME_LOG_COLS)
        self.cur_f = (self.dir / "currents.csv").open("w", newline="")
        self.cur_w = csv.writer(self.cur_f)
        self.cur_w.writerow(CURRENTS_COLS)
        self._state_row("stage", time.time())
        self.t_cam = threading.Thread(target=self._cam_loop, daemon=True, name="record_camera")
        self.t_ang = threading.Thread(target=self._angle_loop, daemon=True, name="record_angles")

    def start(self):
        self.t_cam.start()
        self.t_ang.start()
        add_rpc_observer(self._on_rpc)

    # ---- 本体から呼ぶ -------------------------------------------------------
    def set_mode(self, mode):
        if mode == self.mode:
            return
        self.mode = mode
        self._state_row("stage", time.time())

    def gripper(self, state):
        """グリッパーに指令した瞬間に呼ぶ (build_demo_episodes は指令の時刻で 0/1 を切り替える)."""
        self._state_row("gripper_cmd", time.time(), gripper_state=int(state))

    def grab(self):
        """呼んだあとに撮れたコマを返す (古いコマを渡さない). 戻り値は grab_rgb と同じ (rgb, bgr_small)."""
        t_req = time.time()
        deadline = t_req + FRAME_WAIT_SEC
        with self.frame_cv:
            while not self.stop_flag:
                if self.latest is not None and self.latest[1] >= t_req:
                    return self.to_model_fn(self.latest[0])
                left = deadline - time.time()
                if left <= 0:
                    break
                self.frame_cv.wait(left)
        return None, None

    def stop(self):
        """スレッドを止めて書き切る. 何度呼んでもよい."""
        if self.stop_flag:
            return
        remove_rpc_observer(self._on_rpc)
        self.stop_flag = True
        with self.frame_cv:
            self.frame_cv.notify_all()
        for t in (self.t_cam, self.t_ang):
            if t.is_alive():
                t.join(timeout=STOP_JOIN_SEC)
        ended = time.time()
        alive = [t.name for t in (self.t_cam, self.t_ang) if t.is_alive()]
        # codex R1-5: スレッドが止まっていないのに mp4 を閉じると, あとから write() が走って競合する.
        # 止まらなかったときは mp4 を閉じず (カメラも本体で閉じない: alive() を見る),
        # 記録を「異常」として残す (build_correction_episodes はこの run を使わない).
        self.stopped_cleanly = not alive and not self.write_errors
        with self.csv_lock:
            for f in (self.states_f, self.flog_f, self.cur_f):
                try:
                    f.flush(); f.close()
                except Exception:
                    if f is self.cur_f:
                        self.currents_write_errors += 1
                    try:
                        f.close()
                    except Exception:
                        pass
        if self.writer is not None and not alive:
            try:
                self.writer.release()
            except Exception:
                pass
        el = ended - self.started
        meta = {"frames": self.n_frames, "elapsed_sec": round(el, 3),
                "effective_fps": round(self.n_frames / el, 3) if el > 0 else 0.0,
                "width": self.size[0] if self.size else None,
                "height": self.size[1] if self.size else None,
                "actual_samples": self.n_actual, "actual_read_failures": self.n_actual_fail,
                "actual_hz": round(self.n_actual / el, 2) if el > 0 else 0.0,
                "actual_main_samples": self.n_actual_main, "send_rpcs": self.n_sends,
                "target_fps": RECORD_FPS, "status": ("thread_not_stopped" if alive else "write_error" if self.write_errors else "ok"),
                "write_errors": self.write_errors,
                "threads_alive": alive,
                "currents_mode": self.currents_mode, "currents_rows": self.n_currents,
                "currents_rows_incomplete": self.n_currents_fail,
                "currents_write_errors": self.currents_write_errors,
                "currents_regs_every": [f"{a}:{b}" for a, b in EVERY_REGS],
                "currents_shoulder_hi_every": SHOULDER_HI_EVERY,
                "read_servo_ms": _ms_stats(self.read_ms)}
        (self.dir / "top_meta.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False))
        if alive:
            print(f"[RECORD] [WARN] 記録スレッドが止まりません ({', '.join(alive)}). "
                  "mp4 を閉じずに終えます (この記録は学習データに使いません)")
        print(f"[RECORD] 記録終了 コマ {self.n_frames} ({meta['effective_fps']} fps) "
              f"実測角 {self.n_actual} 回 ({meta['actual_hz']} Hz)  電流 {self.n_currents} 行"
              f" ({self.currents_mode})  → {self.dir}")

    # ---- 中身 ---------------------------------------------------------------
    def alive(self):
        """記録スレッドがまだ動いているか (stop() のあとに見る. 動いていればカメラを閉じない)."""
        return self.t_cam.is_alive() or self.t_ang.is_alive()

    def _state_row(self, ev, ts, angles=None, gripper_state=None, speed=None, ts_send=None, note=""):
        row = [f"{ts:.6f}", ev, self.mode]
        for k in range(6):
            row.append("" if angles is None else f"{float(angles[k]):.3f}")
        row += ["" if speed is None else str(speed), "" if gripper_state is None else str(gripper_state),
                "" if ts_send is None else f"{ts_send:.6f}", "", "", "", note]
        with self.csv_lock:
            if self.states_f.closed:
                return False
            self.states_w.writerow(row)
            self._maybe_flush(ts)
        return True

    def _valid(self, a):
        if self.angles_ok is not None:
            return bool(self.angles_ok(a))
        try:
            return len(a) == 6 and all(math.isfinite(float(v)) for v in a)
        except Exception:
            return False

    def _on_rpc(self, cmd_obj, resp, t0, t1, err):
        """daemon とのやりとり 1 回ごと (install_daemon_lock から). 何も送らない・読み直さない.

        get_angles: 本体 (記録スレッド以外) が読めた角度を actual 行に残す (note=main. 記録スレッドの分は
                    _angle_loop が残すので二重にしない)
        send_angles: 始めた時刻 (ts / ts_send)・目標・速度・やりとりの長さ (note) を send_angles 行に残す
        そのほか (ping 以外): rpc 行に cmd とやりとりの長さを残す
        """
        if self.stop_flag or not isinstance(cmd_obj, dict):
            return
        cmd = cmd_obj.get("cmd")
        dur = f"rpc_sec={t1 - t0:.3f}" + ("" if err is None else f" error={type(err).__name__}")
        if cmd == "read_servo" and threading.current_thread() is self.t_ang:
            return                      # 記録スレッドの読み取り (actual 行と currents.csv に残す. 依頼 116)
        if cmd == "get_angles":
            if threading.current_thread() is self.t_ang or err is not None or not isinstance(resp, dict):
                return
            a = resp.get("angles")
            if a is None or not self._valid(a):
                return
            if self._state_row("actual", t1, angles=a, note="main"):   # 書けた行だけ数える (codex 推奨)
                self.n_actual_main += 1
        elif cmd == "send_angles":
            ang = cmd_obj.get("angles")
            if self._state_row("send_angles", t0, angles=ang if ang is not None and len(ang) == 6 else None,
                               speed=cmd_obj.get("speed"), ts_send=t0, note=dur):
                self.n_sends += 1
        elif cmd != "ping":
            self._state_row("rpc", t0, ts_send=t0, note=f"cmd={cmd} {dur}")

    def _maybe_flush(self, now):
        if now - self.last_flush >= FLUSH_INTERVAL:
            try:
                self.states_f.flush(); self.flog_f.flush()
            except Exception:
                pass
            try:
                self.cur_f.flush()
            except Exception:
                self.currents_write_errors += 1     # 電流が書けていないことを残す (codex R2 推奨 3)
            self.last_flush = now

    def _cam_loop(self):
        last_written = 0.0
        while not self.stop_flag:
            ok, frame = self.cap.read()
            ts = time.time()
            if self.stop_flag:
                break
            if not ok or frame is None:
                time.sleep(0.02)
                continue
            with self.frame_cv:
                self.latest = (frame, ts)
                self.frame_cv.notify_all()
            if ts - last_written < FRAME_INTERVAL - 1e-3:
                continue
            last_written = ts
            try:
                if self.writer is None:
                    h, w = frame.shape[:2]
                    self.size = (w, h)
                    self.writer = cv2.VideoWriter(str(self.dir / "top.mp4"),
                                                  cv2.VideoWriter_fourcc(*FOURCC), RECORD_FPS, (w, h))
                    if not self.writer.isOpened():
                        raise RuntimeError(f"VideoWriter を開けない ({FOURCC})")
                if not self.writer.isOpened():
                    raise RuntimeError("VideoWriter が閉じている")
                self.writer.write(frame)
                with self.csv_lock:
                    if not self.flog_f.closed:
                        self.flog_w.writerow([self.n_frames, f"{ts:.6f}", self.mode])
                        self._maybe_flush(ts)
                self.n_frames += 1
            except Exception as e:
                self.write_errors += 1
                if self.write_errors <= 3:
                    print(f"[RECORD] [WARN] コマの書き込みに失敗: {e}")

    def _angle_loop(self):
        nxt = time.time()
        while not self.stop_flag:
            a, ts = self._read_once()     # read_servo のときは currents.csv と同じ時刻で actual 行を書く
            if a is None:
                self.n_actual_fail += 1
            else:
                self._state_row("actual", ts, angles=a)
                self.n_actual += 1
            nxt = _pace(nxt)


def _pace(nxt):
    """記録の読み取りの間合い (1 秒 RECORD_FPS 回). 次に読む予定の時刻を返す (依頼 116. 依頼 121 で 2 か所から使う)."""
    nxt += ANGLE_INTERVAL
    wait = nxt - time.time()
    if -wait > ANGLE_INTERVAL:
        nxt = time.time()       # 1 回分より大きく遅れた分は取り戻さない (詰めて読まない)
    # 1 回分までの遅れ (肩の上位も読む回は 100 ms を少し超える. 依頼 116) は, 次を早めに読んで取り戻す.
    # ただし読んだあとは必ず YIELD_SEC は鍵を手放す (すぐ取り直すと, 待っているパッドの命令が何回分も待つ)
    time.sleep(max(wait, YIELD_SEC))
    return nxt


WINDOW_MARGIN_SEC = 0.02        # 本体が眠る時間の終わりから引く分 (依頼 121)
READ_BUDGET_SEC = 0.15          # 1 回の読み取りに見込む時間 (11-07 の測りで 中央値 約 85 ms・肩の上位を読む回 約 105 ms)
WINDOW_POLL_SEC = 0.005         # 読んでよい時間を待つ間合い
CLOSE_LIMIT_SEC = 1.0           # stop() で currents.csv を閉じる・meta を書くのに待つ上限


def _bounded(fn, sec):
    """fn を別のスレッドで呼び, sec 秒だけ待つ. 終われば True (ファイルの書き込みで止まらないように. 依頼 121)."""
    t = threading.Thread(target=fn, daemon=True, name="record_currents_close")
    t.start()
    t.join(timeout=sec)
    return not t.is_alive()


class CurrentsRecorder(_ServoCurrents):
    """教えた動きの再生 (record_teach_demo.py run --execute) の間, 爪と肩の電流を currents.csv に残す (依頼 121).

    読み方・列・肩の上位を SHOULDER_HI_EVERY 回に 1 回にする扱い・読めない回を空にして続ける扱いは
    実機推論の記録 (PolicyRecorder. 依頼 116) と同じ部品 (_ServoCurrents) を使う. 違うのは次:
      - **本体 (再生) が daemon を使わずに眠っている間だけ読む**. 本体は眠る前に window(秒) で知らせる
        (到達の確かめ・静止の確かめの 0.3 秒の間・待ちの間の読み取りの合間). 読み始めてよいのは,
        読み終わる見込み (READ_BUDGET_SEC) が眠りの終わり - WINDOW_MARGIN_SEC より前のときだけ.
        なので見込みどおりに答える回は, 本体の daemon とのやりとりは電流の記録を待たない (到達・静止の確かめの
        回数と時刻は今までと同じ. codex R1・R2 必須). **残る場合**: read_servo が見込みより長くかかり, 本体が
        起きる時刻を超えた回は, 本体の次のやりとりがその分 (最大 READ_SERVO_TIMEOUT_SEC) 待つ. daemon とのやりとりが
        眠りの終わりを超えて続いた回を overruns に数える (本体がちょうどその時に daemon を使ったかは見ていない).
        間合いは 1 秒 RECORD_FPS 回まで (それより詰めて読まない). 本体が眠らない間 (send_angles の返事待ち
        約 1.6 秒・爪の命令と待ち・再送の前の待ち) は読まないので, 1 秒あたりの行は 10 より少ない
      - pause() から resume() までは window があっても読まない (爪の命令と待ち・再送の前の待ち.
        「爪を動かした直後にシリアルを触らない」(2026-10-19))
      - states.csv・top.mp4 は今までどおり recorder.py (別のプロセス) が書く. ここは currents.csv と
        currents_meta.json だけを書く. read_servo で読んだ角度は ts を決めるのに使い, angle_sink があれば
        angle_sink(角度, 読んだ時刻) で渡す (依頼 202: record_teach_demo が states.csv の actual 行にする.
        渡すのは読めた回だけ. 読み方・読む時機は変えない. 渡す側の失敗は loop_errors に数えて続ける)
      - daemon に read_servo がなければ (unknown cmd) 読むのをやめる (currents.csv は列名だけ)
    記録の失敗で再生を止めない (例外は外へ出さない. stop()・pause() は書き込みで止まったスレッドを待ち続けない).
    daemon のソケットは本体と共用なので, 使う前に install_daemon_lock() を呼ぶこと.
    """

    NO_READ_SERVO_MSG = "電流は残しません (再生と記録はそのまま続けます)"

    def __init__(self, run_dir, read_servo_fn):
        self.dir = Path(run_dir) / "recording"
        self.dir.mkdir(parents=True, exist_ok=True)
        self.read_servo_fn = read_servo_fn
        self.read_angles_fn = lambda: None
        self.angles_ok = None
        self.angle_sink = None          # 依頼 202: 読めた角度を渡す先 (角度, 読んだ時刻). None なら渡さない
        self.currents_mode = "read_servo"
        self.n_reads = 0
        self.n_currents = 0
        self.n_currents_fail = 0
        self.read_ms = []
        self.currents_write_errors = 0
        self.loop_errors = 0            # 読み取りの外側で起きた例外の数 (起きても続ける)
        self.overruns = 0               # 眠りの終わりを超えて読んでいた回 (本体が待った回)
        self.overrun_ms = []
        self.stop_flag = False
        self.paused = False
        self.allow_until = 0.0          # この時刻 (time.time) までに読み終わるなら読んでよい
        self.window_sec = 0.0           # 知らされた眠りの合計 (重なりを含む. 目安)
        self.next_at = 0.0
        self.gate = threading.Lock()    # 1 回の読み取り (と書き込み) の間持つ
        self.n_pauses = 0
        self.pause_gate_timeouts = 0
        self.paused_sec = 0.0
        self._paused_at = None
        self.csv_lock = threading.Lock()
        self.started = None
        self.meta = None
        self.last_flush = time.time()
        self.cur_f = (self.dir / "currents.csv").open("w", newline="")
        self.cur_w = csv.writer(self.cur_f)
        self.cur_w.writerow(CURRENTS_COLS)
        self.t = threading.Thread(target=self._loop, daemon=True, name="record_currents")

    def start(self):
        self.started = time.time()
        self.t.start()

    def window(self, sec):
        """本体が今から sec 秒 daemon を使わない (眠る) と知らせる. 読むのはこの間だけ."""
        if sec <= 0 or self.paused:     # 止めている間に知らされた眠りは使わない
            return
        now = time.time()
        self.window_sec += sec
        self.allow_until = max(self.allow_until, now + sec - WINDOW_MARGIN_SEC)

    def pause(self):
        """読み取りを止める. 読んでいる途中ならその 1 回が終わるまで待つ (上限 READ_SERVO_TIMEOUT_SEC + 0.5 秒.
        書き込みで止まったスレッドを待ち続けない. 上限を超えたら数えて止める印だけ立てる)."""
        got = self.gate.acquire(timeout=READ_SERVO_TIMEOUT_SEC + 0.5)
        try:
            if not got:
                self.pause_gate_timeouts += 1
            if not self.paused:
                self.paused = True
                self.n_pauses += 1
                self._paused_at = time.time()
            self.allow_until = 0.0
        finally:
            if got:
                self.gate.release()

    def resume(self):
        self.allow_until = 0.0          # 再開したら, 次に眠りを知らされるまで読まない
        if self.paused:
            self.paused = False
            if self._paused_at is not None:
                self.paused_sec += time.time() - self._paused_at
            self._paused_at = None

    def stop(self):
        """スレッドを止めて書き切り, 中身の数を dict で返す (currents_meta.json にも書く). 何度呼んでもよい.
        待つのは STOP_JOIN_SEC (スレッド) + CLOSE_LIMIT_SEC × 2 (閉じる・meta) まで."""
        if self.meta is not None:
            return self.meta
        self.stop_flag = True
        if self.t.is_alive():
            self.t.join(timeout=STOP_JOIN_SEC)
        ended = time.time()
        alive = self.t.is_alive()
        if self.paused and self._paused_at is not None:
            self.paused_sec += ended - self._paused_at     # 止めたまま終わった分
            self._paused_at = None

        def _close():
            with self.csv_lock:
                try:
                    self.cur_f.flush(); self.cur_f.close()
                except Exception:
                    self.currents_write_errors += 1
                    try:
                        self.cur_f.close()
                    except Exception:
                        pass
        closed = _bounded(_close, CLOSE_LIMIT_SEC)
        el = (ended - self.started) if self.started else 0.0
        run_sec = max(el - self.paused_sec, 0.0)
        self.meta = {"currents_mode": self.currents_mode, "currents_rows": self.n_currents,
                     "currents_rows_incomplete": self.n_currents_fail,
                     "currents_write_errors": self.currents_write_errors,
                     "loop_errors": self.loop_errors,
                     "currents_regs_every": [f"{a}:{b}" for a, b in EVERY_REGS],
                     "currents_shoulder_hi_every": SHOULDER_HI_EVERY,
                     "elapsed_sec": round(el, 3),
                     "paused_sec": round(self.paused_sec, 3), "pauses": self.n_pauses,
                     "pause_gate_timeouts": self.pause_gate_timeouts,
                     "window_sec": round(self.window_sec, 3),
                     "currents_hz": round(self.n_currents / el, 2) if el > 0 else 0.0,
                     "currents_hz_excl_paused": round(self.n_currents / run_sec, 2) if run_sec > 0 else 0.0,
                     "target_hz_max": RECORD_FPS,
                     "read_budget_sec": READ_BUDGET_SEC,
                     "overruns": self.overruns, "overrun_ms": _ms_stats(self.overrun_ms),
                     "read_servo_ms": _ms_stats(self.read_ms),
                     "status": ("thread_not_stopped" if alive else "ok" if closed else "close_timeout"),
                     "source": "record_teach_demo (依頼 121)",
                     "reads_only_while_main_sleeps": True}
        saved = {"ok": False, "error": None}

        def _save():
            try:
                (self.dir / "currents_meta.json").write_text(meta_text)
                saved["ok"] = True
            except Exception as e:
                saved["error"] = f"{type(e).__name__}: {e}"
        meta_text = json.dumps(self.meta, indent=2, ensure_ascii=False)
        if not _bounded(_save, CLOSE_LIMIT_SEC):
            saved["error"] = "timeout"
        # 書けたかを戻り値 (= 試行 JSON) に残す (codex R3 推奨 2. ファイルの中には書かない)
        self.meta["meta_file"] = "ok" if saved["ok"] else saved["error"]
        if not saved["ok"]:
            print(f"[RECORD] [WARN] currents_meta.json を書けません ({saved['error']}. 試行 JSON には残します)")
        print(f"[RECORD] 電流の記録終了 {self.n_currents} 行 ({self.meta['currents_hz']} 行/秒. 本体が眠っている間だけ"
              f"読む. 爪の間 {self.meta['paused_sec']} 秒は読まない. 見込み超え {self.overruns} 回. {self.currents_mode})"
              f"  → {self.dir / 'currents.csv'}")
        return self.meta

    def _maybe_flush(self, now):
        if now - self.last_flush >= FLUSH_INTERVAL:
            try:
                self.cur_f.flush()
            except Exception:
                self.currents_write_errors += 1
            self.last_flush = now

    def _valid(self, a):
        try:
            return len(a) == 6 and all(math.isfinite(float(v)) for v in a)
        except Exception:
            return False

    def _read_budget(self):
        """次の 1 回の読み取りに見込む時間 (依頼 156 の PowerRegsRecorder は番地を足す回だけ長くとる)."""
        return READ_BUDGET_SEC

    def _loop(self):
        while not self.stop_flag and self.currents_mode == "read_servo":
            now = time.time()
            if self.paused or now < self.next_at or now + self._read_budget() > self.allow_until:
                time.sleep(WINDOW_POLL_SEC)
                continue
            with self.gate:
                if self.paused or self.stop_flag or time.time() + self._read_budget() > self.allow_until:
                    continue
                t0 = time.time()
                limit = self.allow_until + WINDOW_MARGIN_SEC      # 本体が起きる時刻 (の見込み)
                self.next_at = t0 + ANGLE_INTERVAL
                n_before = len(self.read_ms)
                try:
                    a, ts = self._read_once()
                    if a is not None and self.angle_sink is not None:
                        self.angle_sink(a, ts)      # 依頼 202: states.csv の actual 行へ
                except Exception as e:      # 記録の失敗で再生を止めない
                    self.loop_errors += 1
                    if self.loop_errors <= 3:
                        print(f"[RECORD] [WARN] 電流の読み取りで例外: {type(e).__name__}: {e}")
                # daemon とのやりとりが終わった時刻 (currents.csv への書き込みは含めない. codex R3 推奨 1)
                rpc_end = (t0 + self.read_ms[-1] / 1000.0) if len(self.read_ms) > n_before else time.time()
                over = rpc_end - limit
                if over > 0:
                    self.overruns += 1
                    self.overrun_ms.append(over * 1000.0)


def signed_current(lo, hi):
    """番地 69 (下位) と 70 (上位) から符号付きの値. どちらかがなければ None (依頼 116)."""
    if lo is None or hi is None:
        return None
    v = lo + 256 * (hi & 127)
    return -v if hi & 128 else v


def _ms_stats(xs):
    if not xs:
        return None
    ys = sorted(xs)
    return {"n": len(ys), "median": round(ys[len(ys) // 2], 1),
            "p95": round(ys[min(len(ys) - 1, int(len(ys) * 0.95))], 1), "max": round(ys[-1], 1)}


READ_SERVO_TIMEOUT_SEC = 1.5    # 記録の read_servo 1 回の上限. 鍵を持ったまま既定のタイムアウトまで待つと
                                # パッドの stop も待たせるので短くする (いつもは 0.1 秒. 番地が 4 つとも
                                # 返事なしでも 0.9 秒ほど. codex R1 推奨 3). 超えたら次の回はつなぎ直す


def daemon_read_servo(cmd_obj):
    """記録スレッドの read_servo (依頼 116). 呼んだときの visual_servo_pick._daemon_request を使う
    (install_daemon_lock の鍵を通る)."""
    return vsp._daemon_request(cmd_obj, timeout_override=READ_SERVO_TIMEOUT_SEC)


# ---- 2026-11-12 依頼 156: 電源 (12V・2A → 5A) の前後比較のために, 爪の位置とサーボの電圧も読む ----------------
# record_teach_demo.py run --power 2A のときだけ使う (付けなければ CurrentsRecorder のまま. 実機推論は変えない).
# 番地は Feetech STS 想定で, この機体では確かめていない (tools/pi/servo_check_pi.py と同じ前提). 値は生のまま残す:
#   爪の位置  (7, 56) 下位・(7, 57) 上位 → gripper_pos (signed_current と同じ組み立て. 上位の 128 を向きとみなす)
#   電圧      (2, 62) 肩・(7, 62) 爪 (0.1V 想定)
# get_gripper_value は使わない (2026-10-19 段階C で, 開いた直後の gv のあとに daemon が固まった. 番地 69/70 と同じ
# get_servo_data で読む). 足した番地は currents.csv に入れず power_regs.csv に書く (currents.csv の列と行は今までどおり).
# 1 回に足すのは 2 つまで (番地 1 つ 約 20 ms. 肩の上位を読む回には足さない) なので, 1 回の読み取りは多くて 約 125 ms.
# 読み始めてよいかの決め方 (本体が眠っている間だけ) は同じで, 番地を足す回だけ見込みを 1 つにつき
# POWER_REG_BUDGET_SEC 長くとる (READ_BUDGET_SEC 0.15 → 0.19 秒). 足した番地が POWER_GIVE_UP 回続けて返事なしなら,
# その番地はその run ではもう頼まない (本体を待たせるのは多くてその回数まで. overruns に数える).
POWER_POS_REGS = [(7, 56), (7, 57)]
POWER_VOLT_REGS = [(2, 62), (7, 62)]
POWER_REGS = POWER_POS_REGS + POWER_VOLT_REGS
POWER_COLS = ["ts", "r7_56", "r7_57", "gripper_pos", "r2_62", "r7_62", "note"]
POWER_REG_BUDGET_SEC = 0.02     # 足した番地 1 つにつき, 読み始めてよいかの見込みに足す時間 (番地 1 つ 約 20 ms)
POWER_GIVE_UP = 2               # 足した番地が続けてこの回数 返事なしなら, その run ではもう頼まない (codex R1 必須 1:
                                # 2.9.6 は返事のない番地 1 つに 約 0.1 秒待つので, 頼み続けると本体を待たせる)


def power_extra_regs(n):
    """n 回目 (0 始まり) の読み取りに足す番地. 肩の上位を読む回 (n % SHOULDER_HI_EVERY == 0) には足さない.
    残りの回は 爪の位置 → 電圧 → 爪の位置 → 電圧 の順 (5 回に 2 回ずつ)."""
    k = n % SHOULDER_HI_EVERY
    if k == 0:
        return []
    return list(POWER_POS_REGS) if k % 2 == 1 else list(POWER_VOLT_REGS)


class PowerRegsRecorder(CurrentsRecorder):
    """CurrentsRecorder に爪の位置と電圧の番地を足す (依頼 156). 足した番地は {run}/recording/power_regs.csv.
    読めなかった番地は空にして note に no_reply を書く (止めない)."""

    def __init__(self, run_dir, read_servo_fn):
        super().__init__(run_dir, read_servo_fn)
        self.pw_f = (self.dir / "power_regs.csv").open("w", newline="")
        self.pw_w = csv.writer(self.pw_f)
        self.pw_w.writerow(POWER_COLS)
        self.pw_asked = {f"{s}:{a}": 0 for s, a in POWER_REGS}
        self.pw_got = {f"{s}:{a}": 0 for s, a in POWER_REGS}
        self.pw_rows = 0
        self.pw_write_errors = 0
        self.pw_noreply_run = {k: 0 for k in self.pw_asked}    # 続けて返事なしの回数
        self.pw_given_up = []                                   # もう頼まない番地 ("2:62" など)

    def _extras(self):
        return [r for r in power_extra_regs(self.n_reads) if f"{r[0]}:{r[1]}" not in self.pw_given_up]

    def _read_budget(self):
        return READ_BUDGET_SEC + POWER_REG_BUDGET_SEC * len(self._extras())

    def _servo_cmd(self):
        cmd = super()._servo_cmd()
        cmd["regs"] += [list(r) for r in self._extras()]
        return cmd

    def _current_row(self, ts, t0, t1, cmd_obj, resp, err, a_note):
        try:
            self._power_row(ts, cmd_obj, resp, err)
        except Exception as e:
            self.pw_write_errors += 1
            if self.pw_write_errors <= 3:
                print(f"[RECORD] [WARN] power_regs.csv に書けません: {type(e).__name__}: {e}")
        super()._current_row(ts, t0, t1, cmd_obj, resp, err, a_note)

    def _power_row(self, ts, cmd_obj, resp, err):
        asked = [tuple(r) for r in cmd_obj["regs"] if tuple(r) in POWER_REGS]
        if not asked:
            return
        got = {}
        if err is None and isinstance(resp, dict):
            for r in resp.get("regs") or []:
                try:
                    sid, addr, val = int(r[0]), int(r[1]), r[2]
                except Exception:
                    continue
                if (sid, addr) in asked and isinstance(val, int) and not isinstance(val, bool) and 0 <= val <= 255:
                    got[(sid, addr)] = val
        replied = err is None and isinstance(resp, dict)
        for r in asked:
            k = f"{r[0]}:{r[1]}"
            self.pw_asked[k] += 1
            if r in got:
                self.pw_got[k] += 1
                self.pw_noreply_run[k] = 0
            elif replied:               # daemon は答えたが, その番地だけ返事がない
                self.pw_noreply_run[k] += 1
                if self.pw_noreply_run[k] >= POWER_GIVE_UP and k not in self.pw_given_up:
                    self.pw_given_up.append(k)
                    print(f"[RECORD] [WARN] 番地 {k} が {POWER_GIVE_UP} 回続けて返事なし. この run ではもう読みません "
                          "(本体を待たせないため. 再生は続けます)")
        notes = []
        if err is not None:
            notes.append(f"error={type(err).__name__}: {str(err)[:80]}")
        elif not isinstance(resp, dict):
            notes.append("error=bad_response")
        missing = [f"{s}:{a}" for s, a in asked if (s, a) not in got]
        if missing and err is None and isinstance(resp, dict):
            notes.append("no_reply=" + ",".join(missing))
        pos = signed_current(got.get((7, 56)), got.get((7, 57)))

        def v(r):
            return "" if got.get(r) is None else str(got[r])
        row = [f"{ts:.6f}", v((7, 56)), v((7, 57)), "" if pos is None else str(pos),
               v((2, 62)), v((7, 62)), " ".join(notes)]
        with self.csv_lock:
            if self.pw_f.closed:
                return
            self.pw_w.writerow(row)
            self.pw_rows += 1

    def _maybe_flush(self, now):
        if now - self.last_flush >= FLUSH_INTERVAL:
            try:
                self.pw_f.flush()
            except Exception:
                self.pw_write_errors += 1
        super()._maybe_flush(now)

    def stop(self):
        if self.meta is not None:
            return self.meta
        meta = super().stop()

        def _close():
            with self.csv_lock:
                try:
                    self.pw_f.flush(); self.pw_f.close()
                except Exception:
                    self.pw_write_errors += 1
                    try:
                        self.pw_f.close()
                    except Exception:
                        pass
        closed = _bounded(_close, CLOSE_LIMIT_SEC)
        meta["power_regs"] = {
            "file": "power_regs.csv", "rows": self.pw_rows, "asked": dict(self.pw_asked), "got": dict(self.pw_got),
            "write_errors": self.pw_write_errors, "closed": closed, "given_up": list(self.pw_given_up),
            "regs": {"gripper_pos": ["7:56", "7:57"], "voltage": ["2:62", "7:62"]},
            "schedule": f"肩の上位を読まない回に 2 つずつ (爪の位置と電圧を交互). {SHOULDER_HI_EVERY} 回に 2 回ずつ",
            "note": "Feetech STS 想定の番地. この機体で未確認. 生の値 (電圧は 0.1V 想定)",
            "source": "record_teach_demo --power (依頼 156)"}
        # currents_meta.json は依頼 121 の中身のまま. 足した番地の数は power_regs_meta.json に書く (codex R1 推奨 6)
        pw_text = json.dumps(meta["power_regs"], indent=2, ensure_ascii=False)
        saved = {"ok": False}

        def _save():
            try:
                (self.dir / "power_regs_meta.json").write_text(pw_text)
                saved["ok"] = True
            except Exception:
                pass
        _bounded(_save, CLOSE_LIMIT_SEC)
        meta["power_regs"]["meta_file"] = "ok" if saved["ok"] else "error"
        print(f"[RECORD] 爪の位置と電圧 {self.pw_rows} 行 (読めた数 {self.pw_got}"
              f"{'. もう読まなかった番地 ' + ','.join(self.pw_given_up) if self.pw_given_up else ''})"
              f"  → {self.dir / 'power_regs.csv'}")
        return meta
