"""問題に寄る動画を作る（サービスロボット導入シミュレーション・2026-11-02）

カメラの目的を分ける（エイイチと決めた）
- 引き：全体の流れを見る（60°見下ろし・左30°／右30°の店全体）
- 寄り：問題が起きている所を見る（ロボットが動けなくなった・長く止まった所と時刻）

問題がどこでいつ起きるかは回してみないと分からないので、1回の回で引きを高解像度（3840×2160）で撮り、
その回の stops.csv から問題を拾って、引きの映像から切り出して寄りを作る（寄りのカメラを別に置かない）。

流れ
1. stops.csv の止まりのうち、長いもの（MIN_STOP 秒以上、または「動けなくなった」）を拾う
2. 同じ所（SAME_M 以内）で続けて起きた止まりを1つの「問題」にまとめ、止まっていた秒の合計で並べる
3. 上位 TOP 件それぞれについて：
   - 引き：問題が始まる少し前から、その場所に輪を付けて見せる（どこで起きたか）
   - 寄り：問題の場所を中心に 1280×720 で切り出す（何が起きたか）。左30°・右30°のうち、その場所が画面の真ん中に近い方を使う
4. 引き（全体の流れ）→ 問題ごとに［引きで場所 → 寄り］をつなぐ

使い方: python3 focus_clips.py <録画のフォルダ> <回のフォルダ> <出力フォルダ>
  要るもの：python3・numpy・Pillow・ffmpeg。問題の数は FOCUS_TOP（既定 3）、字は FOCUS_FONT で替えられる
  問題が1つもない回（30秒以上の止まりがない）は、引きだけの動画になる
  録画のフォルダ：{名前}.mp4・{名前}_frames.csv・{名前}_camera.json（wide_l30・wide_r30）
"""
import csv
import json
import os
import math
import subprocess
import sys
from pathlib import Path

import numpy as np
from PIL import Image, ImageDraw, ImageFont

MIN_STOP = 30.0      # これ以上の止まりを問題として拾う（秒・店の時刻）
SAME_M = 1.5         # この距離以内の止まりは同じ所
SAME_GAP = 120.0     # 前の止まりが終わってからこの秒以内なら同じ問題
TOP = int(os.environ.get("FOCUS_TOP", "3"))   # 動画にする問題の数
CROP = (1280, 720)   # 寄りの切り出しの大きさ（3840×2160 の 1/3）
LEAD = 10.0          # 問題が始まる前から見せる秒（店の時刻）
TAIL = 6.0
MAX_FOCUS = 120.0    # 寄りの長さの上限（店の時刻）
WIDE_SEC = 3.0       # 問題ごとの「引きで場所」の長さ（動画の秒）
import os
# 字（Linux の Noto、Mac のヒラギノ）。見つかった最初のものを使う
FONT = next((f for f in (os.environ.get("FOCUS_FONT", ""),
                         "/usr/share/fonts/opentype/noto/NotoSansCJK-Bold.ttc",
                         "/System/Library/Fonts/ヒラギノ角ゴシック W6.ttc",
                         "/System/Library/Fonts/Hiragino Sans GB.ttc",
                         "/Library/Fonts/Arial Unicode.ttf") if f and os.path.exists(f)), None)
CAMS = ("wide_l30", "wide_r30")


def load_cam(rec, name):
    J = json.load(open(next(rec.glob(f"*{name}_camera.json")), encoding="utf-8"))
    V = np.array(J["worldToCameraMatrix"], dtype=float).reshape(4, 4)
    P = np.array(J["projectionMatrix"], dtype=float).reshape(4, 4)
    W, H = J["width"], J["height"]
    mp4 = next(f for f in rec.glob(f"*{name}.mp4"))
    frames = [(int(r["frame"]), float(r["t_s"])) for r in csv.DictReader(open(next(rec.glob(f"*{name}_frames.csv"))))]

    def to_px(x, y):
        """店の座標 (x, y)（床）→ 画面の画素 (u, v)。Unity は x→X、y→Z、床の高さ Y=0"""
        c = P @ V @ np.array([x, 0.0, y, 1.0])
        nx, ny = c[0] / c[3], c[1] / c[3]
        return (nx + 1) / 2 * W, (1 - ny) / 2 * H

    return dict(name=name, mp4=mp4, W=W, H=H, frames=frames, to_px=to_px)


def t_to_frame(frames, t):
    return min(frames, key=lambda f: abs(f[1] - t))[0]


def episodes(run):
    rows = list(csv.DictReader(open(run / "stops.csv", encoding="utf-8")))
    ev = [r for r in rows if float(r["duration_s"]) >= MIN_STOP or r["resolved"] == "動けなくなった"]
    ev.sort(key=lambda r: float(r["t_s"]))
    eps = []
    for r in ev:
        t0, d = float(r["t_s"]), float(r["duration_s"])
        x, y = float(r["x"]), float(r["z"])
        for e in eps:
            if math.hypot(e["x"] - x, e["y"] - y) <= SAME_M and t0 - e["t1"] <= SAME_GAP:
                e["t1"] = max(e["t1"], t0 + d)
                e["total"] += d
                e["n"] += 1
                e["robots"].add(r["robot_id"])
                e["causes"][r["cause"]] = e["causes"].get(r["cause"], 0) + d
                e["stuck"] += r["resolved"] == "動けなくなった"
                break
        else:
            eps.append(dict(x=x, y=y, t0=t0, t1=t0 + d, total=d, n=1, robots={r["robot_id"]},
                            causes={r["cause"]: d}, stuck=int(r["resolved"] == "動けなくなった")))
    eps.sort(key=lambda e: -e["total"])
    return eps


def mmss(t):
    return f"{int(t // 60)}:{int(t % 60):02d}"


def caption(e):
    cause = max(e["causes"], key=e["causes"].get)
    what = {"もう1台のロボット": "ロボットどうしが道をふさぎ合って止まる",
            "人が立ち止まっていた": "人が立ち止まっていて、ロボットが止まる",
            "お客さんとすれ違った": "お客さんとすれ違えず、ロボットが止まる",
            "スタッフとすれ違った": "スタッフとすれ違えず、ロボットが止まる"}.get(cause, f"ロボットが止まる（{cause}）")
    return what, f"店の時刻 {mmss(e['t0'])}〜{mmss(e['t1'])}・止まった合計 {int(e['total'])}秒（{e['n']}回）"


def overlay_png(path, W, H, title, sub, ring=None, ring_r=60):
    im = Image.new("RGBA", (W, H), (0, 0, 0, 0))
    d = ImageDraw.Draw(im)
    s = W / 1280
    if FONT is None:
        raise SystemExit("日本語の字が見つからない。FOCUS_FONT に .ttc/.ttf の場所を入れて回す")
    f1, f2 = ImageFont.truetype(FONT, int(30 * s)), ImageFont.truetype(FONT, int(22 * s))
    pad = int(14 * s)
    tw = max(d.textlength(title, font=f1), d.textlength(sub, font=f2)) + 2 * pad
    d.rectangle([0, 0, tw, int(86 * s)], fill=(17, 17, 17, 210))
    d.text((pad, int(8 * s)), title, font=f1, fill=(245, 245, 240, 255))
    d.text((pad, int(50 * s)), sub, font=f2, fill=(220, 220, 214, 255))
    if ring:
        u, v = ring
        r = ring_r * s
        d.ellipse([u - r, v - r, u + r, v + r], outline=(232, 89, 12, 255), width=max(3, int(6 * s)))
    im.save(path)


def run_ff(args):
    subprocess.run(["ffmpeg", "-y", "-loglevel", "error", *args], check=True)


def clip(cam, f0, f1, out, crop=None, ov=None, speed=1.0):
    vf = [f"trim=start_frame={f0}:end_frame={f1}", "setpts=PTS-STARTPTS"]
    if speed != 1.0:
        vf.append(f"setpts={1 / speed}*PTS")
    if crop:
        cx, cy = crop
        vf.append(f"crop={CROP[0]}:{CROP[1]}:{cx}:{cy}")
    else:
        vf.append(f"scale={CROP[0]}:{CROP[1]}")
    chain = ",".join(vf)
    if ov:
        run_ff(["-i", str(cam["mp4"]), "-i", str(ov), "-filter_complex",
                f"[0:v]{chain}[b];[b][1:v]overlay=0:0,format=yuv420p,fps=30[v]", "-map", "[v]",
                "-c:v", "libx264", "-preset", "veryfast", "-crf", "20", str(out)])
    else:
        run_ff(["-i", str(cam["mp4"]), "-vf", chain + ",format=yuv420p,fps=30", "-c:v", "libx264", "-preset", "veryfast", "-crf", "20", str(out)])


def main():
    rec, run, out = map(Path, sys.argv[1:4])
    out.mkdir(parents=True, exist_ok=True)
    cams = {n: load_cam(rec, n) for n in CAMS}
    eps = episodes(run)
    json.dump([{**e, "robots": sorted(e["robots"])} for e in eps], open(out / "episodes.json", "w", encoding="utf-8"), ensure_ascii=False, indent=1)
    parts = []
    # 引き：全体の流れ（左30°・回の最初の問題の少し前から 8 秒）
    L = cams["wide_l30"]
    t_open = max(L["frames"][0][1], (min(e["t0"] for e in eps[:TOP]) if eps else L["frames"][0][1] + 900) - 120)
    f0 = t_to_frame(L["frames"], t_open)
    ov = out / "ov_open.png"
    overlay_png(ov, CROP[0], CROP[1], "引き：店全体の流れ", "60°見下ろし・左30°")
    clip(L, f0, f0 + 240, out / "p00_open.mp4", ov=ov)
    parts.append(out / "p00_open.mp4")
    for i, e in enumerate(eps[:TOP], 1):
        # その場所が画面の真ん中に近いカメラを使う
        best = min(cams.values(), key=lambda c: math.hypot(c["to_px"](e["x"], e["y"])[0] - c["W"] / 2, c["to_px"](e["x"], e["y"])[1] - c["H"] / 2))
        u, v = best["to_px"](e["x"], e["y"])
        what, sub = caption(e)
        # 引きで場所（輪を付ける）
        fs = t_to_frame(best["frames"], e["t0"] - LEAD)
        ov = out / f"ov_{i}_wide.png"
        k = CROP[0] / best["W"]
        overlay_png(ov, CROP[0], CROP[1], f"問題{i}：ここで起きる", "引き・" + ("左30°" if best["name"].endswith("l30") else "右30°"), ring=(u * k, v * k), ring_r=40)
        clip(best, fs, fs + int(WIDE_SEC * 30), out / f"p{i:02d}a_wide.mp4", ov=ov)
        # 寄り：問題の場所を中心に切り出す
        cx = int(min(max(u - CROP[0] / 2, 0), best["W"] - CROP[0]))
        cy = int(min(max(v - CROP[1] / 2, 0), best["H"] - CROP[1]))
        f0 = t_to_frame(best["frames"], e["t0"] - LEAD)
        f1 = t_to_frame(best["frames"], min(e["t1"] + TAIL, e["t0"] + MAX_FOCUS))
        ov = out / f"ov_{i}_focus.png"
        overlay_png(ov, CROP[0], CROP[1], f"寄り：{what}", sub, ring=(u - cx, v - cy), ring_r=70)
        clip(best, f0, f1, out / f"p{i:02d}b_focus.mp4", crop=(cx, cy), ov=ov, speed=0.5)
        parts += [out / f"p{i:02d}a_wide.mp4", out / f"p{i:02d}b_focus.mp4"]
    lst = out / "list.txt"
    lst.write_text("".join(f"file '{p.name}'\n" for p in parts))
    run_ff(["-f", "concat", "-safe", "0", "-i", str(lst), "-c", "copy", str(out / "focus_trial.mp4")])
    for i, e in enumerate(eps[:TOP], 1):
        print(i, caption(e), (round(e["x"], 1), round(e["y"], 1)))
    print("OK", out / "focus_trial.mp4")


if __name__ == "__main__":
    main()
