#!/usr/bin/env python3
"""eval_policy_offline.py — π0.5 サーバーの予測を、お手本と答え合わせする (アームは動かさない).

学習が終わったら、実機で動かす前にこれを実行する (lab Log11 / LOG358).
お手本の各ステップの「上カメラ画像 + 関節角 + グリッパー」をそのままサーバーへ送り,
返ってきた 1 秒分の予測 (10 x 7) と, お手本のその後 1 秒の動きを比べる.

見るもの:
  - 止まり (STUCK): お手本は 1 秒で 2° 以上動くのに, 予測は 0.5° 未満しか動かない
                    (LOG358: 学習データの停止を覚えて, ブロックの真上で止まった現象)
  - ずれ: 予測とお手本の関節角の差 (1 秒分の平均・最大)
  - グリッパー: 0.5 秒先までの開閉の予測が, お手本と食い違う回数

注意: 使うのは学習に使ったお手本なので, 「覚えたか」の確認であって, 初めて見る位置での確認ではない.
実機は, 着いた姿勢で止まったまま次の画像を撮る. その「止まった姿勢から動き出せるか」を見るのが一番の目的.

前提:
  - Runpod で serve_policy が起動し, Mac から SSH トンネル (localhost:8000) が張れている
  - pip install msgpack "websockets>=11"   (run_policy_client.py と同じ)

Usage:
  python3 pi05/eval_policy_offline.py                          # 位置ごとに 1 本ずつ, 3 ステップおき
  python3 pi05/eval_policy_offline.py --episodes all --stride 1
  python3 pi05/eval_policy_offline.py --raw-dir /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1/raw_episodes
記録: /Volumes/KIOXIA/r2r2r-data/lab/Log11/offline_eval/{stamp}/ (rows.csv / summary.json)
"""
import argparse
import csv
import json
import sys
import time
from datetime import datetime
from pathlib import Path

import cv2
import numpy as np

sys.path.insert(0, str(Path(__file__).resolve().parent))
from policy_ws import PolicyClient  # noqa: E402

TASK = "put the red block in the white box"
KIOXIA = Path("/Volumes/KIOXIA")
DATA_ROOT = KIOXIA / "r2r2r-data"
DEFAULT_RAW = DATA_ROOT / "datasets" / "mecharm_redblock_box_teach_v1_trim2" / "raw_episodes"
DEFAULT_OUT = DATA_ROOT / "lab" / "Log11" / "offline_eval"
HORIZON = 10              # 学習設定の action_horizon (0.1 秒 x 10 = 1 秒)
CLIENT_STEPS = 5          # run_policy_client.py が 1 チャンクで進む既定のステップ数 (0.5 秒先)
STUCK_DEMO_DEG = 2.0      # お手本が 1 秒でこれ以上動く場面で
STUCK_PRED_DEG = 0.5      # 予測がこれ未満しか動かなければ「止まり」


def pick_episodes(raw_dir: Path, mode: str):
    eps = sorted(p.parent for p in raw_dir.glob("*/episode.npz"))
    if not eps:
        sys.exit(f"[ERROR] お手本が見つからない: {raw_dir}")
    if mode == "all":
        return eps
    # ブロックの位置ごとに 1 本: つかんだ瞬間 (グリッパーの状態が閉に変わった所) の j0 を 5° 刻みで分ける
    groups = {}
    for ep in eps:
        s = np.load(ep / "episode.npz")["state"]
        ch = np.where(np.diff(s[:, 6]) != 0)[0]
        key = round(float(s[ch[0] + 1, 0]) / 5.0) if len(ch) else None
        groups.setdefault(key, ep)
    return [groups[k] for k in sorted(groups, key=lambda k: (k is None, k))]


def load_rgb(path: Path):
    bgr = cv2.imread(str(path))
    if bgr is None:
        raise RuntimeError(f"画像を読めない: {path}")
    return cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB)   # run_policy_client.py と同じく RGB で送る


def future(arr, t, n):
    """t から n ステップ分. 終わりを越える分は最後の値で埋める."""
    out = arr[t:t + n]
    if len(out) < n:
        out = np.concatenate([out, np.repeat(arr[-1:], n - len(out), axis=0)])
    return out


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--host", default="localhost")
    ap.add_argument("--port", type=int, default=8000)
    ap.add_argument("--raw-dir", default=str(DEFAULT_RAW))
    ap.add_argument("--episodes", default="per-position", choices=["per-position", "all"])
    ap.add_argument("--stride", type=int, default=3, help="何ステップおきに問い合わせるか")
    ap.add_argument("--out", default=str(DEFAULT_OUT))
    args = ap.parse_args()

    out_root = Path(args.out)
    if str(out_root).startswith(str(KIOXIA)) and not KIOXIA.is_mount():
        sys.exit(f"[ERROR] KIOXIA がマウントされていない: {KIOXIA}")
    eps = pick_episodes(Path(args.raw_dir), args.episodes)
    stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    out_dir = out_root / stamp
    out_dir.mkdir(parents=True, exist_ok=True)
    print(f"[EVAL  ] お手本 {len(eps)} 本 / {args.stride} ステップおき / 記録 {out_dir}")
    for ep in eps:
        print(f"         {ep.name}")

    policy = PolicyClient(args.host, args.port)
    print(f"[POLICY] 接続 {policy.uri}")

    fields = ["episode", "t", "j0", "j1", "j2", "j3", "j4", "j5", "grip", "latency_s",
              "demo_move_1s", "pred_move_1s", "demo_move_05s", "pred_move_05s",
              "err_mean", "err_max", "err_max_joint", "grip_demo_05s", "grip_pred_05s", "grip_mismatch", "stuck"]
    rows = []
    per_ep = {}
    t_start = time.time()
    with (out_dir / "rows.csv").open("w", newline="") as f:
        w = csv.DictWriter(f, fieldnames=fields)
        w.writeheader()
        for ep in eps:
            z = np.load(ep / "episode.npz")
            state, actions = z["state"].astype(np.float32), z["actions"].astype(np.float32)
            frames = sorted((ep / "frames").glob("*.jpg"))
            if len(frames) != len(state):
                sys.exit(f"[ERROR] 画像とステップの数が違う: {ep.name} ({len(frames)} / {len(state)})")
            stats = {"queries": 0, "stuck": 0, "grip_mismatch": 0, "err_mean": [], "stuck_t": []}
            for t in range(0, len(state), args.stride):
                q = state[t]
                t0 = time.time()
                out = policy.infer({"observation/image": load_rgb(frames[t]), "observation/state": q, "prompt": TASK})
                lat = time.time() - t0
                pred = np.asarray(out["actions"], dtype=np.float32)[:HORIZON]
                demo = future(actions, t, HORIZON)
                d1 = float(np.abs(demo[-1, :6] - q[:6]).max())
                p1 = float(np.abs(pred[-1, :6] - q[:6]).max())
                d05 = float(np.abs(demo[CLIENT_STEPS - 1, :6] - q[:6]).max())
                p05 = float(np.abs(pred[CLIENT_STEPS - 1, :6] - q[:6]).max())
                diff = np.abs(pred[:, :6] - demo[:, :6])
                g_demo = "".join(str(int(v > 0.5)) for v in demo[:CLIENT_STEPS, 6])
                g_pred = "".join(str(int(v > 0.5)) for v in pred[:CLIENT_STEPS, 6])
                stuck = d1 >= STUCK_DEMO_DEG and p1 < STUCK_PRED_DEG
                row = {"episode": ep.name, "t": t, **{f"j{k}": round(float(q[k]), 2) for k in range(6)},
                       "grip": int(q[6] > 0.5), "latency_s": round(lat, 3),
                       "demo_move_1s": round(d1, 2), "pred_move_1s": round(p1, 2),
                       "demo_move_05s": round(d05, 2), "pred_move_05s": round(p05, 2),
                       "err_mean": round(float(diff.mean()), 2), "err_max": round(float(diff.max()), 2),
                       "err_max_joint": int(diff.max(axis=0).argmax()),
                       "grip_demo_05s": g_demo, "grip_pred_05s": g_pred,
                       "grip_mismatch": int(g_demo != g_pred), "stuck": int(stuck)}
                w.writerow(row)
                rows.append(row)
                stats["queries"] += 1
                stats["stuck"] += int(stuck)
                stats["grip_mismatch"] += int(g_demo != g_pred)
                stats["err_mean"].append(float(diff.mean()))
                if stuck:
                    stats["stuck_t"].append(t)
                mark = "  [STUCK]" if stuck else ""
                gm = f"  グリッパー お手本{g_demo} 予測{g_pred}" if g_demo != g_pred else ""
                print(f"[{ep.name[-15:]} t={t:3d}] 1秒の動き お手本{d1:5.1f}° 予測{p1:5.1f}°  "
                      f"ずれ 平均{diff.mean():4.1f}° 最大{diff.max():4.1f}°(j{row['err_max_joint']}){mark}{gm}")
            per_ep[ep.name] = {"queries": stats["queries"], "stuck": stats["stuck"], "stuck_t": stats["stuck_t"],
                               "grip_mismatch": stats["grip_mismatch"],
                               "err_mean_median": round(float(np.median(stats["err_mean"])), 2),
                               "err_mean_max": round(float(np.max(stats["err_mean"])), 2)}
    policy.close()

    n = len(rows)
    summary = {
        "stamp": stamp, "raw_dir": args.raw_dir, "episodes_mode": args.episodes, "stride": args.stride,
        "queries": n, "elapsed_sec": round(time.time() - t_start, 1),
        "stuck": sum(r["stuck"] for r in rows), "grip_mismatch": sum(r["grip_mismatch"] for r in rows),
        "err_mean_median": round(float(np.median([r["err_mean"] for r in rows])), 2),
        "thresholds": {"stuck_demo_deg": STUCK_DEMO_DEG, "stuck_pred_deg": STUCK_PRED_DEG,
                       "horizon": HORIZON, "client_steps": CLIENT_STEPS},
        "per_episode": per_ep,
    }
    (out_dir / "summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2))
    print()
    print(f"[SUMMARY] 問い合わせ {n} 回 / 止まり {summary['stuck']} 回 / グリッパーの食い違い {summary['grip_mismatch']} 回 / "
          f"ずれの中央値 {summary['err_mean_median']}°")
    for name, s in per_ep.items():
        print(f"  {name}: 止まり {s['stuck']}/{s['queries']} {s['stuck_t']}  グリッパー {s['grip_mismatch']}  "
              f"ずれ中央値 {s['err_mean_median']}°")
    print(f"[DONE  ] 記録: {out_dir}")


if __name__ == "__main__":
    main()
