#!/usr/bin/env python3
"""measure_timing_alignment.py — 録画と記録から「映像と関節の時刻のずれ」と「制御の1回の間隔」を測る (依頼 249).

読むだけ. アーム・サーボ・daemon には一切つながない. runs/ と datasets/ の既存ファイルは書き換えない.

対象 (3 組):
  demo     : datasets/mecharm_redblock_box_split10_v2 の全エピソード
             元の記録 = runs/run_pick_place/{run}/ (record_teach_demo.py. build_demo_episodes.py で作った)
  policy   : runs/pi05_policy/ の直近 N 本 (recording/ がある run. 既定 20)
  pad_corr : ゲームパッド補正 (log.jsonl に type=pad) の run のうち, datasets/mecharm_redblock_box_corr_v5
             に入っている本 (build_correction_episodes.py --gap-fill speed20 --trim-hold で作った)

測ること:
  1. 映像と関節の時刻のずれ
     - 1 コマごとに capture_ts といちばん近い actual の時刻の差
     - コマ間隔・関節を読む間隔 (狙いの値と実際. 読み取りの失敗の数)
     - 学習データでの組み方 (state = 10Hz の格子 t に actual を線形補間. 画像 = t にいちばん近いコマ)
       を作り直し, エピソードの state と突き合わせて同じ格子を見つけたうえで, 組のずれを出す
     - カメラ自体の遅れ: 動き出しの場面で, 映像のコマ間差分 (動きの量) と関節の速さの立ち上がりの時刻を比べる.
       あわせて全体の相互相関の山の位置も出す
  2. 制御の1回の間隔
     - 指令を送った間隔・送ってから届くまで・サーボの読み取り時間・推論 1 回の待ち (latency_s)
     - 中央値の 2 倍を超えて空いた所の数と, つかむ前/つかんだ後/放した後 のどこか
  3. demo / policy / pad_corr の比べ

出力: /Volumes/KIOXIA/r2r2r-data/diag/timing_249/{YYYYmmdd_HHMMSS}/
  per_run.csv  pairs_dataset.csv  frame_actual_offset.csv  gaps.csv  camera_lag_onsets.csv
  camera_lag_xcorr.csv  summary.md  fig_*.png

Usage:
  python3 diag/measure_timing_alignment.py
  python3 diag/measure_timing_alignment.py --policy-n 20 --no-video   # カメラの遅れを測らない (速い)
"""
import argparse
import csv
import json
import os
import sys
from datetime import datetime
from pathlib import Path

import cv2
import numpy as np

REPO = Path(__file__).resolve().parent.parent
sys.path.insert(0, str(REPO / "pi05"))

import build_demo_episodes as bde  # noqa: E402  (読むための関数だけ使う)
import build_correction_episodes as bce  # noqa: E402
from rewind_correct import excluded_windows, step_keep_mask  # noqa: E402

DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data")
OUT_ROOT = DATA_ROOT / "diag" / "timing_249"
DEMO_DS = DATA_ROOT / "datasets" / "mecharm_redblock_box_split10_v2"
CORR_DS = DATA_ROOT / "datasets" / "mecharm_redblock_box_corr_v5"
RUNS_PP = DATA_ROOT / "runs" / "run_pick_place"
RUNS_POL = DATA_ROOT / "runs" / "pi05_policy"
FPS = bde.FPS

# カメラの遅れの見積もり
VID_SMALL = (96, 54)            # 差分を取る大きさ
STILL_DEG_S = 1.0               # これ未満は止まり
MOVE_DEG_S = 5.0                # これを超えたら動き
STILL_BEFORE_SEC = 0.8          # 動き出しの前に止まっている長さ
MAX_JOINT_GAP_SEC = 0.35        # 動き出しをはさむ 2 つの読みの間がこれ以下のときだけ使う
VID_K_MAD = 6.0                 # 映像の動き出し = 止まりの中央値 + この倍の MAD を超えた最初のコマ
XCORR_LAGS = np.arange(-0.30, 0.6001, 0.01)
XCORR_GRID = 0.01


def abort(msg):
    sys.exit("[ABORT] " + msg)


def ensure_ssd_mounted():
    if not os.path.ismount("/Volumes/KIOXIA"):
        abort("KIOXIA が未マウント。ローカルには書きません。")
    if not DATA_ROOT.exists():
        abort(f"データの場所がない: {DATA_ROOT}")


def st(x):
    """中央値・95%点・最大 (と数)."""
    x = np.asarray([v for v in x if v is not None and np.isfinite(v)], dtype=float)
    if len(x) == 0:
        return {"n": 0, "median": None, "p95": None, "max": None}
    return {"n": int(len(x)), "median": float(np.median(x)), "p95": float(np.percentile(x, 95)),
            "max": float(np.max(x))}


def fmt(s, scale=1.0, nd=1):
    if not s or s["n"] == 0:
        return "—"
    return f"{s['median'] * scale:.{nd}f} / {s['p95'] * scale:.{nd}f} / {s['max'] * scale:.{nd}f} (n={s['n']})"


def visible(names):
    return sorted(n for n in names if not n.startswith("._"))


# ---------------------------------------------------------------- 記録を読む
def read_states(path):
    """states.csv を全部読む (actual は build_demo_episodes と同じ決まりで弾いた数も数える)."""
    rows = []
    with path.open(newline="") as f:
        for r in csv.DictReader(f):
            try:
                r["_ts"] = float(r["ts"])
            except Exception:
                continue
            rows.append(r)
    act_t, act_note, invalid = [], [], 0
    for r in rows:
        if r.get("event_type") != "actual":
            continue
        try:
            q = [float(r[f"j{i}"]) for i in range(6)]
        except Exception:
            invalid += 1
            continue
        if any(abs(v) > bde.ANGLE_ABS_MAX for v in q):
            invalid += 1
            continue
        act_t.append(r["_ts"]); act_note.append(r.get("note") or "")
    return rows, np.array(act_t), act_note, invalid


def gripper_events(rows):
    ev = sorted((r["_ts"], int(float(r["gripper_state"]))) for r in rows
                if r.get("event_type") == "gripper_cmd" and r.get("gripper_state") not in (None, ""))
    return ev


def phase_of(ts, gev):
    """つかむ前 / つかんだ後 / 放した後. 最初の閉じる指令とその後の最初の開く指令で分ける."""
    t_close = next((t for t, s in gev if s == 1), None)
    if t_close is None:
        return "つかむ前(閉じる指令なし)"
    t_open = next((t for t, s in gev if s == 0 and t > t_close), None)
    if ts < t_close:
        return "つかむ前"
    if t_open is None or ts < t_open:
        return "つかんだ後"
    return "放した後"


def stage_at(rows, ts):
    """その時刻の stage 列 (demo は区間名, policy は model/correct/wait など)."""
    s = ""
    for r in rows:
        if r["_ts"] > ts:
            break
        if r.get("stage"):
            s = r["stage"]
    return s


def read_currents_ms(path):
    out = []
    if not path.exists():
        return out
    with path.open(newline="") as f:
        for r in csv.DictReader(f):
            try:
                out.append(float(r["rpc_ms"]))
            except Exception:
                pass
    return out


def read_log(path):
    out = []
    if not path.exists():
        return out
    for line in path.open(errors="ignore"):
        try:
            out.append(json.loads(line))
        except Exception:
            pass
    return out


def note_val(note, key):
    for tok in (note or "").split():
        if tok.startswith(key + "="):
            try:
                return float(tok.split("=", 1)[1])
            except ValueError:
                return None
    return None


# ---------------------------------------------------------------- カメラの遅れ
def video_motion(mp4, fts):
    """コマ間差分の大きさ (1 秒あたり). 時刻は 2 コマの capture_ts の中点."""
    cap = cv2.VideoCapture(str(mp4))
    prev, vals, mids, dts, i = None, [], [], [], 0
    while i < len(fts):
        ok, fr = cap.read()
        if not ok:
            break
        g = cv2.cvtColor(cv2.resize(fr, VID_SMALL, interpolation=cv2.INTER_AREA), cv2.COLOR_BGR2GRAY)
        g = cv2.GaussianBlur(g, (3, 3), 0).astype(np.float32)
        if prev is not None:
            dt = fts[i] - fts[i - 1]
            if dt > 0:
                vals.append(float(np.mean(np.abs(g - prev))) / dt)
                mids.append((fts[i] + fts[i - 1]) / 2); dts.append(dt)
        prev = g; i += 1
    cap.release()
    return np.array(mids), np.array(vals), np.array(dts), i


def joint_speed(at, aq):
    dt = np.diff(at)
    ok = dt > 1e-3
    v = np.full(len(dt), np.nan)
    v[ok] = np.max(np.abs(np.diff(aq, axis=0))[ok], axis=1) / dt[ok]
    return v, dt


def onset_lags(at, aq, vm_t, vm_v, vm_dt):
    """動き出しごとに 映像の立ち上がり − 関節の立ち上がり."""
    v, dt = joint_speed(at, aq)
    res = []
    if len(vm_v) < 10:
        return res
    for k in range(1, len(v)):
        if not (np.isfinite(v[k]) and v[k] > MOVE_DEG_S and dt[k] <= MAX_JOINT_GAP_SEC):
            continue
        # 区間 k (at[k]→at[k+1]) で動き出した. その前 STILL_BEFORE_SEC は止まり
        j = k - 1; still_ok = True
        while j >= 0 and at[k] - at[j] < STILL_BEFORE_SEC:
            if not np.isfinite(v[j]) or v[j] >= STILL_DEG_S or dt[j] > MAX_JOINT_GAP_SEC:
                still_ok = False; break
            j -= 1
        if not still_ok or j < 0:
            continue
        t_joint = (at[k] + at[k + 1]) / 2
        unc_joint = (at[k + 1] - at[k]) / 2
        base = (vm_t >= at[k] - STILL_BEFORE_SEC) & (vm_t <= at[k])
        if base.sum() < 2:
            continue
        b = vm_v[base]; med = np.median(b); mad = np.median(np.abs(b - med)) + 1e-6
        thr = med + VID_K_MAD * 1.4826 * mad
        win = np.where((vm_t > at[k] - 0.2) & (vm_t <= at[k] + 1.5) & (vm_v > thr))[0]
        if len(win) == 0:
            continue
        n = win[0]
        res.append({"t_joint_onset": t_joint, "joint_unc_s": unc_joint, "t_video_onset": vm_t[n],
                    "video_unc_s": vm_dt[n] / 2, "lag_s": vm_t[n] - t_joint,
                    "unc_s": unc_joint + vm_dt[n] / 2, "joint_speed": float(v[k]),
                    "video_thr": float(thr), "video_val": float(vm_v[n])})
    return res


def xcorr_lag(at, aq, vm_t, vm_v):
    """関節の速さと映像の動きの量の相互相関. 正 = 映像が遅れている."""
    v, dt = joint_speed(at, aq)
    mid = (at[1:] + at[:-1]) / 2
    ok = np.isfinite(v) & (dt <= MAX_JOINT_GAP_SEC)
    if ok.sum() < 20 or len(vm_v) < 20:
        return None, None
    jt, jv = mid[ok], v[ok]
    t0 = max(jt[0], vm_t[0]) + 0.7; t1 = min(jt[-1], vm_t[-1]) - 0.7
    if t1 - t0 < 5:
        return None, None
    grid = np.arange(t0, t1, XCORR_GRID)
    # 関節の速さが分かっている所 (読みの間が短い所) だけを使う
    near = np.min(np.abs(grid[:, None] - jt[None, :]), axis=1) if len(grid) * len(jt) < 4e7 else None
    jg = np.interp(grid, jt, jv)
    best, best_r = None, -2
    for L in XCORR_LAGS:
        vg = np.interp(grid + L, vm_t, vm_v)
        m = np.ones(len(grid), bool) if near is None else near <= MAX_JOINT_GAP_SEC
        if m.sum() < 50:
            continue
        a = jg[m] - jg[m].mean(); b = vg[m] - vg[m].mean()
        den = np.sqrt((a * a).sum() * (b * b).sum())
        r = float((a * b).sum() / den) if den > 0 else -2
        if r > best_r:
            best_r, best = r, float(L)
    return best, best_r


# ---------------------------------------------------------------- 学習データの組を作り直す
def rebuild_pairs(group, run_dir, ep_dir):
    """エピソードの state と同じ 10Hz 格子を見つけ, 各ステップの画像と関節の組のずれを出す."""
    rec = run_dir / "recording"
    at, aq, _gt, _gs = bde.load_states(rec / "states.csv")
    order = np.argsort(at); at, aq = at[order], aq[order]
    at, aq = bde.drop_spikes(at, aq)
    fidx, fts = bde.load_frame_log(rec / "top_frame_log.csv")
    t0, t1 = max(fts[0], at[0]), min(fts[-1], at[-1])
    t = np.arange(t0, t1, 1.0 / FPS)
    meta = json.loads((ep_dir / "meta.json").read_text())
    if group == "demo":
        q = np.stack([np.interp(t, at, aq[:, k]) for k in range(6)], axis=1)
        cand = np.ones(len(fts), bool)
    else:
        mode = (meta.get("gap_fill") or {}).get("mode", "linear")
        q, _ = bce.fill_motion_gaps(t, at, aq, mode=mode)
        log = bce.load_log(run_dir / "log.jsonl")
        mode_ts, modes = bce.load_stage_times(rec / "states.csv")
        cand, _ = step_keep_mask(fts, mode_ts, modes, excluded_windows(log))
    ep = np.load(ep_dir / "episode.npz")
    trel = ep["t_orig"] if "t_orig" in ep.files else ep["t"]
    offs = np.round(trel * FPS).astype(int)
    S = ep["state"][:, :6].astype(np.float64)
    best_n0, best_err = None, np.inf
    for n0 in range(0, len(t) - offs[-1]):
        e = float(np.max(np.abs(q[n0 + offs] - S)))
        if e < best_err:
            best_err, best_n0 = e, n0
            if e < 1e-3:
                break
    tt = t[best_n0 + offs]
    cf = fts[cand]
    nn = np.clip(np.searchsorted(cf, tt), 1, len(cf) - 1)
    left, right = cf[nn - 1], cf[nn]
    nn = np.where(np.abs(tt - left) <= np.abs(right - tt), nn - 1, nn)
    frame_off = cf[nn] - tt
    k = np.clip(np.searchsorted(at, tt), 1, len(at) - 1)
    near_actual = np.minimum(np.abs(at[k - 1] - tt), np.abs(at[k] - tt))
    bracket = at[k] - at[k - 1]
    moved = np.max(np.abs(aq[k] - aq[k - 1]), axis=1)
    rows = []
    for i in range(len(tt)):
        rows.append({"group": group, "run": run_dir.name, "step": i, "t_abs": f"{tt[i]:.6f}",
                     "frame_minus_state_s": round(float(frame_off[i]), 4),
                     "nearest_actual_s": round(float(near_actual[i]), 4),
                     "actual_bracket_s": round(float(bracket[i]), 4),
                     "bracket_move_deg": round(float(moved[i]), 3),
                     "interp_over_gap": int(bracket[i] > bce.GAP_SEC)})
    return rows, best_err


# ---------------------------------------------------------------- 補間の誤差の見積もり
DEMO_BLOCK_SEC = 1.04           # お手本の送る命令の往復 (send_rtt 中央値). この間 actual が読めない


def interp_error_sim(run_dir, block=DEMO_BLOCK_SEC):
    """実機推論の密な actual で, 送るたびに block 秒読めなかったことにして線形補間し, 消した実測との差を出す.
    戻り値は 消した実測 1 点ごとの 6 関節の最大誤差 (度) と, その 2 点の間に動いた量 (度)."""
    rows, _t, _n, _i = read_states(run_dir / "recording" / "states.csv")
    at, aq, _gt, _gs = bde.load_states(run_dir / "recording" / "states.csv")
    o = np.argsort(at); at, aq = bde.drop_spikes(at[o], aq[o])
    sends = [r["_ts"] for r in rows if r.get("event_type") == "send_angles"]
    errs, moves = [], []
    for ts in sends:
        drop = (at > ts) & (at < ts + block)
        keep_l = np.where(at <= ts)[0]; keep_r = np.where(at >= ts + block)[0]
        if drop.sum() == 0 or len(keep_l) == 0 or len(keep_r) == 0:
            continue
        a, b = keep_l[-1], keep_r[0]
        if at[b] - at[a] > block + 0.3:      # もともと抜けていた所は使わない
            continue
        idx = np.where(drop)[0]
        w = (at[idx] - at[a]) / (at[b] - at[a])
        est = aq[a][None, :] + w[:, None] * (aq[b] - aq[a])[None, :]
        e = np.max(np.abs(est - aq[idx]), axis=1)
        errs += list(e); moves += [float(np.max(np.abs(aq[b] - aq[a])))] * len(e)
    return errs, moves


# ---------------------------------------------------------------- 1 run を測る
def measure_run(group, run_dir, do_video):
    rec = run_dir / "recording"
    rows, at_all, notes, invalid = read_states(rec / "states.csv")
    at_all = np.sort(at_all)
    fidx, fts = bde.load_frame_log(rec / "top_frame_log.csv")
    gev = gripper_events(rows)
    res = {"group": group, "run": run_dir.name}
    out_off, out_gaps, out_on = [], [], []

    # コマ間隔・読み間隔
    fdt = np.diff(fts); adt = np.diff(at_all)
    res["frame_dt"] = st(fdt); res["actual_dt"] = st(adt)
    res["n_frames"] = int(len(fts)); res["n_actual"] = int(len(at_all)); res["actual_invalid"] = invalid
    if group == "demo":
        trial = json.loads((run_dir / f"{run_dir.name}.json").read_text())
        rm = json.loads((rec / "currents_meta.json").read_text()) if (rec / "currents_meta.json").exists() else {}
        asmp = trial.get("actual_sampling") or {}
        res["frame_target"] = "指定なし (カメラ任せ. 約30fps)"
        res["actual_target"] = f"本体 {asmp.get('target_hz')}Hz + 記録スレッド 最大 {rm.get('target_hz_max')}Hz (本体が眠る間だけ)"
        res["read_fail"] = int((asmp.get("reads_attempted", 0) - asmp.get("reads_ok", 0))
                               + (asmp.get("reads_once_attempted", 0) - asmp.get("reads_once_ok", 0))
                               + rm.get("currents_rows_incomplete", 0) + rm.get("loop_errors", 0))
        tm_p = rec / "top_meta.json"
        res["top_status"] = json.loads(tm_p.read_text()).get("status") if tm_p.exists() else "top_meta.json なし"
    else:
        tm_p = rec / "top_meta.json"
        tm = json.loads(tm_p.read_text()) if tm_p.exists() else {"status": "top_meta.json なし"}
        res["frame_target"] = "10.0fps"     # policy_recorder.RECORD_FPS
        res["actual_target"] = "記録スレッド 10Hz + 本体の読み (note=main)"
        res["read_fail"] = int(tm.get("actual_read_failures", 0)) + int(tm.get("currents_rows_incomplete", 0))
        res["top_status"] = tm.get("status")

    # 1 コマごとの いちばん近い actual との差 (actual − capture)
    k = np.clip(np.searchsorted(at_all, fts), 1, len(at_all) - 1)
    dl, dr = at_all[k - 1] - fts, at_all[k] - fts
    near = np.where(np.abs(dl) <= np.abs(dr), dl, dr)
    res["frame_actual_abs"] = st(np.abs(near))
    for i in range(len(fts)):
        out_off.append({"group": group, "run": run_dir.name, "frame_idx": int(fidx[i]),
                        "capture_ts": f"{fts[i]:.6f}", "nearest_actual_minus_capture_s": round(float(near[i]), 4)})

    # サーボの読み取り時間
    res["servo_read_ms"] = st(read_currents_ms(rec / "currents.csv"))

    # 指令・到着・推論
    if group == "demo":
        trial = json.loads((run_dir / f"{run_dir.name}.json").read_text())
        segs = trial.get("segments") or []
        send_ts = [r["_ts"] for r in rows if r.get("event_type") == "stage"]
        res["send_dt"] = st(np.diff(send_ts))
        res["send_rpc"] = st([s.get("send_rtt_sec") for s in segs if s.get("kind") == "move"])
        res["send_to_arrival"] = st([s.get("segment_elapsed_sec") for s in segs if s.get("kind") == "move"])
        res["latency"] = st([])
        res["infer_dt"] = st([])
        res["send_note"] = "stage 行 = 区間 1 つにつき 1 回送る (single_send_per_segment). 届く = 到着確認まで (segment_elapsed_sec)"
    else:
        sends = [r for r in rows if r.get("event_type") == "send_angles"]
        send_ts = [r["_ts"] for r in sends]
        res["send_dt"] = st(np.diff(send_ts))
        res["send_rpc"] = st([note_val(r.get("note"), "rpc_sec") for r in sends])
        log = read_log(run_dir / "log.jsonl")
        chunks = [j for j in log if "chunk" in j and "type" not in j]
        res["latency"] = st([c.get("latency_s") for c in chunks])
        res["infer_dt"] = st(np.diff([c["t"] for c in chunks if "t" in c]))
        ex = [c["t_exec_end"] - c["t_exec_start"] for c in chunks
              if c.get("t_exec_end") and c.get("t_exec_start")]
        pads = [p.get("elapsed_sec") for p in log if p.get("type") == "pad" and p.get("elapsed_sec") is not None]
        res["send_to_arrival"] = st(ex)
        res["pad_send_to_reach"] = st(pads)
        res["ts_arrival_recorded"] = sum(1 for r in sends if r.get("ts_arrival"))
        res["send_note"] = "send_angles 行. 届く = チャンク 1 つの実行 (t_exec_start→t_exec_end. 中継点こみ). ts_arrival は空"

    # 間隔が大きく空いた所
    for kind, ts in (("send", np.array(send_ts)), ("actual", at_all), ("frame", fts)):
        if len(ts) < 3:
            continue
        d = np.diff(ts); med = float(np.median(d))
        for i in np.where(d > 2 * med)[0]:
            out_gaps.append({"group": group, "run": run_dir.name, "kind": kind,
                             "t_start": f"{ts[i]:.6f}", "gap_s": round(float(d[i]), 4),
                             "median_s": round(med, 4), "phase": phase_of(ts[i], gev),
                             "stage": stage_at(rows, ts[i])})

    # カメラの遅れ
    if do_video:
        at, aq, _gt, _gs = bde.load_states(rec / "states.csv")
        o = np.argsort(at); at, aq = bde.drop_spikes(at[o], aq[o])
        vm_t, vm_v, vm_dt, nread = video_motion(rec / "top.mp4", fts)
        res["video_frames_read"] = nread
        for r in onset_lags(at, aq, vm_t, vm_v, vm_dt):
            r.update({"group": group, "run": run_dir.name, "phase": phase_of(r["t_joint_onset"], gev)})
            out_on.append(r)
        res["xcorr_lag_s"], res["xcorr_r"] = xcorr_lag(at, aq, vm_t, vm_v)
    return res, out_off, out_gaps, out_on


# ---------------------------------------------------------------- まとめ
def pick_runs(policy_n):
    demo_eps = visible(os.listdir(DEMO_DS / "raw_episodes"))
    demo = [(RUNS_PP / e, DEMO_DS / "raw_episodes" / e) for e in demo_eps]
    pol_all = [d for d in visible(os.listdir(RUNS_POL))
               if d.startswith("pi05_") and (RUNS_POL / d / "recording" / "states.csv").exists()
               and (RUNS_POL / d / "recording" / "top_frame_log.csv").exists()]
    policy = [(RUNS_POL / d, None) for d in pol_all[-policy_n:]]
    pad_all = [d for d in pol_all if any('"type": "pad"' in ln for ln in
                                          (RUNS_POL / d / "log.jsonl").open(errors="ignore"))]
    corr_eps = set(visible(os.listdir(CORR_DS / "raw_episodes")))
    pad = [(RUNS_POL / d, CORR_DS / "raw_episodes" / d) for d in pad_all if d in corr_eps]
    return demo, policy, pad, pad_all


def write_csv(path, rows):
    if not rows:
        path.write_text("")
        return
    keys = list(rows[0].keys())
    for r in rows:
        for kk in r:
            if kk not in keys:
                keys.append(kk)
    with path.open("w", newline="") as f:
        w = csv.DictWriter(f, fieldnames=keys)
        w.writeheader(); w.writerows(rows)


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--policy-n", type=int, default=20)
    ap.add_argument("--no-video", action="store_true", help="カメラの遅れを測らない")
    args = ap.parse_args()

    ensure_ssd_mounted()
    out = OUT_ROOT / datetime.now().strftime("%Y%m%d_%H%M%S")
    if out.exists():
        abort(f"同じ名前の出力先がある: {out}")
    out.mkdir(parents=True)

    demo, policy, pad, pad_all = pick_runs(args.policy_n)
    print(f"[INFO] demo {len(demo)} / policy {len(policy)} / pad_corr {len(pad)} (pad の run 全体 {len(pad_all)})")

    results, offs, gaps, onsets, pairs, match = [], [], [], [], [], []
    for group, items in (("demo", demo), ("policy", policy), ("pad_corr", pad)):
        for run_dir, ep_dir in items:
            print(f"[RUN] {group} {run_dir.name}")
            try:
                r, o, g, on = measure_run(group, run_dir, not args.no_video)
            except Exception as e:
                print(f"[WARN] {run_dir.name}: {type(e).__name__}: {e}")
                results.append({"group": group, "run": run_dir.name, "error": f"{type(e).__name__}: {e}"})
                continue
            results.append(r); offs += o; gaps += g; onsets += on
            if ep_dir is not None:
                try:
                    pr, err = rebuild_pairs(group, run_dir, ep_dir)
                    pairs += pr
                    match.append({"group": group, "run": run_dir.name, "state_match_max_err_deg": round(err, 5)})
                except Exception as e:
                    print(f"[WARN] 組の作り直し {run_dir.name}: {type(e).__name__}: {e}")
                    match.append({"group": group, "run": run_dir.name, "state_match_max_err_deg": None,
                                  "error": f"{type(e).__name__}: {e}"})

    def vals(rows, group, key, fn=float):
        return [fn(r[key]) for r in rows if r["group"] == group and r.get(key) not in (None, "")]

    groups = ["demo", "policy", "pad_corr"]
    G = {}
    for g in groups:
        rr = [r for r in results if r["group"] == g and "error" not in r]
        G[g] = {"runs": len(rr)}
        G[g]["frame_actual_abs"] = st([abs(v) for v in vals(offs, g, "nearest_actual_minus_capture_s")])
        G[g]["frame_actual_signed_med"] = st(vals(offs, g, "nearest_actual_minus_capture_s"))["median"]
        G[g]["n_frames"] = sum(r["n_frames"] for r in rr)
        G[g]["n_actual"] = sum(r["n_actual"] for r in rr)
        G[g]["read_fail"] = sum(r.get("read_fail", 0) for r in rr)
        G[g]["actual_invalid"] = sum(r.get("actual_invalid", 0) for r in rr)
        G[g]["frame_target"] = rr[0]["frame_target"] if rr else "—"
        G[g]["actual_target"] = rr[0]["actual_target"] if rr else "—"
        G[g]["send_note"] = rr[0]["send_note"] if rr else "—"
        G[g]["xcorr"] = [(r["run"], r.get("xcorr_lag_s"), r.get("xcorr_r")) for r in rr]
        G[g]["pairs_frame"] = st([abs(v) for v in vals(pairs, g, "frame_minus_state_s")])
        G[g]["pairs_frame_signed"] = st(vals(pairs, g, "frame_minus_state_s"))
        G[g]["pairs_near_actual"] = st(vals(pairs, g, "nearest_actual_s"))
        G[g]["pairs_bracket"] = st(vals(pairs, g, "actual_bracket_s"))
        pg = [p for p in pairs if p["group"] == g]
        G[g]["pairs_n"] = len(pg)
        G[g]["pairs_over_gap"] = sum(p["interp_over_gap"] for p in pg)
        G[g]["pairs_over_gap_moving"] = sum(1 for p in pg if p["interp_over_gap"] and p["bracket_move_deg"] > 0.5)
        G[g]["onset"] = st([o["lag_s"] for o in onsets if o["group"] == g])
        G[g]["onset_unc"] = st([o["unc_s"] for o in onsets if o["group"] == g])
        G[g]["gaps"] = {}
        for p in [x for x in gaps if x["group"] == g]:
            key = (p["kind"], p["phase"])
            G[g]["gaps"][key] = G[g]["gaps"].get(key, 0) + 1
    # 元の値を合わせた統計 (run ごとの統計の統計にしない)
    pooled = pool_raw(results, demo, policy, pad)
    for g in groups:
        G[g].update(pooled[g])

    sim_e, sim_m = [], []
    for run_dir, _ in policy:
        try:
            e, m = interp_error_sim(run_dir)
            sim_e += e; sim_m += m
        except Exception as ex:
            print(f"[WARN] 補間の見積もり {run_dir.name}: {type(ex).__name__}: {ex}")
    G["sim"] = {"err": st(sim_e), "err_moving": st([e for e, m in zip(sim_e, sim_m) if m > 0.5]),
                "n_send_windows": len(set(sim_m)) if sim_m else 0}

    write_csv(out / "per_run.csv", [flatten(r) for r in results])
    write_csv(out / "frame_actual_offset.csv", offs)
    write_csv(out / "gaps.csv", gaps)
    write_csv(out / "camera_lag_onsets.csv", onsets)
    write_csv(out / "camera_lag_xcorr.csv",
              [{"group": g, "run": a, "lag_s": b, "r": c} for g in groups for a, b, c in G[g]["xcorr"]])
    write_csv(out / "pairs_dataset.csv", pairs)
    write_csv(out / "state_match.csv", match)
    make_figs(out, offs, pairs, onsets, G)
    write_summary(out, G, demo, policy, pad, pad_all, match, args)
    print(f"[DONE] {out}")


def pool_raw(results, demo, policy, pad):
    """各 run の元の値を合わせた統計."""
    out = {}
    for g, items in (("demo", demo), ("policy", policy), ("pad_corr", pad)):
        fdt, adt, srv, sdt, rpc, arr, lat, inf, padr = [], [], [], [], [], [], [], [], []
        for run_dir, _ in items:
            rec = run_dir / "recording"
            try:
                rows, at_all, _n, _inv = read_states(rec / "states.csv")
                _fi, fts = bde.load_frame_log(rec / "top_frame_log.csv")
            except Exception:
                continue
            fdt += list(np.diff(fts)); adt += list(np.diff(np.sort(at_all)))
            srv += read_currents_ms(rec / "currents.csv")
            if g == "demo":
                trial = json.loads((run_dir / f"{run_dir.name}.json").read_text())
                segs = [s for s in trial.get("segments") or [] if s.get("kind") == "move"]
                sdt += list(np.diff([r["_ts"] for r in rows if r.get("event_type") == "stage"]))
                rpc += [s.get("send_rtt_sec") for s in segs]
                arr += [s.get("segment_elapsed_sec") for s in segs]
            else:
                sends = [r for r in rows if r.get("event_type") == "send_angles"]
                sdt += list(np.diff([r["_ts"] for r in sends]))
                rpc += [note_val(r.get("note"), "rpc_sec") for r in sends]
                log = read_log(run_dir / "log.jsonl")
                ch = [j for j in log if "chunk" in j and "type" not in j]
                lat += [c.get("latency_s") for c in ch]
                inf += list(np.diff([c["t"] for c in ch if "t" in c]))
                arr += [c["t_exec_end"] - c["t_exec_start"] for c in ch if c.get("t_exec_end") and c.get("t_exec_start")]
                padr += [p.get("elapsed_sec") for p in log if p.get("type") == "pad" and p.get("elapsed_sec") is not None]
        out[g] = {"P_frame_dt": st(fdt), "P_actual_dt": st(adt), "P_servo_ms": st(srv), "P_send_dt": st(sdt),
                  "P_send_rpc": st(rpc), "P_arrival": st(arr), "P_latency": st(lat), "P_infer_dt": st(inf),
                  "P_pad_reach": st(padr)}
    return out


def flatten(r):
    o = {}
    for k, v in r.items():
        if isinstance(v, dict) and "median" in v:
            for kk in ("n", "median", "p95", "max"):
                o[f"{k}_{kk}"] = None if v[kk] is None else (round(v[kk], 5) if isinstance(v[kk], float) else v[kk])
        else:
            o[k] = v
    return o


def make_figs(out, offs, pairs, onsets, G):
    try:
        import matplotlib
        matplotlib.use("Agg")
        import matplotlib.pyplot as plt
    except Exception as e:
        print(f"[WARN] 図を作れない: {e}")
        return
    colors = {"demo": "#2a6fdb", "policy": "#e07b39", "pad_corr": "#3a9e5f"}
    fig, axes = plt.subplots(1, 3, figsize=(15, 4))
    for g, c in colors.items():
        v = [abs(float(o["nearest_actual_minus_capture_s"])) * 1000 for o in offs if o["group"] == g]
        if v:
            axes[0].hist(v, bins=60, range=(0, 600), alpha=0.5, color=c, label=g, density=True)
        p = [float(x["frame_minus_state_s"]) * 1000 for x in pairs if x["group"] == g]
        if p:
            axes[1].hist(p, bins=40, range=(-80, 80), alpha=0.5, color=c, label=g, density=True)
        on = [o["lag_s"] * 1000 for o in onsets if o["group"] == g]
        if on:
            axes[2].hist(on, bins=30, range=(-300, 600), alpha=0.5, color=c, label=g)
    axes[0].set_title("1 frame: |nearest actual - capture_ts| (ms)")
    axes[1].set_title("dataset pair: frame capture_ts - state t (ms)")
    axes[2].set_title("camera lag by motion onset (ms, + = video later)")
    for a in axes:
        a.legend(); a.grid(alpha=0.3)
    fig.tight_layout(); fig.savefig(out / "fig_offsets.png", dpi=110); plt.close(fig)


def write_summary(out, G, demo, policy, pad, pad_all, match, args):
    L = []
    gn = {"demo": "お手本 (split10_v2)", "policy": f"実機推論 (直近{len(policy)}本)", "pad_corr": "補正お手本 (corr_v5 のゲームパッド分)"}
    groups = ["demo", "policy", "pad_corr"]
    L.append("# 依頼249 映像と関節の時刻のずれ・制御の間隔\n")
    L.append(f"作成 {datetime.now().isoformat(timespec='seconds')} / `diag/measure_timing_alignment.py`"
             f"{' --no-video' if args.no_video else ''}\n")
    L.append("読んだ記録:\n")
    L.append(f"- お手本: {DEMO_DS}/raw_episodes ({len(demo)} 本) ← 元 {RUNS_PP}/<run>/recording")
    L.append(f"- 実機推論: {RUNS_POL} の {policy[0][0].name} 〜 {policy[-1][0].name} ({len(policy)} 本)" if policy else "- 実機推論: なし")
    L.append(f"- 補正お手本: {CORR_DS}/raw_episodes のうちゲームパッド補正の {len(pad)} 本 "
             f"({', '.join(p[0].name for p in pad)}) ← 元 {RUNS_POL}/<run>. ゲームパッド補正の run は全部で {len(pad_all)} 本\n")
    L.append("表の値は 中央値 / 95%点 / 最大 (n=数).\n")

    L.append("## 1. 映像と関節の時刻のずれ (記録そのもの)\n")
    L.append("| 項目 | " + " | ".join(gn[g] for g in groups) + " |")
    L.append("|---|" + "---|" * len(groups))
    L.append("| 1コマと最寄りの actual の差 (ms) | " + " | ".join(fmt(G[g]["frame_actual_abs"], 1000, 0) for g in groups) + " |")
    L.append("| 映像のコマ間隔 狙い | " + " | ".join(str(G[g]["frame_target"]) for g in groups) + " |")
    L.append("| 映像のコマ間隔 実際 (ms) | " + " | ".join(fmt(G[g]["P_frame_dt"], 1000, 0) for g in groups) + " |")
    L.append("| 関節を読む間隔 狙い | " + " | ".join(str(G[g]["actual_target"]) for g in groups) + " |")
    L.append("| 関節を読む間隔 実際 (ms) | " + " | ".join(fmt(G[g]["P_actual_dt"], 1000, 0) for g in groups) + " |")
    L.append("| 読み取りの失敗 (数) / 弾いた値 | " + " | ".join(f"{G[g]['read_fail']} / {G[g]['actual_invalid']}" for g in groups) + " |")
    L.append("| コマ数 / actual 数 | " + " | ".join(f"{G[g]['n_frames']} / {G[g]['n_actual']}" for g in groups) + " |")
    L.append("")
    L.append("## 1b. 学習データでの組のずれ (作り直した格子で)\n")
    L.append("組み方 (コードで確認): 10Hz の格子 t を置き, state の関節 6 列 = actual の **線形補間** (np.interp)"
             " (補正は actual の間が 0.25 秒を超える所を speed20 の直線で埋める), 爪 = その時刻までの最後の指令,"
             " 画像 = capture_ts が t に **いちばん近い** コマ (補正は除いた区間のコマを候補から外す).\n")
    L.append("| 項目 | " + " | ".join(gn[g] for g in groups if G[g]["pairs_n"]) + " |")
    L.append("|---|" + "---|" * sum(1 for g in groups if G[g]["pairs_n"]))
    pg = [g for g in groups if G[g]["pairs_n"]]
    L.append("| ステップ数 | " + " | ".join(str(G[g]["pairs_n"]) for g in pg) + " |")
    L.append("| 画像の時刻 − state の時刻 の絶対値 (ms) | " + " | ".join(fmt(G[g]["pairs_frame"], 1000, 0) for g in pg) + " |")
    L.append("| state の時刻と最寄りの実測の差 (ms) | " + " | ".join(fmt(G[g]["pairs_near_actual"], 1000, 0) for g in pg) + " |")
    L.append("| 補間に使った 2 つの実測の間 (ms) | " + " | ".join(fmt(G[g]["pairs_bracket"], 1000, 0) for g in pg) + " |")
    L.append("| 0.25 秒を超える間をまたいで補間したステップ (うち 0.5° 超動いた) | "
             + " | ".join(f"{G[g]['pairs_over_gap']} ({G[g]['pairs_over_gap_moving']})" for g in pg) + " |")
    me = [m for m in match if m.get("state_match_max_err_deg") is not None]
    L.append(f"\n補間の誤差の見積もり (実機推論 {len(policy)} 本の密な actual で, 送るたびに {DEMO_BLOCK_SEC} 秒"
             f" 読めなかったことにして線形補間し, 消した実測と比べた. 6 関節の最大誤差): "
             f"全体 {fmt(G['sim']['err'], 1, 2)}°, 前後で 0.5° 超動いた所 {fmt(G['sim']['err_moving'], 1, 2)}°."
             " 実機推論の 1 回の動きはお手本の区間の動きより小さいので, お手本の誤差はこれより大きくなりうる.")
    L.append(f"\n格子の見つけ方の確かめ: エピソードの state と作り直した state の差の最大 = "
             f"{max((m['state_match_max_err_deg'] for m in me), default=None)}° ({len(me)}/{len(match)} 本で一致を確認. state_match.csv)\n")

    L.append("## 1c. カメラ自体の遅れの見積もり\n")
    L.append(f"やり方 A (動き出し): 関節の速さが {STILL_BEFORE_SEC} 秒以上 {STILL_DEG_S}°/秒未満のあと {MOVE_DEG_S}°/秒を超えた区間"
             f" (前後の読みの間 ≤ {MAX_JOINT_GAP_SEC} 秒) を関節の動き出し (区間の中点) とし,"
             f" 映像のコマ間差分 (96x54 灰色・1 秒あたり) が直前の止まりの 中央値 + {VID_K_MAD}×MAD を初めて超えたコマ間 (中点) を映像の動き出しとする."
             " 差 = 映像 − 関節 (正 = 映像が遅れる). 1 件ごとの確からしさ = 関節の読みの間の半分 + コマ間隔の半分.\n")
    L.append("やり方 B (相互相関): 関節の速さと映像の動きの量を 10ms の格子にのせ, −0.3〜+0.6 秒ずらして相関がいちばん高いずれ.\n")
    L.append("| 項目 | " + " | ".join(gn[g] for g in groups) + " |")
    L.append("|---|" + "---|" * len(groups))
    L.append("| A: 映像の遅れ (ms) | " + " | ".join(fmt(G[g]["onset"], 1000, 0) for g in groups) + " |")
    L.append("| A: 1件の確からしさ ± (ms) | " + " | ".join(fmt(G[g]["onset_unc"], 1000, 0) for g in groups) + " |")
    for g in groups:
        xs = [(a, b, c) for a, b, c in G[g]["xcorr"] if b is not None]
        lags = [b for _, b, _ in xs]; rs = [c for _, _, c in xs]
        G[g]["xc"] = (st(lags), st(rs))
    L.append("| B: 相関の山のずれ (ms) run ごと | " + " | ".join(fmt(G[g]["xc"][0], 1000, 0) for g in groups) + " |")
    L.append("| B: 山の相関係数 | " + " | ".join(fmt(G[g]["xc"][1], 1, 2) for g in groups) + " |")
    L.append("")

    L.append("## 2. 制御の1回の間隔\n")
    L.append("| 項目 | " + " | ".join(gn[g] for g in groups) + " |")
    L.append("|---|" + "---|" * len(groups))
    L.append("| 指令を送った間隔 (s) | " + " | ".join(fmt(G[g]["P_send_dt"], 1, 2) for g in groups) + " |")
    L.append("| 送る命令の往復 (s) | " + " | ".join(fmt(G[g]["P_send_rpc"], 1, 3) for g in groups) + " |")
    L.append("| 送ってから届くまで (s) | " + " | ".join(fmt(G[g]["P_arrival"], 1, 2) for g in groups) + " |")
    L.append("| パッドの 1 刻み 送って着くまで (s) | " + " | ".join(fmt(G[g]["P_pad_reach"], 1, 2) for g in groups) + " |")
    L.append("| サーボを読む時間 (ms) | " + " | ".join(fmt(G[g]["P_servo_ms"], 1, 0) for g in groups) + " |")
    L.append("| 推論 1 回の待ち latency_s (s) | " + " | ".join(fmt(G[g]["P_latency"], 1, 2) for g in groups) + " |")
    L.append("| 推論の間隔 (s) | " + " | ".join(fmt(G[g]["P_infer_dt"], 1, 2) for g in groups) + " |")
    L.append("\n送る・届くの意味: " + " / ".join(f"{gn[g]}: {G[g]['send_note']}" for g in groups) + "\n")
    L.append("### 中央値の 2 倍を超えて空いた所 (数)\n")
    L.append("| 組 | 種類 | つかむ前 | つかんだ後 | 放した後 | その他 |")
    L.append("|---|---|---|---|---|---|")
    for g in groups:
        for kind in ("send", "actual", "frame"):
            d = {ph: n for (k, ph), n in G[g]["gaps"].items() if k == kind}
            other = sum(n for ph, n in d.items() if ph not in ("つかむ前", "つかんだ後", "放した後"))
            L.append(f"| {gn[g]} | {kind} | {d.get('つかむ前', 0)} | {d.get('つかんだ後', 0)} | {d.get('放した後', 0)} | {other} |")
    L.append("\n(stage 別の内訳は gaps.csv)\n")
    (out / "summary.md").write_text("\n".join(L) + "\n")


if __name__ == "__main__":
    main()
