"""SO-101 で Mac・カメラ2台・アームから取れるデータを実測する (依頼 258). 記録だけ. 解析は check_258_analyze.py.

エイイチへの指示はすべてこのウィンドウに大きな字で出す. 各段階はスペースで始め, q でいつでも止める.

段階:
  s0  つながり (2枚の基板・サーボ12個・カメラ2台・iPhone)      動かさない
  s1  机側カメラのピント (desk_cam_focus_257.py を起動)          動かさない
  s2  読み取りの速さ (関節・カメラ・同時)                        動かさない (読むだけのポート)
  s34 テレオペ: 爪の開け閉め5回 → ゆっくり・ふつう・速く 各15秒   フォロワーはリーダーに従って動く
  s5  lerobot-record で 10秒×3回                                  テレオペ
  s6  lerobot-replay で記録の1回を2回再生                          始める前にウィンドウで確認

フォロワーに指令が出るのは s34・s5・s6 だけ. s0〜s2 は connect(handshake=False) で開いて読むだけ
(トルク・EEPROM・キャリブレーション・上限値・カメラの設定には書かない).
テレオペ (s34・s5) の前に, リーダーとフォロワーの姿勢の差が小さくなるまで始められないようにする
(LeRobot のテレオペは1コマ目からリーダーの角度をそのまま送り, ならす仕組みがないため).

使い方 (conda の lerobot 環境で):
    python so101/check_258.py                 # s0 から s6 まで順に
    python so101/check_258.py --only s2 s34   # 一部だけ
"""

import argparse
import csv
import json
import os
import shutil
import signal
import subprocess
import sys
import threading
import time
from datetime import datetime
from pathlib import Path

import cv2
import numpy as np
from PIL import Image, ImageDraw, ImageFont

HERE = Path(__file__).resolve().parent
REPO = HERE.parent
OUT = HERE / "check_258"
FOLLOWER_PORT = "/dev/tty.usbmodem5CBC4321631"
LEADER_PORT = "/dev/tty.usbmodem5CBC4323581"
FOLLOWER_ID, LEADER_ID = "ptm_follower", "ptm_leader"
CAL_DIR = Path.home() / ".cache/huggingface/lerobot/calibration"
CAL_FOLLOWER = CAL_DIR / "robots/so_follower" / f"{FOLLOWER_ID}.json"
CAL_LEADER = CAL_DIR / "teleoperators/so_leader" / f"{LEADER_ID}.json"
BASE_PY = "/opt/homebrew/Caskroom/miniforge/base/bin/python3"  # cv2_enumerate_cameras が入っている python
FONT_PATH = "/System/Library/Fonts/ヒラギノ角ゴシック W6.ttc"
MOTORS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]
CAM_KEYS = {"wrist": "usb hd camera", "desk": "innomaker"}
DATASET_ROOT = OUT / "dataset"
DATASET_REPO = "local/check_258"
FPS = 30
POSE_TOL_DEG = 10.0  # テレオペ・再生を始めてよい姿勢の差 (関節1〜5, 度)
POSE_TOL_GRIP = 15.0  # 爪 (0〜100)
WIN = "SO-101 check 258"


class Abort(Exception):
    pass


# ---- 画面 -------------------------------------------------------------------------

class Screen:
    W, H = 1280, 800

    def __init__(self):
        self.f_big = ImageFont.truetype(FONT_PATH, 60)
        self.f_mid = ImageFont.truetype(FONT_PATH, 40)
        self.f_small = ImageFont.truetype(FONT_PATH, 26)
        cv2.namedWindow(WIN, cv2.WINDOW_NORMAL)
        cv2.resizeWindow(WIN, self.W, self.H)

    def show(self, title, lines=(), small=(), color=(255, 255, 255), previews=()):
        img = Image.new("RGB", (self.W, self.H), (0, 0, 0))
        d = ImageDraw.Draw(img)
        y = 30
        d.text((40, y), title, font=self.f_big, fill=color)
        y += 90
        for ln in lines:
            d.text((40, y), ln, font=self.f_mid, fill=(255, 255, 255))
            y += 56
        y = max(y + 10, self.H - 40 - 32 * len(small))
        for ln in small:
            d.text((40, y), ln, font=self.f_small, fill=(180, 180, 180))
            y += 32
        arr = cv2.cvtColor(np.asarray(img), cv2.COLOR_RGB2BGR)
        x = self.W - 20
        for p in previews:
            if p is None:
                continue
            th = cv2.resize(p, (320, int(320 * p.shape[0] / p.shape[1])))
            x -= th.shape[1]
            arr[self.H - th.shape[0] - 200:self.H - 200, x:x + th.shape[1]] = th
            x -= 10
        cv2.imshow(WIN, arr)

    def key(self, ms=1):
        k = cv2.waitKey(ms) & 0xFF
        if k == ord("q"):
            raise Abort("q が押された")
        return k

    def wait_space(self, title, lines=(), small=(), color=(0, 255, 255), check=None):
        """スペースを待つ. check() が (ok, 追加の行) を返すときは ok の間だけ始められる."""
        while True:
            extra, ok = [], True
            if check is not None:
                ok, extra = check()
            foot = ["スペース = 始める   q = 止める"] if ok else ["まだ始められない (上の差を小さくする)   q = 止める"]
            self.show(title, list(lines) + list(extra), list(small) + foot, color)
            if self.key(100 if check else 30) == ord(" ") and ok:
                return

    def countdown(self, title, seconds, lines_fn=None, small=(), previews_fn=None, color=(0, 255, 0), tick=None):
        t0 = time.perf_counter()
        while True:
            el = time.perf_counter() - t0
            if el >= seconds:
                return
            lines = lines_fn(el) if lines_fn else []
            self.show(f"{title}   残り {seconds - el:4.1f} 秒", lines, list(small) + ["q = 止める"], color,
                      previews_fn() if previews_fn else ())
            if tick:
                tick(el)
            self.key(80)


# ---- カメラの番号 (名前で突き合わせる) --------------------------------------------

def resolve_cameras():
    sys.path.insert(0, str(HERE))
    import desk_cam_focus_257 as dcf

    exe, ff = dcf.ffmpeg_video_devices()
    code = ("import cv2, json; from cv2_enumerate_cameras import enumerate_cameras;"
            "print(json.dumps([(c.index, c.name or '') for c in enumerate_cameras(cv2.CAP_AVFOUNDATION)]))")
    r = subprocess.run([BASE_PY, "-c", code], capture_output=True, text=True)
    cvd = json.loads(r.stdout) if r.returncode == 0 else []
    found = {}
    for key, needle in CAM_KEYS.items():
        ff_hit = [n for _, n in ff if needle in n.lower()]
        cv_hit = [i for i, n in cvd if needle in n.lower()]
        found[key] = cv_hit[0] if (ff_hit and len(cv_hit) == 1) else None
    return {"ffmpeg": ff, "opencv": cvd, "index": found,
            "iriun": any("iriun" in n.lower() for _, n in ff) or any("iriun" in n.lower() for _, n in cvd)}


# ---- 読むだけのサーボのバス ---------------------------------------------------------

def load_cal(path):
    from lerobot.motors import MotorCalibration
    raw = json.loads(Path(path).read_text())
    return {k: MotorCalibration(**v) for k, v in raw.items()}


def open_ro_bus(port, cal_path):
    """SOFollower/SOLeader と同じ関節名・単位のバスを, 書き込みなしで開く (handshake もしない)."""
    from lerobot.motors import Motor, MotorNormMode, SerialMotorsBus
    motors = {m: Motor(i + 1, "sts3215", MotorNormMode.RANGE_0_100 if m == "gripper" else MotorNormMode.DEGREES)
              for i, m in enumerate(MOTORS)}
    bus = SerialMotorsBus(port, motors, calibration=load_cal(cal_path))
    bus.connect(handshake=False)
    return bus


def close_ro(bus):
    try:
        bus.disconnect(disable_torque=False)  # 既定の True はトルクを切る書き込みをするので使わない
    except Exception as e:
        print("[BUS] disconnect:", e)


def pose_diff(a, b):
    return {m: (a[m] - b[m]) for m in MOTORS}


def pose_ok(diff):
    return all(abs(diff[m]) <= (POSE_TOL_GRIP if m == "gripper" else POSE_TOL_DEG) for m in MOTORS)


def pose_lines(diff, label_a, label_b):
    out = [f"{label_a} − {label_b} の差 (関節1〜5 は {POSE_TOL_DEG:.0f}° 以内, 爪は {POSE_TOL_GRIP:.0f} 以内で開始可)"]
    row = []
    for m in MOTORS:
        tol = POSE_TOL_GRIP if m == "gripper" else POSE_TOL_DEG
        mark = "○" if abs(diff[m]) <= tol else "×"
        row.append(f"{m[:8]} {diff[m]:+6.1f}{mark}")
    out += ["   ".join(row[:3]), "   ".join(row[3:])]
    return out


# ---- カメラを記録するスレッド ---------------------------------------------------------

PROPS = ["CAP_PROP_FRAME_WIDTH", "CAP_PROP_FRAME_HEIGHT", "CAP_PROP_FPS", "CAP_PROP_FOURCC", "CAP_PROP_AUTO_EXPOSURE",
         "CAP_PROP_EXPOSURE", "CAP_PROP_AUTO_WB", "CAP_PROP_WB_TEMPERATURE", "CAP_PROP_GAIN", "CAP_PROP_BRIGHTNESS",
         "CAP_PROP_CONTRAST", "CAP_PROP_SATURATION", "CAP_PROP_AUTOFOCUS", "CAP_PROP_FOCUS"]


class CamRec(threading.Thread):
    """1コマごとに Mac の時計で届いた時刻を記録する. 頼むのは解像度・MJPG・fps だけ (他の設定は読むだけ)."""

    def __init__(self, name, index, w, h, stop, video_path=None, keep_small=False):
        super().__init__(daemon=True)
        self.name_, self.index, self.w, self.h, self.stop = name, index, w, h, stop
        self.video_path, self.keep_small = video_path, keep_small
        self.rows, self.small, self.latest, self.props, self.error = [], [], None, {}, None
        self.ready = threading.Event()

    def run(self):
        cap = cv2.VideoCapture(self.index, cv2.CAP_AVFOUNDATION)
        if not cap.isOpened():
            self.error = f"OpenCV {self.index} を開けない"
            self.ready.set()
            return
        cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
        cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.w)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.h)
        cap.set(cv2.CAP_PROP_FPS, FPS)
        for p in PROPS:
            v = cap.get(getattr(cv2, p))
            if p == "CAP_PROP_FOURCC":
                iv = int(v)
                v = "".join(chr((iv >> 8 * k) & 0xFF) for k in range(4)) if iv > 0 else str(v)
            self.props[p] = v
        writer = None
        self.ready.set()
        n = 0
        try:
            while not self.stop.is_set():
                ok, f = cap.read()
                t = time.perf_counter()
                if not ok:
                    self.rows.append((t, -1, float("nan"), float("nan"), 0, 0))
                    time.sleep(0.005)
                    continue
                gray = cv2.cvtColor(f, cv2.COLOR_BGR2GRAY)
                h, w = gray.shape
                c = gray[int(h * 0.3):int(h * 0.7), int(w * 0.3):int(w * 0.7)]
                blur = float(cv2.Laplacian(c, cv2.CV_64F).var())
                self.rows.append((t, cap.get(cv2.CAP_PROP_POS_MSEC), blur, float(gray.mean()), w, h))
                if self.keep_small:
                    self.small.append(cv2.resize(gray, (160, 120), interpolation=cv2.INTER_AREA))
                if self.video_path is not None:
                    if writer is None:
                        writer = cv2.VideoWriter(str(self.video_path), cv2.VideoWriter_fourcc(*"mp4v"), FPS, (w, h))
                    writer.write(f)
                self.latest = f
                n += 1
        finally:
            if writer is not None:
                writer.release()
            cap.release()

    def save(self, stem):
        with open(f"{stem}.csv", "w", newline="") as fh:
            wr = csv.writer(fh)
            wr.writerow(["t", "pos_msec", "blur_center", "mean", "w", "h"])
            wr.writerows(self.rows)
        if self.keep_small and self.small:
            np.save(f"{stem}_small.npy", np.stack(self.small))
        Path(f"{stem}_props.json").write_text(json.dumps({"index": self.index, "request": [self.w, self.h, FPS, "MJPG"],
                                                          "props": self.props, "error": self.error},
                                                         ensure_ascii=False, indent=2))


def start_cams(idx, w, h, stop, video_dir=None, keep_small=False):
    cams = {}
    for key in CAM_KEYS:
        vp = (video_dir / f"{key}.mp4") if video_dir else None
        cams[key] = CamRec(key, idx[key], w, h, stop, vp, keep_small)
        cams[key].start()
    for c in cams.values():
        c.ready.wait(10)
    return cams


# ---- s0 つながり --------------------------------------------------------------------

def stage0(scr, ctx):
    scr.show("0. つながりを確かめる", ["サーボとカメラを読むだけ. 動かしません."])
    scr.key(30)
    r = subprocess.run([sys.executable, str(HERE / "read_servos_248.py"), "--json", str(OUT / "s0_servos.json")],
                       capture_output=True, text=True)
    servos = json.loads((OUT / "s0_servos.json").read_text()) if (OUT / "s0_servos.json").exists() else {}
    cams = resolve_cameras()
    ctx["cams"] = cams
    n_servo = {p: (len(v) if isinstance(v, list) else 0) for p, v in servos.items()} if isinstance(servos, dict) else {}
    res = {"servo_counts": n_servo, "read_servos_stdout": r.stdout[-3000:], "cameras": cams}
    (OUT / "s0.json").write_text(json.dumps(res, ensure_ascii=False, indent=2))
    ok = n_servo.get(FOLLOWER_PORT) == 6 and n_servo.get(LEADER_PORT) == 6 and all(
        v is not None for v in cams["index"].values())
    lines = [f"フォロワー {n_servo.get(FOLLOWER_PORT)} 個 / リーダー {n_servo.get(LEADER_PORT)} 個",
             f"手首カメラ OpenCV {cams['index']['wrist']} / 机側カメラ OpenCV {cams['index']['desk']}",
             f"iPhone (Iriun) {'見えている' if cams['iriun'] else '見えない'}"]
    if not ok:
        scr.show("0. 足りないものがある. 止めます", lines, color=(0, 0, 255))
        scr.key(4000)
        raise Abort("つながりが足りない: " + " / ".join(lines))
    scr.wait_space("0. つながり OK", lines)


# ---- s1 机側カメラのピント --------------------------------------------------------------

def stage1(scr, ctx):
    scr.wait_space("1. 机側カメラのピント合わせ",
                   ["次に開く画面で, 机側カメラのピントと置き場所を合わせる",
                    "数字 (中央) が大きいほどピントが合っている",
                    "合ったら スペースで保存 → q で終わる"])
    before = set((HERE / "check_257").glob("desk_*.json")) if (HERE / "check_257").exists() else set()
    scr.show("1. ピント合わせの画面を開いています", ["保存はスペース, 終わりは q (その画面で)"])
    scr.key(30)
    cv2.destroyWindow(WIN)
    cv2.waitKey(1)
    subprocess.run([BASE_PY, str(HERE / "desk_cam_focus_257.py")])
    cv2.namedWindow(WIN, cv2.WINDOW_NORMAL)
    cv2.resizeWindow(WIN, Screen.W, Screen.H)
    after = set((HERE / "check_257").glob("desk_*.json")) if (HERE / "check_257").exists() else set()
    new = sorted(after - before)
    dst = OUT / "s1"
    dst.mkdir(exist_ok=True)
    for js in new:
        for p in (js, js.with_suffix(".png")):
            if p.exists():
                shutil.copy2(p, dst / p.name)
    ref = subprocess.run([BASE_PY, str(HERE / "desk_cam_focus_257.py"), "--score-image",
                          str(HERE / "check_248" / "desk_opencv1.png")], capture_output=True, text=True)
    (dst / "ref_248_score.txt").write_text(ref.stdout + ref.stderr)
    scr.wait_space("1. ピント合わせ 終わり", [f"保存した画像 {len(new)} 枚", "次は 2. 読み取りの速さ (触らない)"])


# ---- s2 読み取りの速さ ---------------------------------------------------------------

def joint_loop(stop, rows, extras=False, duration=30.0):
    fb = open_ro_bus(FOLLOWER_PORT, CAL_FOLLOWER)
    lb = open_ro_bus(LEADER_PORT, CAL_LEADER)
    try:
        t_end = time.perf_counter() + duration
        nxt = time.perf_counter()
        while time.perf_counter() < t_end and not stop.is_set():
            t0 = time.perf_counter()
            err = ""
            try:
                fp = fb.sync_read("Present_Position", num_retry=2)
            except Exception as e:
                fp, err = {}, f"F:{e}"
            t1 = time.perf_counter()
            try:
                lp = lb.sync_read("Present_Position", num_retry=2)
            except Exception as e:
                lp, err = {}, err + f" L:{e}"
            t2 = time.perf_counter()
            row = {"t0": t0, "follower_ms": (t1 - t0) * 1e3, "leader_ms": (t2 - t1) * 1e3, "err": err}
            if extras:
                for reg in ["Present_Current", "Present_Load", "Present_Temperature", "Present_Voltage"]:
                    a = time.perf_counter()
                    try:
                        v = fb.sync_read(reg, normalize=False, num_retry=2)
                        row[reg] = json.dumps(v)
                    except Exception as e:
                        row[reg] = f"ERR {e}"
                    row[reg + "_ms"] = (time.perf_counter() - a) * 1e3
            row["total_ms"] = (time.perf_counter() - t0) * 1e3
            rows.append(row)
            nxt += 1 / FPS
            time.sleep(max(0.0, nxt - time.perf_counter()))
    finally:
        close_ro(fb)
        close_ro(lb)


def save_rows(path, rows):
    if not rows:
        return
    keys = list(dict.fromkeys(k for r in rows for k in r))
    with open(path, "w", newline="") as fh:
        wr = csv.DictWriter(fh, keys)
        wr.writeheader()
        wr.writerows(rows)


def run_bg(scr, title, seconds, target, lines=(), cams=None):
    stop = threading.Event()
    th = threading.Thread(target=target, args=(stop,), daemon=True)
    th.start()
    try:
        scr.countdown(title, seconds + 1, lambda el: list(lines),
                      previews_fn=(lambda: [c.latest for c in cams.values()]) if cams else None,
                      small=["アームにもカメラにも触らない"])
        th.join(timeout=seconds + 10)
    finally:
        stop.set()
        th.join(timeout=5)


def stage2(scr, ctx):
    idx = ctx["cams"]["index"]
    d = OUT / "s2"
    d.mkdir(exist_ok=True)
    scr.wait_space("2. 読み取りの速さ (動かさない)",
                   ["約 2分40秒. アームにもカメラにも触らないで待つ",
                    "a) 関節の位置 30秒  b) 位置+電流など 15秒",
                    "c) カメラ 1280x720 30秒  d) 640x480 30秒",
                    "e) カメラ2台+関節 同時 30秒"])
    rows = []
    run_bg(scr, "2a. 関節の位置を読む", 30, lambda s: joint_loop(s, rows, False, 30), ["フォロワーとリーダーを読む"])
    save_rows(d / "a_joints.csv", rows)
    rows = []
    run_bg(scr, "2b. 位置+電流・負荷・温度・電圧", 15, lambda s: joint_loop(s, rows, True, 15))
    save_rows(d / "b_joints_extras.csv", rows)
    for tag, (w, h) in [("c_cam_1280x720", (1280, 720)), ("d_cam_640x480", (640, 480))]:
        stop = threading.Event()
        cams = start_cams(idx, w, h, stop)
        try:
            scr.countdown(f"2. カメラ {w}x{h}", 30, lambda el: ["カメラ2台を同時に開いている"],
                          previews_fn=lambda: [c.latest for c in cams.values()], small=["触らない"])
        finally:
            stop.set()
            for c in cams.values():
                c.join(5)
        for k, c in cams.items():
            c.save(d / f"{tag}_{k}")
    stop = threading.Event()
    cams = start_cams(idx, 640, 480, stop)
    rows = []
    try:
        run_bg(scr, "2e. カメラ2台+関節 同時", 30, lambda s: joint_loop(s, rows, True, 30), cams=cams)
    finally:
        stop.set()
        for c in cams.values():
            c.join(5)
    save_rows(d / "e_joints.csv", rows)
    for k, c in cams.items():
        c.save(d / f"e_cam_{k}")


# ---- テレオペ (s34) --------------------------------------------------------------------

def pose_gate(scr, title, lines, ref_fn, label_a, label_b):
    """読むだけのバスで姿勢の差を出し続け, 差が小さいときだけスペースで進める."""
    fb = open_ro_bus(FOLLOWER_PORT, CAL_FOLLOWER)
    lb = open_ro_bus(LEADER_PORT, CAL_LEADER) if ref_fn is None else None
    try:
        def check():
            f = fb.sync_read("Present_Position", num_retry=2)
            ref = lb.sync_read("Present_Position", num_retry=2) if lb else ref_fn()
            diff = pose_diff(ref, f) if lb else pose_diff(ref, f)
            return pose_ok(diff), pose_lines(diff, label_a, label_b)
        scr.wait_space(title, lines, check=check)
    finally:
        close_ro(fb)
        if lb:
            close_ro(lb)


class Teleop(threading.Thread):
    """lerobot-teleoperate と同じクラス・同じ既定の設定 (max_relative_target=None) で 30Hz のテレオペ.
    毎周: リーダーを読む → send_action → フォロワーの位置・電流・負荷・速さを読む. 温度は1秒ごと.
    フォロワーを connect する前に Goal_Position を今の位置に書いておく (トルクが入った瞬間に飛ばないように. RAM)."""

    def __init__(self, stop):
        super().__init__(daemon=True)
        self.stop, self.rows, self.phase, self.error, self.latest = stop, [], "start", None, {}
        self.ready = threading.Event()

    def run(self):
        from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig
        from lerobot.teleoperators.so_leader import SO101Leader, SO101LeaderConfig
        leader = robot = None
        try:
            leader = SO101Leader(SO101LeaderConfig(port=LEADER_PORT, id=LEADER_ID))
            leader.connect(calibrate=False)
            robot = SO101Follower(SO101FollowerConfig(port=FOLLOWER_PORT, id=FOLLOWER_ID))
            robot.bus.connect()
            raw = robot.bus.sync_read("Present_Position", normalize=False, num_retry=2)
            robot.bus.sync_write("Goal_Position", raw, normalize=False)
            robot.bus.disconnect(disable_torque=False)
            robot.connect(calibrate=False)
            self.ready.set()
            nxt = time.perf_counter()
            last_temp = 0.0
            while not self.stop.is_set():
                t0 = time.perf_counter()
                act = leader.get_action()
                t1 = time.perf_counter()
                sent = robot.send_action(act)
                t2 = time.perf_counter()
                pos = robot.bus.sync_read("Present_Position", num_retry=2)
                t3 = time.perf_counter()
                cur = robot.bus.sync_read("Present_Current", normalize=False, num_retry=2)
                load = robot.bus.sync_read("Present_Load", normalize=False, num_retry=2)
                vel = robot.bus.sync_read("Present_Velocity", normalize=False, num_retry=2)
                t4 = time.perf_counter()
                row = {"t0": t0, "t_act": t1, "t_sent": t2, "t_pos": t3, "t_extra": t4, "phase": self.phase}
                for m in MOTORS:
                    row[f"act_{m}"] = act[f"{m}.pos"]
                    row[f"sent_{m}"] = sent[f"{m}.pos"]
                    row[f"pos_{m}"] = pos[m]
                    row[f"cur_{m}"] = cur[m]
                    row[f"load_{m}"] = load[m]
                    row[f"vel_{m}"] = vel[m]
                if t0 - last_temp >= 1.0:
                    temp = robot.bus.sync_read("Present_Temperature", normalize=False, num_retry=2)
                    for m in MOTORS:
                        row[f"temp_{m}"] = temp[m]
                    last_temp = t0
                self.rows.append(row)
                self.latest = {m: (act[f"{m}.pos"], pos[m]) for m in MOTORS}
                nxt += 1 / FPS
                dt = nxt - time.perf_counter()
                if dt > 0:
                    time.sleep(dt)
                else:
                    nxt = time.perf_counter()
        except Exception as e:
            self.error = repr(e)
            self.ready.set()
        finally:
            for dev in (robot, leader):
                try:
                    if dev is not None and dev.is_connected:
                        dev.disconnect()
                except Exception as e:
                    print("[TELEOP] disconnect:", e)


def stage34(scr, ctx):
    idx = ctx["cams"]["index"]
    d = OUT / "s34"
    d.mkdir(exist_ok=True)
    pose_gate(scr, "3. テレオペの準備",
              ["フォロワーは休みの姿勢のまま. リーダーをフォロワーと同じ姿勢に合わせる",
               "差が小さくなったらスペース. フォロワーが動き出す"],
              None, "リーダー", "フォロワー")
    stop = threading.Event()
    cams = start_cams(idx, 640, 480, stop, video_dir=d, keep_small=True)
    tele = Teleop(stop)
    tele.start()
    tele.ready.wait(20)
    phases = []

    def set_phase(name):
        tele.phase = name
        phases.append((time.perf_counter(), name))

    def prev():
        return [c.latest for c in cams.values()]

    def status(_el=None):
        if not tele.latest:
            return []
        return ["差 (リーダー−フォロワー) " + "  ".join(f"{m[:5]} {a - p:+5.1f}" for m, (a, p) in tele.latest.items())]

    try:
        if tele.error:
            raise Abort(f"テレオペが始められない: {tele.error}")
        set_phase("free")
        scr.wait_space("3. テレオペ中 (ならし)", ["リーダーを少し動かして, フォロワーがついてくるのを確かめる",
                                             "次は 爪の開け閉め5回. 腕は止めて爪だけ動かす",
                                             "爪が手首カメラと机側カメラに映るようにしておく"])
        set_phase("grip_pre")
        scr.countdown("3. 腕を止めて待つ", 3, lambda el: ["腕も爪も止める"], previews_fn=prev)
        for k in range(5):
            set_phase(f"grip_{k + 1}")
            scr.countdown(f"3. 今! 爪をすばやく閉じて開く ({k + 1}/5)", 2.0,
                          lambda el: ["閉じる → すぐ開く. 腕は止めたまま"], previews_fn=prev, color=(0, 128, 255))
            set_phase(f"grip_{k + 1}_rest")
            scr.countdown(f"3. 止めて待つ ({k + 1}/5)", 3.0, lambda el: ["腕も爪も止める"], previews_fn=prev)
        for name, jp, note in [("slow", "ゆっくり", "1往復 5秒くらい"), ("normal", "ふつう", "1往復 2秒くらい"),
                               ("fast", "速く", "できるだけ速く. ただし物や人に当たらない範囲で")]:
            set_phase(f"{name}_pre")
            scr.wait_space(f"4. 次は「{jp}」15秒", [f"腕全体を大きく動かす ({note})", "肩・ひじ・手首・爪をぜんぶ使う",
                                                 "スペースで始める (テレオペは続いている)"])
            set_phase(name)
            scr.countdown(f"4. {jp}に大きく動かす", 15, lambda el: status(), previews_fn=prev,
                          color=(0, 128, 255) if name == "fast" else (0, 255, 0))
        set_phase("rest")
        scr.wait_space("4. 終わり. リーダーで休みの姿勢へ", ["フォロワーを机の上の休みの姿勢まで下ろす",
                                                      "スペースでテレオペを止める (フォロワーの力が抜ける)"])
    finally:
        stop.set()
        tele.join(10)
        for c in cams.values():
            c.join(5)
        save_rows(d / "teleop.csv", tele.rows)
        for k, c in cams.items():
            c.save(d / f"cam_{k}")
        (d / "phases.json").write_text(json.dumps({"phases": phases, "error": tele.error}, ensure_ascii=False, indent=2))


# ---- s5 lerobot-record ------------------------------------------------------------------

def run_sub(scr, title, cmd, log_path, lines_fn, color=(0, 255, 0)):
    """サブプロセスを動かしながら, ウィンドウに状況を出す. q で SIGINT を送る."""
    log = open(log_path, "w")
    p = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, bufsize=1, cwd=str(REPO))
    buf = []

    def reader():
        for ln in p.stdout:
            log.write(ln)
            log.flush()
            buf.append((time.perf_counter(), ln.rstrip()))
    th = threading.Thread(target=reader, daemon=True)
    th.start()
    t0 = time.perf_counter()
    try:
        while p.poll() is None:
            scr.show(title, lines_fn(buf, time.perf_counter() - t0),
                     [buf[-1][1][-110:] if buf else "", "q = 止める"], color)
            scr.key(100)
    except Abort:
        p.send_signal(signal.SIGINT)
        try:
            p.wait(20)
        except subprocess.TimeoutExpired:
            p.kill()
        raise
    finally:
        th.join(5)
        log.close()
    return p.returncode


def stage5(scr, ctx):
    idx = ctx["cams"]["index"]
    if DATASET_ROOT.exists():
        raise Abort(f"{DATASET_ROOT} がすでにある (上書きしない)")
    cams = (f"{{ wrist: {{type: opencv, index_or_path: {idx['wrist']}, width: 640, height: 480, fps: {FPS}, fourcc: MJPG}}, "
            f"desk: {{type: opencv, index_or_path: {idx['desk']}, width: 640, height: 480, fps: {FPS}, fourcc: MJPG}} }}")
    cmd = [sys.executable, "-m", "lerobot.scripts.lerobot_record",
           "--robot.type=so101_follower", f"--robot.port={FOLLOWER_PORT}", f"--robot.id={FOLLOWER_ID}",
           f"--robot.cameras={cams}",
           "--teleop.type=so101_leader", f"--teleop.port={LEADER_PORT}", f"--teleop.id={LEADER_ID}",
           f"--dataset.repo_id={DATASET_REPO}", f"--dataset.root={DATASET_ROOT}", "--dataset.no_stamp=true",
           "--dataset.push_to_hub=false", f"--dataset.fps={FPS}", "--dataset.single_task=check 258 practice",
           "--dataset.num_episodes=3", "--dataset.episode_time_s=10", "--dataset.reset_time_s=10",
           "--display_data=false", "--play_sounds=false"]
    (OUT / "s5_cmd.json").write_text(json.dumps(cmd, ensure_ascii=False, indent=2))
    pose_gate(scr, "5. LeRobot の記録 (10秒×3回)",
              ["各回: 休みの姿勢から始めて, 腕を動かし, 10秒で休みの姿勢に戻す",
               "回と回の間に10秒の休み (テレオペは続く)",
               "リーダーをフォロワーと同じ姿勢に合わせてスペース"],
              None, "リーダー", "フォロワー")

    def lines_fn(buf, el):
        state = "準備中"
        for _, ln in buf:
            if "Recording episode" in ln:
                state = "記録中: " + ln.split("Recording episode")[-1].strip() + " 回目 (0から)"
            elif "Reset the environment" in ln:
                state = "休み: 休みの姿勢に戻して待つ"
            elif "Stop recording" in ln:
                state = "記録おわり. 書き出し中"
        return [state, "10秒で 休みの姿勢 → 動かす → 休みの姿勢", "最後の回の終わりは必ず休みの姿勢 (力が抜ける)"]
    rc = run_sub(scr, "5. 記録中 (lerobot-record)", cmd, OUT / "s5_record.log", lines_fn)
    if rc != 0:
        raise Abort(f"lerobot-record が {rc} で終わった. s5_record.log を見る")


# ---- s6 lerobot-replay ------------------------------------------------------------------

def episode_first_action(ep):
    import pandas as pd
    files = sorted((DATASET_ROOT / "data").rglob("*.parquet"))
    df = pd.concat([pd.read_parquet(f) for f in files])
    df = df[df["episode_index"] == ep].sort_values("frame_index")
    a = df.iloc[0]["action"]
    return {m: float(a[i]) for i, m in enumerate(MOTORS)}


def stage6(scr, ctx, episode=0):
    first = episode_first_action(episode)
    for k in (1, 2):
        log_csv = OUT / f"s6_replay{k}.csv"
        if log_csv.exists():
            raise Abort(f"{log_csv} がすでにある")
        pose_gate(scr, f"6. 記録の {episode} 回目をフォロワーで再生 ({k}/2)",
                  ["記録と同じ速さでフォロワーが動く (約10秒). リーダーは使わない",
                   "フォロワーのまわりに物や手がないことを確かめる",
                   "差が大きいときは, 力の抜けたフォロワーを手で休みの姿勢に合わせる",
                   "よければスペースで再生を始める"],
                  lambda: first, "記録の最初", "フォロワー")
        cmd = [sys.executable, str(HERE / "replay_logged_258.py"), str(log_csv),
               "--robot.type=so101_follower", f"--robot.port={FOLLOWER_PORT}", f"--robot.id={FOLLOWER_ID}",
               f"--dataset.repo_id={DATASET_REPO}", f"--dataset.root={DATASET_ROOT}", f"--dataset.episode={episode}",
               "--play_sounds=false"]
        rc = run_sub(scr, f"6. 再生中 ({k}/2)", cmd, OUT / f"s6_replay{k}.log",
                     lambda buf, el: ["フォロワーが記録どおりに動いています", f"{el:4.1f} 秒"], color=(0, 128, 255))
        if rc != 0:
            raise Abort(f"再生が {rc} で終わった. s6_replay{k}.log を見る")
        scr.countdown("6. 再生おわり", 2, lambda el: ["フォロワーの力が抜けています"])


# ---- 本体 -------------------------------------------------------------------------------

STAGES = {"s0": stage0, "s1": stage1, "s2": stage2, "s34": stage34, "s5": stage5, "s6": stage6}


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--only", nargs="*", choices=list(STAGES), help="行う段階 (省略で全部)")
    args = ap.parse_args()
    OUT.mkdir(parents=True, exist_ok=True)
    todo = args.only or list(STAGES)
    if "s0" not in todo:
        todo = ["s0"] + todo  # カメラの番号は毎回名前で引き直す
    scr = Screen()
    ctx = {}
    done, status = [], "ok"
    try:
        for s in todo:
            print(f"[STAGE] {s} 始め {datetime.now():%H:%M:%S}")
            STAGES[s](scr, ctx)
            done.append(s)
        scr.show("ぜんぶ終わり", ["おつかれさまでした. 何かキーで閉じる"], color=(0, 255, 0))
        cv2.waitKey(10000)
    except Abort as e:
        status = f"止めた: {e}"
        print("[ABORT]", e)
        try:
            scr.show("止めました", [str(e)[:60]], color=(0, 0, 255))
            cv2.waitKey(3000)
        except Exception:
            pass
    finally:
        with open(OUT / "run_log.jsonl", "a") as fh:
            fh.write(json.dumps({"time": datetime.now().isoformat(), "todo": todo, "done": done, "status": status},
                                ensure_ascii=False) + "\n")
        cv2.destroyAllWindows()
    return 0 if status == "ok" else 1


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