"""check_258.py が so101/check_258/ に残した記録を解析する (依頼 258). アーム・カメラにはつながない.

出力: so101/check_258/analysis.json と, 画面に表のもとになる数字.

使い方 (conda の lerobot 環境で):
    python so101/check_258_analyze.py
"""

import csv
import json
import subprocess
import sys
from pathlib import Path

import cv2
import numpy as np
import pandas as pd

HERE = Path(__file__).resolve().parent
OUT = HERE / "check_258"
MOTORS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]
FPS = 30
DT = 1 / FPS
FFPROBE = "/opt/homebrew/Caskroom/miniforge/base/envs/lerobot/bin/ffprobe"


def stats(x):
    x = np.asarray([v for v in x if np.isfinite(v)], dtype=float)
    if len(x) == 0:
        return None
    return {"n": int(len(x)), "median": round(float(np.median(x)), 2), "p95": round(float(np.percentile(x, 95)), 2),
            "max": round(float(x.max()), 2), "mean": round(float(x.mean()), 2), "std": round(float(x.std()), 2)}


# ---- s1 ------------------------------------------------------------------------------

def s1():
    d = OUT / "s1"
    res = {"saved": []}
    for js in sorted(d.glob("desk_*.json")):
        j = json.loads(js.read_text())
        res["saved"].append({"image": j["image"], "focus": {k: round(v, 1) for k, v in j["focus"].items()},
                             "center_max_so_far": round(j.get("center_max_so_far", 0), 1)})
    ref = d / "ref_248_score.txt"
    res["ref_248"] = ref.read_text().strip() if ref.exists() else None
    return res


# ---- s2 ------------------------------------------------------------------------------

def joints(path):
    df = pd.read_csv(path)
    res = {"n": len(df), "errors": int((df["err"].fillna("") != "").sum()),
           "follower_ms": stats(df["follower_ms"]), "leader_ms": stats(df["leader_ms"]),
           "both_ms": stats(df["follower_ms"] + df["leader_ms"]), "total_ms": stats(df["total_ms"]),
           "over_33ms": int((df["total_ms"] > 1000 / FPS).sum())}
    for reg in ["Present_Current", "Present_Load", "Present_Temperature", "Present_Voltage"]:
        if reg + "_ms" in df:
            res[reg + "_ms"] = stats(df[reg + "_ms"])
            res[reg + "_sample"] = df[reg].iloc[len(df) // 2]
    return res


def cam(stem):
    df = pd.read_csv(f"{stem}.csv")
    props = json.loads(Path(f"{stem}_props.json").read_text())
    ok = df[df["pos_msec"] != -1]
    t = ok["t"].to_numpy()
    if len(t) < 3:
        return {"frames": len(t), "props": props}
    iv = np.diff(t) * 1e3
    drops = int(sum(max(0, round(x / (1000 / FPS)) - 1) for x in iv if x > 1.5 * 1000 / FPS))
    pm = ok["pos_msec"].to_numpy()
    t_ = t[t >= t[0] + 1.0]  # 開いた直後の1秒を除いたもの
    iv1 = np.diff(t_) * 1e3
    after = {"fps": round((len(t_) - 1) / (t_[-1] - t_[0]), 2), "interval_ms": stats(iv1),
             "interval_over_50ms": int((iv1 > 50).sum()),
             "drops_est": int(sum(max(0, round(x / (1000 / FPS)) - 1) for x in iv1 if x > 1.5 * 1000 / FPS))}
    return {"frames": int(len(t)), "read_fail": int(len(df) - len(ok)), "fps": round((len(t) - 1) / (t[-1] - t[0]), 2),
            "interval_ms": stats(iv), "interval_over_50ms": int((iv > 50).sum()), "drops_est": drops, "after_first_1s": after,
            "pos_msec_varies": bool(np.ptp(pm) > 0), "pos_msec_interval_ms": stats(np.diff(pm)) if np.ptp(pm) > 0 else None,
            "resolution": [int(ok["w"].iloc[0]), int(ok["h"].iloc[0])], "blur_center": stats(ok["blur_center"]),
            "props": props["props"], "t": t}


def cam_pair(a, b):
    """手首のコマごとに, いちばん近い机側のコマの届いた時刻の差 (ms). 2台はつながっていないので位相はばらばら."""
    ta, tb = a.get("t"), b.get("t")
    if ta is None or tb is None:
        return None
    j = np.clip(np.searchsorted(tb, ta), 1, len(tb) - 1)
    near = np.where(np.abs(tb[j] - ta) < np.abs(tb[j - 1] - ta), tb[j], tb[j - 1])
    d = (near - ta) * 1e3
    keep = ta >= max(ta[0], tb[0]) + 1.0
    return {"wrist_minus_desk_nearest_ms": stats(np.abs(d)), "after_first_1s": stats(np.abs(d[keep]))}


def s2():
    d = OUT / "s2"
    res = {}
    for k, f in [("a_joints", "a_joints.csv"), ("b_joints_extras", "b_joints_extras.csv"), ("e_joints", "e_joints.csv")]:
        if (d / f).exists():
            res[k] = joints(d / f)
    for tag in ["c_cam_1280x720", "d_cam_640x480", "e_cam"]:
        cams = {k: cam(d / f"{tag}_{k}") for k in ("wrist", "desk") if (d / f"{tag}_{k}.csv").exists()}
        if len(cams) == 2:
            res[tag + "_pair"] = cam_pair(cams["wrist"], cams["desk"])
        for k, v in cams.items():
            v.pop("t", None)
            res[f"{tag}_{k}"] = v
    return res


# ---- s34 -----------------------------------------------------------------------------

def load_cam34(key):
    stem = OUT / "s34" / f"cam_{key}"
    df = pd.read_csv(f"{stem}.csv")
    df = df[df["pos_msec"] != -1].reset_index(drop=True)
    small = np.load(f"{stem}_small.npy").astype(np.float32)
    n = min(len(df), len(small))
    return df.iloc[:n], small[:n]


def onset_series(t, x, t_from, t_to, base_from, base_to, k=6.0, floor=0.0):
    """t_from〜t_to の中で, 基準区間の中央値から k×MAD (かつ floor) を越えた最初の時刻."""
    base = x[(t >= base_from) & (t < base_to)]
    if len(base) < 3:
        return None
    med = np.median(base)
    mad = np.median(np.abs(base - med)) * 1.4826 + 1e-9
    thr = max(k * mad, floor)
    m = (t >= t_from) & (t < t_to) & (np.abs(x - med) > thr)
    idx = np.flatnonzero(m)
    return float(t[idx[0]]) if len(idx) else None


def s34():
    d = OUT / "s34"
    tele = pd.read_csv(d / "teleop.csv")
    phases = json.loads((d / "phases.json").read_text())["phases"]
    ph_t = [(float(t), n) for t, n in phases]

    def span(name):
        for i, (t, n) in enumerate(ph_t):
            if n == name:
                return t, (ph_t[i + 1][0] if i + 1 < len(ph_t) else tele["t0"].iloc[-1])
        return None

    res = {"loops": len(tele), "loop_interval_ms": stats(np.diff(tele["t0"]) * 1e3),
           "loop_work_ms": stats((tele["t_extra"] - tele["t0"]) * 1e3),
           "leader_read_ms": stats((tele["t_act"] - tele["t0"]) * 1e3),
           "send_ms": stats((tele["t_sent"] - tele["t_act"]) * 1e3),
           "follower_pos_read_ms": stats((tele["t_pos"] - tele["t_sent"]) * 1e3),
           "extra_reads_ms": stats((tele["t_extra"] - tele["t_pos"]) * 1e3)}

    # 爪のできごと: リーダーの爪 (action) が動き始めた時刻を基準に, フォロワーの爪 (state)・手首カメラ・机側カメラ.
    # 画像は, 合図の区間で画素ごとの揺れが大きい上位 3% (= 爪が動く場所) だけを見て, 合図の前の平均からの差で決める
    cams = {k: load_cam34(k) for k in ("wrist", "desk")}
    tg = tele["t_pos"].to_numpy()
    ta = tele["t_act"].to_numpy()
    gp = tele["pos_gripper"].to_numpy()
    ga = tele["act_gripper"].to_numpy()

    def first(t, x, t0, t1, base, thr):
        i = np.flatnonzero((t >= t0) & (t < t1) & (np.abs(x - base) > thr))
        return float(t[i[0]]) if len(i) else None

    events = []
    for k in range(1, 6):
        sp = span(f"grip_{k}")
        if sp is None:
            continue
        s0, b0, w1 = sp[0], sp[0] - 1.0, sp[0] + 5.0
        ev = {"event": k, "leader_gripper_max_in_window": round(float(ga[(ta >= s0) & (ta < w1)].max()), 1)}
        la = first(ta, ga, s0 - 0.5, w1, np.median(ga[(ta >= b0) & (ta < s0)]), 3.0)
        ev["leader_action"] = la
        ev["follower_state"] = first(tg, gp, s0 - 0.5, w1, np.median(gp[(tg >= b0) & (tg < s0)]), 3.0)
        for cname, (df, small) in cams.items():
            if la is None:
                ev[f"{cname}_cam"] = None
                continue
            tc = df["t"].to_numpy()
            mw, mb = (tc >= s0 - 0.5) & (tc < w1), (tc >= b0) & (tc < s0)
            sd = small[mw].std(axis=0)
            mask = sd > np.percentile(sd, 97)
            base = small[mb].mean(axis=0)
            sig = np.abs(small[:, mask] - base[mask]).mean(axis=1)
            bs = sig[mb]
            thr = np.median(bs) + 6 * (np.median(np.abs(bs - np.median(bs))) * 1.4826 + 0.3)
            ev[f"{cname}_cam"] = first(tc, sig, la - 0.3, w1, 0.0, thr)
        events.append(ev)
    rel = {}

    def rel_ms(a_, b_):
        return [round((e[a_] - e[b_]) * 1e3) if (e.get(a_) and e.get(b_)) else None for e in events]
    for a_, b_ in [("follower_state", "leader_action"), ("wrist_cam", "leader_action"), ("desk_cam", "leader_action"),
                   ("wrist_cam", "follower_state"), ("desk_cam", "follower_state"), ("wrist_cam", "desk_cam")]:
        rel[f"{a_}_minus_{b_}_ms_per_event"] = rel_ms(a_, b_)
    res["grip_events"] = [{k: (round(v, 4) if isinstance(v, float) else v) for k, v in e.items()} for e in events]
    res["grip_lag"] = rel

    # リーダー (action) に対するフォロワー (state) の遅れ: 速い区間での相互相関 (関節ごと)
    lag = {}
    for m in MOTORS:
        a = np.interp(np.arange(ta[0], ta[-1], 0.005), ta, tele[f"act_{m}"])
        s = np.interp(np.arange(ta[0], ta[-1], 0.005), tg, tele[f"pos_{m}"])
        va, vs = np.diff(a), np.diff(s)
        best, bl = -1, 0
        for L in range(0, 101):  # 0〜500ms
            c = np.corrcoef(va[:len(va) - L], vs[L:])[0, 1] if L else np.corrcoef(va, vs)[0, 1]
            if np.isfinite(c) and c > best:
                best, bl = c, L
        lag[m] = {"lag_ms": bl * 5, "corr": round(float(best), 3)}
    res["follower_lag_xcorr"] = lag

    # 速さ別
    seg = {}
    for name in ["slow", "normal", "fast"]:
        sp = span(name)
        if sp is None:
            continue
        m_ = (tele["t0"] >= sp[0]) & (tele["t0"] < sp[1])
        sub = tele[m_]
        t = sub["t_pos"].to_numpy()
        r = {"seconds": round(sp[1] - sp[0], 1), "joints": {}}
        for mo in MOTORS:
            p = sub[f"pos_{mo}"].to_numpy()
            v = np.abs(np.diff(p) / np.diff(t))
            err = (sub[f"act_{mo}"] - sub[f"pos_{mo}"]).abs().to_numpy()
            # rustypot が符号つきにして返す (負の値が出る). 電流は 6.5mA/目盛, 負荷は 0.1%/目盛, 速さは 目盛/秒
            cur = sub[f"cur_{mo}"].to_numpy()
            load = sub[f"load_{mo}"].to_numpy()
            sv = sub[f"vel_{mo}"].to_numpy()
            temps = sub[f"temp_{mo}"].dropna().to_numpy() if f"temp_{mo}" in sub else np.array([])
            unit = "%" if mo == "gripper" else "deg"
            # 離れた区間: |差| > 10 (deg か %) が 0.1 秒以上続いた区間
            far = err > 10
            runs, start = [], None
            for i, f in enumerate(far):
                if f and start is None:
                    start = i
                if (not f or i == len(far) - 1) and start is not None:
                    end = i if not f else i + 1
                    if t[min(end, len(t) - 1)] - t[start] >= 0.1:
                        runs.append(round(float(t[min(end, len(t) - 1)] - t[start]), 2))
                    start = None
            r["joints"][mo] = {
                f"max_speed_{unit}_s": round(float(np.percentile(v, 99)), 1) if len(v) else None,
                f"peak_speed_{unit}_s": round(float(v.max()), 1) if len(v) else None,
                "servo_velocity_raw_max": int(np.abs(sv).max()),
                "servo_velocity_deg_s_max": round(float(np.abs(sv).max() * 360 / 4096), 1),
                f"max_leader_diff_{unit}": round(float(err.max()), 1),
                f"p95_leader_diff_{unit}": round(float(np.percentile(err, 95)), 1),
                "far_runs_s": runs,
                "current_raw_max": int(np.abs(cur).max()), "current_mA_max": round(float(np.abs(cur).max() * 6.5)),
                "load_raw_max": int(np.abs(load).max()), "load_pct_max": round(float(np.abs(load).max() / 10), 1),
                "temp_start_end": [float(temps[0]), float(temps[-1])] if len(temps) else None,
            }
        for cname, (df, _s) in cams.items():
            mm = (df["t"] >= sp[0]) & (df["t"] < sp[1])
            r[f"{cname}_blur"] = stats(df.loc[mm, "blur_center"])
        seg[name] = r
    res["speed_segments"] = seg

    # ぶれと関節の速さの関係 (s34 の全コマ): 関節1〜5 の速さの最大で区切って, 手首・机側のピントの中央値
    tt = tele["t_pos"].to_numpy()
    body_v = np.zeros(len(tt))
    for mo in MOTORS[:5]:
        p = tele[f"pos_{mo}"].to_numpy()
        v = np.r_[0, np.abs(np.diff(p) / np.maximum(np.diff(tt), 1e-3))]
        body_v = np.maximum(body_v, v)
    bins = [0, 10, 30, 60, 100, 150, 200, 300, 1e9]
    blur_by_speed = {}
    for cname, (df, _s) in cams.items():
        vv = np.interp(df["t"], tt, body_v)
        rows = []
        for lo, hi in zip(bins[:-1], bins[1:]):
            m_ = (vv >= lo) & (vv < hi)
            if m_.sum() >= 5:
                rows.append({"speed_deg_s": f"{lo}-{hi if hi < 1e9 else ''}", "frames": int(m_.sum()),
                             "blur_median": round(float(np.median(df.loc[m_, "blur_center"])), 1)})
        blur_by_speed[cname] = rows
    res["blur_by_body_speed"] = blur_by_speed

    # 爪の値と手首カメラの爪の開き: 爪区間の手首のコマで, 開いた状態 (grip_pre) からの差の大きさと爪の値の相関
    sp0, sp_end = span("grip_pre"), span("grip_5_rest")
    if sp0 and sp_end:
        df, small = cams["wrist"]
        tc = df["t"].to_numpy()
        m_ = (tc >= sp0[0]) & (tc < sp_end[1])
        ref = small[(tc >= sp0[0]) & (tc < sp0[1])].mean(axis=0)
        dev = np.abs(small[m_] - ref).mean(axis=(1, 2))
        g = np.interp(tc[m_], tg, gp)
        res["gripper_vs_wrist_image"] = {"frames": int(m_.sum()),
                                         "pearson_r_gripper_vs_image_change": round(float(np.corrcoef(g, dev)[0, 1]), 3),
                                         "gripper_range_in_events": [round(float(g.min()), 1), round(float(g.max()), 1)]}
        save_gripper_montage(tc, tg, gp, sp0[0], sp_end[1])
    return res


def save_gripper_montage(tc, tg, gp, t0, t1):
    """手首の動画から, 爪の値がちがう6コマを並べた画像を作る (目で合っているか見る用)."""
    cap = cv2.VideoCapture(str(OUT / "s34" / "wrist.mp4"))
    frames = []
    i = 0
    m_idx = set(np.flatnonzero((tc >= t0) & (tc < t1)).tolist())
    while True:
        ok, f = cap.read()
        if not ok:
            break
        if i in m_idx:
            frames.append((i, f))
        i += 1
    cap.release()
    if not frames:
        return
    vals = [(float(np.interp(tc[i], tg, gp)), i, f) for i, f in frames]
    vals.sort(key=lambda x: x[0])
    pick = [vals[int(k * (len(vals) - 1) / 5)] for k in range(6)]
    tiles = []
    for g, i, f in pick:
        im = cv2.resize(f, (320, 240))
        cv2.putText(im, f"gripper {g:.1f}", (8, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 0), 4)
        cv2.putText(im, f"gripper {g:.1f}", (8, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
        tiles.append(im)
    cv2.imwrite(str(OUT / "gripper_vs_wrist.png"), np.vstack([np.hstack(tiles[:3]), np.hstack(tiles[3:])]))


# ---- s5 ------------------------------------------------------------------------------

def s5():
    root = OUT / "dataset"
    if not root.exists():
        return None
    info = json.loads((root / "meta" / "info.json").read_text())
    res = {"info": {k: v for k, v in info.items() if k != "features"}, "features": info["features"]}
    files = sorted((root / "data").rglob("*.parquet"))
    df = pd.concat([pd.read_parquet(f) for f in files])
    res["parquet_files"] = [str(f.relative_to(root)) for f in files]
    res["parquet_columns"] = {c: str(df[c].dtype) for c in df.columns}
    res["parquet_head"] = df.head(3).astype(str).to_dict("records")
    eps = {}
    for ep, g in df.groupby("episode_index"):
        ts = g["timestamp"].to_numpy()
        eps[int(ep)] = {"frames": int(len(g)), "timestamp_first_last": [float(ts[0]), float(ts[-1])],
                        "timestamp_step": stats(np.diff(ts) * 1e3),
                        "timestamp_equals_index_over_fps": bool(np.allclose(ts, g["frame_index"].to_numpy() / FPS, atol=1e-4))}
    res["episodes"] = eps
    vids = sorted((root / "videos").rglob("*.mp4"))
    res["videos"] = []
    for v in vids:
        pr = subprocess.run([FFPROBE, "-v", "error", "-select_streams", "v:0", "-count_frames", "-show_entries",
                             "stream=codec_name,pix_fmt,width,height,r_frame_rate,avg_frame_rate,nb_read_frames,duration",
                             "-of", "json", str(v)], capture_output=True, text=True)
        st = json.loads(pr.stdout).get("streams", [{}])[0] if pr.returncode == 0 else {"err": pr.stderr[-300:]}
        st["file"] = str(v.relative_to(root))
        st["MB"] = round(v.stat().st_size / 1e6, 3)
        res["videos"].append(st)
    total = sum(p.stat().st_size for p in root.rglob("*") if p.is_file())
    secs = sum(e["frames"] for e in eps.values()) / FPS
    res["size"] = {"total_MB": round(total / 1e6, 2), "seconds": round(secs, 1),
                   "MB_per_s": round(total / 1e6 / secs, 3) if secs else None,
                   "video_MB": round(sum(v.stat().st_size for v in vids) / 1e6, 2),
                   "parquet_MB": round(sum(f.stat().st_size for f in files) / 1e6, 3)}
    mbps = res["size"]["MB_per_s"]
    if mbps:
        res["size"]["estimate_MB"] = {f"{sec}s": {"1": round(mbps * sec, 1), "100": round(mbps * sec * 100),
                                                  "1000": round(mbps * sec * 1000)} for sec in (10, 20, 30, 60)}
    return res


# ---- s6 ------------------------------------------------------------------------------

def s6(episode=0):
    root = OUT / "dataset"
    logs = [OUT / f"s6_replay{k}.csv" for k in (1, 2)]
    if not all(p.exists() for p in logs) or not root.exists():
        return None
    files = sorted((root / "data").rglob("*.parquet"))
    df = pd.concat([pd.read_parquet(f) for f in files])
    g = df[df["episode_index"] == episode].sort_values("frame_index")
    rec_state = np.stack(g["observation.state"].to_numpy())
    rec_action = np.stack(g["action"].to_numpy())
    reps = []
    for p in logs:
        r = pd.read_csv(p)
        o = r[r["kind"] == "obs"].sort_values("frame")
        reps.append(o[MOTORS].to_numpy())
    n = min(len(rec_state), *(len(x) for x in reps))
    # replay の obs[i] は i 番目の指令を送る前に読んだ位置. 記録の state[i] も action[i] を送る前に読んだ位置なので同じ並び
    res = {"frames": int(n), "joints": {}}
    for j, m in enumerate(MOTORS):
        d_rec1 = np.abs(reps[0][:n, j] - rec_state[:n, j])
        d_rec2 = np.abs(reps[1][:n, j] - rec_state[:n, j])
        d_12 = np.abs(reps[0][:n, j] - reps[1][:n, j])
        res["joints"][m] = {"replay1_vs_record_mean": round(float(d_rec1.mean()), 2), "replay1_vs_record_max": round(float(d_rec1.max()), 2),
                            "replay2_vs_record_mean": round(float(d_rec2.mean()), 2), "replay2_vs_record_max": round(float(d_rec2.max()), 2),
                            "replay1_vs_replay2_mean": round(float(d_12.mean()), 2), "replay1_vs_replay2_max": round(float(d_12.max()), 2),
                            "final_rec_r1_r2": [round(float(rec_state[n - 1, j]), 2), round(float(reps[0][n - 1, j]), 2), round(float(reps[1][n - 1, j]), 2)],
                            "record_action_vs_state_max": round(float(np.abs(rec_action[:n, j] - rec_state[:n, j]).max()), 2)}
    return res


def main():
    res = {}
    for name, fn in [("s1", s1), ("s2", s2), ("s34", s34), ("s5", s5), ("s6", s6)]:
        try:
            res[name] = fn()
        except FileNotFoundError as e:
            res[name] = {"missing": str(e)}
        except Exception as e:
            res[name] = {"error": repr(e)}
    cal = OUT / "calibration_check.json"
    if cal.exists():
        res["calibration"] = json.loads(cal.read_text())
    (OUT / "analysis.json").write_text(json.dumps(res, ensure_ascii=False, indent=1, default=str))
    print(json.dumps(res, ensure_ascii=False, indent=1, default=str)[:20000])


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