#!/usr/bin/env python3
"""camera_align.py — 上カメラの写り方をお手本に合わせる画面 (Mac 側・アームは動かさない).

上カメラの iPhone は普段使いのもので, 毎回付け外しする. 付けるたびに写り方が変わり,
2026-10-21 の実機では お手本より下に大きくずれたまま動かしてしまった
(runs/pi05_policy/pi05_20261021_082645 は dy=+37.5. 1 分前のドライランは dy=-7.8 で正常).
iPhone を付けたあと, この画面で写り方を合わせてから実機に進む.

画面:
  ライブ映像に 基準画像 (--raw-dir の最初のエピソードの 1 枚目・HOME の姿勢) を半透明で重ねる.
  隅に写り方のずれ (dx, dy) と相関の強さ (response) を出し, 判定を大きく表示する:
    OK       しきい値以内
    ZURE     dx か dy がしきい値の外 (= ずれ)
    NO MATCH 相関が弱く dx/dy が当てにならない (= 測れない)
  q または Esc で終了. **表示だけで, ファイルには何も書かない.**

run_policy_client.py は開始時に この measure_framing / framing_verdict で同じ検査をして,
OK 以外ならアームに何も送らずに終了する (--ignore-framing で続行).

前提:
  - 上カメラ (iPhone 連係) が見えている. アームは HOME に置いておく (基準画像が HOME の姿勢)

Usage:
  RAW=/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_teach_v1_trim2/raw_episodes
  python3 pi05/camera_align.py --raw-dir $RAW
  python3 pi05/camera_align.py --raw-dir $RAW --alpha 0.7 --scale 2   # 基準を濃く / 画面を小さく
"""
import argparse
import sys
import time
from pathlib import Path

import cv2
import numpy as np

_HERE = Path(__file__).resolve().parent
_ROOT = _HERE.parent
_PLACE = _ROOT / "dynamic_tracking_limits" / "place" / "place_modules"   # camera_resolver はここ
for p in (_HERE, _ROOT, _PLACE):
    sys.path.insert(0, str(p))

import camera_resolver  # noqa: E402

IMG_LONG = 384            # build_demo_episodes.py / run_policy_client.py と同じ (学習画像の長辺)
FRAMING_MAX_DY = 20.0     # |dy| がこれを超えたら「ずれ」. 下の実測値の間を取った
FRAMING_MAX_DX = 20.0     # |dx| の上限 (codex R2). お手本の dx は +0.00..+7.23 (2026-10-21 実測)
# response (相関の強さ) がこれ未満なら, dx/dy を信用できないので「測れない」として止める (codex R1).
# 2026-10-21 実測: 正しく対応している比較は 0.411..1.000.
# 無関係な画像は ランダム 0.0375 / 真っ黒 0.0000, 10/17 の 222723 の 1 枚目は 0.0505
# (dy=+8.2 / dx=-2.5 なので, 相関を見ないと「OK」で通ってしまう).
FRAMING_MIN_RESPONSE = 0.20
WINDOW = "camera_align (q で終了)"


def to_small_bgr(frame):
    """学習画像と同じ大きさに縮める (run_policy_client.grab_rgb と同じ処理)."""
    h, w = frame.shape[:2]
    s = IMG_LONG / max(h, w)
    return cv2.resize(frame, (round(w * s), round(h * s)), interpolation=cv2.INTER_AREA)


def load_reference(raw_dir: Path):
    """基準画像 = --raw-dir の最初のエピソードの 1 枚目 (HOME の姿勢). 戻り値: (BGR 画像, パス)."""
    eps = sorted(p.parent for p in raw_dir.glob("*/episode.npz"))
    if not eps:
        sys.exit(f"[ERROR] お手本が見つからない (基準画像を決められない): {raw_dir}")
    frames = sorted((eps[0] / "frames").glob("*.jpg"))
    if not frames:
        sys.exit(f"[ERROR] お手本の画像が無い: {eps[0]}")
    bgr = cv2.imread(str(frames[0]))
    if bgr is None:
        sys.exit(f"[ERROR] 基準画像を読めない: {frames[0]}")
    return bgr, frames[0]


def measure_framing(ref_bgr, cur_bgr):
    """写り方のずれを測る. 戻り値: (dx, dy, response). cv2.phaseCorrelate + ハニング窓.

    2026-10-21 実測 (基準 = teach_v1 の run_0001_20261019_183454 の 1 枚目):
      お手本 191257   dy= +9.1  / お手本 195120 dy= -6.7   ← ブロックの位置で揺れる範囲
      ドライラン 082512 dy= -7.8                            ← お手本と同じ写り方. 通す
      実機 082645       dy=+37.5                            ← 下にずれた写り方. 止める
    dx はブロックの位置で ±6 ほど動く (お手本どうしで +5.0 / +5.8) ので判定には使わず, 表示だけ.
    """
    a = cv2.cvtColor(ref_bgr, cv2.COLOR_BGR2GRAY).astype(np.float32)
    b = cv2.cvtColor(cur_bgr, cv2.COLOR_BGR2GRAY).astype(np.float32)
    if a.shape != b.shape:
        b = cv2.resize(b, (a.shape[1], a.shape[0]), interpolation=cv2.INTER_AREA)
    win = cv2.createHanningWindow((a.shape[1], a.shape[0]), cv2.CV_32F)
    (dx, dy), response = cv2.phaseCorrelate(a, b, win)
    return float(dx), float(dy), float(response)


def framing_verdict(dx, dy, response):
    """写り方の判定. 戻り値: (verdict, 理由). verdict は "OK" / "ずれ" / "測れない".

    相関が弱い (response が低い) ときは dx/dy 自体が当てにならないので,
    「ずれ」ではなく「測れない」として扱う (既定ではどちらも止める).
    """
    if not all(np.isfinite(v) for v in (dx, dy, response)):
        return "測れない", "dx / dy / response に非有限値"
    if response < FRAMING_MIN_RESPONSE:
        return "測れない", (f"相関が弱く dx/dy が当てにならない "
                            f"(response={response:.3f} < {FRAMING_MIN_RESPONSE:.2f})")
    bad = []
    if abs(dy) > FRAMING_MAX_DY:
        bad.append(f"|dy|={abs(dy):.1f} > {FRAMING_MAX_DY:.0f}")
    if abs(dx) > FRAMING_MAX_DX:
        bad.append(f"|dx|={abs(dx):.1f} > {FRAMING_MAX_DX:.0f}")
    if bad:
        return "ずれ", " / ".join(bad)
    return "OK", (f"|dy|<={FRAMING_MAX_DY:.0f} / |dx|<={FRAMING_MAX_DX:.0f} / "
                  f"response>={FRAMING_MIN_RESPONSE:.2f}")


def open_top_camera():
    cams = camera_resolver.resolve_cameras(require_top=True, require_side=False, verbose=False)
    cap = cv2.VideoCapture(cams["top"])
    if not cap.isOpened():
        sys.exit("[ERROR] 上カメラを開けない")
    t0 = time.time()
    while time.time() - t0 < 8.0:           # 連係カメラは映像が来るまで数秒かかることがある
        ok, f = cap.read()
        if ok and f is not None:
            return cap
        time.sleep(0.2)
    cap.release()
    sys.exit("[ERROR] 上カメラから映像が来ない. iPhone のロック解除・連係カメラの接続を確認 "
             "(Photo Booth の『カメラ』メニューに iPhone が出るか)")


def draw_overlay(small_bgr, ref_bgr, dx, dy, resp, alpha, scale):
    ref = ref_bgr
    if ref.shape[:2] != small_bgr.shape[:2]:
        ref = cv2.resize(ref, (small_bgr.shape[1], small_bgr.shape[0]), interpolation=cv2.INTER_AREA)
    view = cv2.addWeighted(small_bgr, 1.0 - alpha, ref, alpha, 0)
    big = cv2.resize(view, None, fx=scale, fy=scale, interpolation=cv2.INTER_NEAREST)
    verdict, _why = framing_verdict(dx, dy, resp)
    # cv2.putText は日本語を出せないので画面はローマ字. 端末には日本語で出す.
    label = {"OK": "OK", "ずれ": "ZURE", "測れない": "NO MATCH"}[verdict]
    color = (0, 200, 0) if verdict == "OK" else (0, 0, 255)
    cv2.putText(big, label, (16, 56), cv2.FONT_HERSHEY_SIMPLEX, 1.8, color, 4, cv2.LINE_AA)
    cv2.putText(big, f"dy={dy:+6.1f}  dx={dx:+6.1f}  (OK: both <= {FRAMING_MAX_DY:.0f})", (16, 92),
                cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2, cv2.LINE_AA)
    cv2.putText(big, f"resp={resp:.3f}  (OK: >= {FRAMING_MIN_RESPONSE:.2f})   q: quit", (16, 116),
                cv2.FONT_HERSHEY_SIMPLEX, 0.6, (220, 220, 220), 1, cv2.LINE_AA)
    return big


def main():
    ap = argparse.ArgumentParser(description="上カメラの写り方をお手本に合わせる (アームは動かさない・表示だけ)")
    ap.add_argument("--raw-dir", required=True,
                    help="基準画像に使う学習エピソード (必須. 最初の 1 本の 1 枚目を基準にする)")
    ap.add_argument("--alpha", type=float, default=0.5, help="基準画像の重ね具合 (0=ライブのみ / 1=基準のみ)")
    ap.add_argument("--scale", type=float, default=3.0, help="画面の拡大倍率")
    args = ap.parse_args()

    ref_bgr, ref_path = load_reference(Path(args.raw_dir))
    print(f"[REF   ] 基準画像: {ref_path}")
    print(f"[ALIGN ] |dy| <= {FRAMING_MAX_DY:.0f} なら OK. アームは HOME に置いた状態で合わせる. "
          "q または Esc で終了 (ファイルには何も書かない)")
    cap = open_top_camera()
    last = None
    try:
        while True:
            for _ in range(2):            # バッファに溜まった古いフレームを捨てる
                cap.grab()
            ok, frame = cap.read()
            if not ok or frame is None:
                if cv2.waitKey(100) & 0xFF in (ord("q"), 27):
                    break
                continue
            small = to_small_bgr(frame)
            dx, dy, resp = measure_framing(ref_bgr, small)
            cv2.imshow(WINDOW, draw_overlay(small, ref_bgr, dx, dy, resp, args.alpha, args.scale))
            if last is None or abs(dy - last) > 1.0:      # 端末は値が動いたときだけ出す
                verdict, why = framing_verdict(dx, dy, resp)
                print(f"[ALIGN ] dy={dy:+6.1f}  dx={dx:+6.1f}  response={resp:.3f}  "
                      f"→ {verdict} ({why})")
                last = dy
            if cv2.waitKey(30) & 0xFF in (ord("q"), 27):
                break
    except KeyboardInterrupt:
        print("\n[STOP  ] Ctrl-C")
    finally:
        cap.release()
        cv2.destroyAllWindows()
        print("[DONE  ] 終了 (何も書いていません)")


if __name__ == "__main__":
    main()
