#!/usr/bin/env python3
"""log25_runs_table.py — 2026-11-09 に撮ったお手本 (record_teach_demo.py) 27 本を 1 本 1 行の表にする
(LOG463・フィジカルAI研究室 Log#25).

読むだけ:
  runs/run_pick_place/run_0001_20261109_*/run_0001_20261109_*.json   (試行 JSON: 人の y/n・区間ごとの到達・ブロックの位置)
  runs/run_pick_place/run_0001_20261109_*/recording/states.csv       (段階の時刻・読んだ角度・爪の命令)
  runs/run_pick_place/run_0001_20261109_*/recording/currents.csv     (爪と肩の電流. 依頼 121)
  lab/Log25/grasp_depth.csv   (pi05/grasp_depth.py --csv の出力. 閉じる直前と閉じて 1.0 秒後のはみ出し・爪の値)
書く: このファイルと同じフォルダの runs_table.csv だけ.

判定 (使う/外す) の決まり:
  1. PICK の区間 (ブロックの上 → つかむ姿勢) が 5 秒より長くかかった本は「爪がブロックに乗った」として外す
     (普段は 1.9〜2.2 秒. 乗った本は到達の許容 3.0° に入るまで, 到達待ちの上限 8 秒近くかかった)
  2. 閉じて 1.0 秒後のはみ出しが 24px を超えた本は「浅い」として外す (grasp_depth.py の線. LOG460・依頼 117)
  3. 教え直す前の記録 (drag_v2_20221013_144348.json) で撮った本は, 位置 1 の比べる相手として残すが学習には使わない

Mac で動かすとき:  python3 log25_runs_table.py
(パスは R2R2R_DATA で変えられる. 既定 /Volumes/KIOXIA/r2r2r-data)
"""
import csv, json, os, statistics
from pathlib import Path

DATA = Path(os.environ.get("R2R2R_DATA", "/Volumes/KIOXIA/r2r2r-data"))
RUNS = DATA / "runs/run_pick_place"
HERE = Path(os.environ.get("LOG25_DIR", DATA / "lab/Log25"))
TEACH_POS = {"drag_v2_20221013_144348.json": 1, "drag_v2_20261109_094222.json": 1,
             "drag_v2_20221013_230205.json": 2}
SNAG_SEC = 5.0
LINE_PX = 24.0


def main():
    gd = {r["run"]: r for r in csv.DictReader(open(HERE / "grasp_depth.csv"))}
    rows = []
    for p in sorted(x for x in RUNS.glob("run_0001_20261109_*") if x.is_dir()):
        s = p.name
        tj = json.load(open(p / f"{s}.json"))
        teach = tj["teach"]["file"]
        seg = [g for g in tj["segments"] if g.get("to_name") == "PICK"][0]
        st = list(csv.DictReader(open(p / "recording/states.csv")))
        t_pick = [float(r["ts"]) for r in st if r["event_type"] == "stage" and r["stage"] == "PICK"][0]
        t_close = [float(r["ts"]) for r in st if r["event_type"] == "gripper_cmd" and r["gripper_state"] == "1"][0]
        cur = list(csv.DictReader(open(p / "recording/currents.csv")))
        sh = [int(r["r2_69"]) for r in cur if t_pick < float(r["ts"]) < t_close and r["r2_69"]]
        g = gd[s]
        after = float(g["after_px"]) if g["after_px"] else float("nan")
        arr = seg["arrival_actual"]
        if seg["segment_elapsed_sec"] > SNAG_SEC:
            use, reason = "外す", "爪がブロックに乗った (PICK の区間 %.1f 秒)" % seg["segment_elapsed_sec"]
        elif not (after <= LINE_PX):
            use, reason = "外す", "浅い (閉じて 1.0 秒後 %.1fpx > %gpx)" % (after, LINE_PX)
        else:
            use, reason = "使う", ""
        if teach == "drag_v2_20221013_144348.json":
            use, reason = "外す", (reason + " / " if reason else "") + "教え直す前の記録"
        rows.append(dict(
            run=s, position=TEACH_POS.get(teach, ""), teach=teach,
            pick=tj["judgments"]["pick"]["verdict"], place=tj["judgments"]["place"]["verdict"],
            before_px=g["before_px"], after_px=g["after_px"], hand_suspect=g["hand_suspect"],
            grip_max=g["grip_current_max"], grip_median=g["grip_current_median"],
            pick_segment_sec=seg["segment_elapsed_sec"], pick_to_close_sec=round(t_close - t_pick, 2),
            pick_arrival_max_delta_deg=seg["arrival_max_delta_deg"],
            j1_arrived=arr[1], j2_arrived=arr[2], j4_arrived=arr[4],
            shoulder_lo_during_pick="/".join(map(str, sorted(set(sh)))),
            block_dist_from_base_mm=tj["positions_before_replay"].get("block_dist_from_base_mm"),
            block_moved_mm=tj["block_move_check"].get("dist_mm"),
            use=use, reason=reason))
    with open(HERE / "runs_table.csv", "w", newline="") as f:
        w = csv.DictWriter(f, fieldnames=list(rows[0]))
        w.writeheader(); w.writerows(rows)
    for r in rows:
        print(",".join(str(v) for v in r.values()))
    for pos in (1, 2):
        u = [r for r in rows if r["position"] == pos and r["use"] == "使う"]
        print(f"位置{pos}: 使う {len(u)} 本 / 撮った {sum(1 for r in rows if r['position'] == pos)} 本")


if __name__ == "__main__":
    main()