"""SO-101 の机側カメラ (Innomaker-U20CAM-720P) のピントと置き場所を見ながら合わせる画面 (依頼 257).

アーム・サーボには触らない. カメラの設定 (露出・ホワイトバランスなど) も書き換えない.
頼むのは 1280x720・MJPG・30fps の取り込みだけ.

カメラの見つけ方:
- ffmpeg -f avfoundation -list_devices true -i "" で名前に "Innomaker" を含む機器を探す
- cv2_enumerate_cameras (camera_resolver と同じ) で OpenCV の番号と名前の対を取り,
  同じ名前の番号を開く. 両方の一覧に名前が出たときだけ開く.
- 見つからなければ両方の一覧を出して止める.

画面に重ねるもの:
- ピントの数値 (グレースケールのラプラシアンの分散) を中央と四隅の5か所で. 中央は大きく, これまでの最大値も出す
- 縦横3等分の細い線
- 解像度・実際の fps

キー: スペース = 静止画 so101/check_257/desk_{時刻}.png と同じ名前の .json (ピントの数値5つ) を保存, q = 終了.
何も押されなくても 20 分で終わる.

使い方:
    python3 so101/desk_cam_focus_257.py
    python3 so101/desk_cam_focus_257.py --score-image so101/check_248/desk_opencv1.png   # カメラを開かずに数値だけ
"""

import argparse
import json
import re
import shutil
import subprocess
import sys
import time
from datetime import datetime
from pathlib import Path

import cv2
import numpy as np

NAME_KEY = "innomaker"
WIDTH, HEIGHT, FPS = 1280, 720, 30
TIMEOUT_S = 20 * 60
ROI_FRAC = 0.2  # 1か所の大きさ (幅・高さに対する割合)
OUT_DIR = Path(__file__).resolve().parent / "check_257"
REGIONS = ["center", "top_left", "top_right", "bottom_left", "bottom_right"]
FFMPEG_FALLBACKS = [
    "/opt/homebrew/Caskroom/miniforge/base/envs/lerobot/bin/ffmpeg",
    "/opt/homebrew/bin/ffmpeg",
]


# ---- ピントの数値 (カメラなしで動く) -------------------------------------------

def region_boxes(w, h, frac=ROI_FRAC):
    """5か所の (x, y, w, h). 四隅は端から少し内側に置く."""
    rw, rh = int(w * frac), int(h * frac)
    mx, my = int(w * 0.03), int(h * 0.03)
    return {
        "center": ((w - rw) // 2, (h - rh) // 2, rw, rh),
        "top_left": (mx, my, rw, rh),
        "top_right": (w - mx - rw, my, rw, rh),
        "bottom_left": (mx, h - my - rh, rw, rh),
        "bottom_right": (w - mx - rw, h - my - rh, rw, rh),
    }


def laplacian_var(gray):
    return float(cv2.Laplacian(gray, cv2.CV_64F).var())


def focus_scores(frame):
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) if frame.ndim == 3 else frame
    h, w = gray.shape[:2]
    return {k: laplacian_var(gray[y:y + bh, x:x + bw]) for k, (x, y, bw, bh) in region_boxes(w, h).items()}


# ---- カメラの見つけ方 ------------------------------------------------------------

def ffmpeg_video_devices():
    exe = shutil.which("ffmpeg") or next((p for p in FFMPEG_FALLBACKS if Path(p).exists()), None)
    if exe is None:
        return None, []
    r = subprocess.run([exe, "-hide_banner", "-f", "avfoundation", "-list_devices", "true", "-i", ""],
                       capture_output=True, text=True)
    devs, in_video = [], False
    for line in r.stderr.splitlines():
        if "video devices" in line:
            in_video = True
            continue
        if "audio devices" in line:
            break
        m = re.search(r"\]\s*\[(\d+)\]\s*(.+)$", line)
        if in_video and m:
            devs.append((int(m.group(1)), m.group(2).strip()))
    return exe, devs


def opencv_devices():
    try:
        from cv2_enumerate_cameras import enumerate_cameras
    except ImportError:
        return None
    return [(c.index, c.name or "") for c in enumerate_cameras(cv2.CAP_AVFOUNDATION)]


def resolve_index():
    exe, ff = ffmpeg_video_devices()
    cvd = opencv_devices()
    print(f"[CAM] ffmpeg ({exe}) の映像機器:")
    for i, n in ff:
        print(f"  ffmpeg [{i}] {n}")
    print("[CAM] OpenCV の番号と名前 (cv2_enumerate_cameras):")
    if cvd is None:
        print("  cv2_enumerate_cameras が入っていない")
    else:
        for i, n in cvd:
            print(f"  OpenCV {i} {n}")

    ff_hit = [(i, n) for i, n in ff if NAME_KEY in n.lower()]
    cv_hit = [(i, n) for i, n in (cvd or []) if NAME_KEY in n.lower()]
    if not ff_hit or not cv_hit:
        print(f"[CAM] 名前に Innomaker を含む機器が見つからない (ffmpeg {len(ff_hit)} 台, OpenCV {len(cv_hit)} 台). 止める.")
        return None
    if len(cv_hit) > 1:
        print(f"[CAM] Innomaker が複数ある: {cv_hit}. 止める.")
        return None
    idx, name = cv_hit[0]
    print(f"[CAM] ffmpeg [{ff_hit[0][0]}] {ff_hit[0][1]} と OpenCV {idx} {name} が同じ名前 → OpenCV {idx} を開く")
    return idx


# ---- 画面 -------------------------------------------------------------------------

def put(img, text, org, scale, color, thick):
    cv2.putText(img, text, org, cv2.FONT_HERSHEY_SIMPLEX, scale, (0, 0, 0), thick + 3, cv2.LINE_AA)
    cv2.putText(img, text, org, cv2.FONT_HERSHEY_SIMPLEX, scale, color, thick, cv2.LINE_AA)


def draw_overlay(frame, scores, best, fps, remain_s):
    img = frame.copy()
    h, w = img.shape[:2]
    for k in (1, 2):
        cv2.line(img, (w * k // 3, 0), (w * k // 3, h), (255, 255, 255), 1)
        cv2.line(img, (0, h * k // 3), (w, h * k // 3), (255, 255, 255), 1)
    for k, (x, y, bw, bh) in region_boxes(w, h).items():
        color = (0, 255, 255) if k == "center" else (0, 255, 0)
        cv2.rectangle(img, (x, y), (x + bw, y + bh), color, 1)
        if k != "center":
            put(img, f"{scores[k]:.0f}", (x + 4, y + bh - 6), 0.6, color, 1)
    cx, cy, cw, ch = region_boxes(w, h)["center"]
    put(img, f"{scores['center']:.0f}", (cx, cy - 40), 1.6, (0, 255, 255), 3)
    put(img, f"max {best:.0f}", (cx, cy - 8), 0.8, (0, 200, 255), 2)
    put(img, f"{w}x{h}  {fps:.1f} fps   space=save  q=quit   {int(remain_s) // 60}:{int(remain_s) % 60:02d}",
        (10, h - 12), 0.6, (255, 255, 255), 1)
    return img


def save_still(frame, scores, best, idx):
    OUT_DIR.mkdir(parents=True, exist_ok=True)
    stem = f"desk_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
    png, js = OUT_DIR / f"{stem}.png", OUT_DIR / f"{stem}.json"
    if png.exists():
        print(f"[SAVE] {png} がすでにある. 保存しない")
        return
    cv2.imwrite(str(png), frame)
    js.write_text(json.dumps({"image": png.name, "opencv_index": idx, "shape": list(frame.shape),
                              "focus": scores, "center_max_so_far": best}, ensure_ascii=False, indent=2))
    print(f"[SAVE] {png}  " + "  ".join(f"{k}={scores[k]:.1f}" for k in REGIONS))


def run_camera():
    idx = resolve_index()
    if idx is None:
        return 1
    cap = cv2.VideoCapture(idx, cv2.CAP_AVFOUNDATION)
    if not cap.isOpened():
        print(f"[CAM] OpenCV {idx} を開けない")
        return 1
    cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, WIDTH)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, HEIGHT)
    cap.set(cv2.CAP_PROP_FPS, FPS)
    print(f"[CAM] 開いた: {int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))}x{int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))} "
          f"{cap.get(cv2.CAP_PROP_FPS):.1f}fps")

    win = "desk cam focus (257)"
    cv2.namedWindow(win, cv2.WINDOW_NORMAL)
    t_start = time.time()
    best, fps, stamps = 0.0, 0.0, []
    saved = 0
    try:
        while True:
            now = time.time()
            if now - t_start >= TIMEOUT_S:
                print("[END] 20分たったので終わる")
                break
            ok, frame = cap.read()
            if not ok:
                if cv2.waitKey(10) & 0xFF == ord("q"):
                    break
                continue
            stamps.append(time.time())
            stamps = stamps[-30:]
            if len(stamps) > 1:
                fps = (len(stamps) - 1) / (stamps[-1] - stamps[0])
            scores = focus_scores(frame)
            best = max(best, scores["center"])
            cv2.imshow(win, draw_overlay(frame, scores, best, fps, TIMEOUT_S - (now - t_start)))
            key = cv2.waitKey(1) & 0xFF
            if key == ord("q"):
                print("[END] q で終わる")
                break
            if key == ord(" "):
                save_still(frame, scores, best, idx)
                saved += 1
    finally:
        cap.release()
        cv2.destroyAllWindows()
        cv2.waitKey(1)
    print(f"[END] 保存 {saved} 枚, 中央の最大 {best:.1f}")
    return 0


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--score-image", nargs="+", help="カメラを開かずに画像のピントの数値を出す")
    args = ap.parse_args()
    if args.score_image:
        for p in args.score_image:
            img = cv2.imread(p)
            if img is None:
                print(f"{p}: 読めない")
                continue
            s = focus_scores(img)
            print(f"{p} {img.shape[1]}x{img.shape[0]}  " + "  ".join(f"{k}={s[k]:.1f}" for k in REGIONS))
        return 0
    return run_camera()


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