#!/usr/bin/env python3
"""依頼139：病院の立体の見た目の静止画・動画に、問題の輪と右のパネルを付ける（1920×1080）。

左 1360×1080＝Unity で撮った 2720×2160 を縮めたもの。輪は床の上（高さ FLOOR_Y）に、cam_{name}.json の行列で画素に移して描く
（依頼136 の vid136.py と同じ色・同じ印。10 秒出す）。引きのカメラ（top_wide・obl_l30_wide）だけ、ロボットの位置に白の輪を足す。
右 560＝配膳のパネル（rec_lp_20261105/p0_round/after/round_after_panel.png）と同じ作り：白地・小さな見出し・大きな時刻・
「ロボット・スタッフがしていること」・数。中身は病院の値（ロボットの状態・助けに出ているスタッフ・大廊下の人数・撮り始めからの問題1〜5）。

  python3 pan139.py stills <回のフォルダ>
  python3 pan139.py video <回のフォルダ> <cam> <出力.mp4>
  python3 pan139.py sheets <回のフォルダ> <出力フォルダ>
"""
import csv, json, math, os, subprocess, sys, tempfile
from PIL import Image, ImageDraw, ImageFont

FFMPEG = "/opt/homebrew/Caskroom/miniforge/base/envs/ros_env/bin/ffmpeg"
FONT_B = os.path.expanduser("~/Library/Fonts/NotoSansJP-Bold.otf")
FONT_R = os.path.expanduser("~/Library/Fonts/NotoSansJP-Regular.otf")
W, H, SW = 1920, 1080, 1360           # 全体・左の場面の幅（配膳のパネル付きと同じ割り）
FLOOR_Y = 0.2                          # 床の上の面（StoreLayout.floorThickness）
REC_FROM = 8845.0                      # 撮り始め（動画の始め。静止画の数もここから数える）
SHOW_S = 10.0
TYPES = {1: "急に止まった", 2: "ひやり", 3: "人がよけさせられた", 4: "立ち往生", 5: "横切り"}
COLORS = {1: (220, 30, 40), 2: (255, 120, 0), 3: (230, 200, 0), 4: (150, 60, 210), 5: (20, 110, 230)}
FAST_COL, OVER_COL = (255, 0, 0), (0, 0, 0)
# 立体の色（配膳の材質。画面での見え方に近い値）
YELLOW, BLUE, WHITE, DARK = (250, 204, 33), (23, 97, 230), (255, 255, 255), (77, 79, 84)
INK, GRAY, PANEL_BG, LINE = (29, 29, 27), (110, 110, 108), (249, 247, 244), (200, 198, 194)
CAM_JA = {"top_wide": "1 真上・引き", "top_near": "2 真上・寄り", "obl_l30_wide": "3 60°見下ろし・左30°・引き",
          "obl_r30_near": "4 60°見下ろし・右30°・寄り"}
CAM_ORDER = ["top_wide", "top_near", "obl_l30_wide", "obl_r30_near"]
WIDE = {"top_wide", "obl_l30_wide"}
SCHEME_JA = {"A": "A ミント×木目", "B": "B 生成り×木目", "C": "C 黄緑×木目"}
STATE = [("待ち", "待っている"), ("行き", "行き"), ("帰り", "帰り"), ("移る", "表と裏口を移る")]


def font(b, s):
    return ImageFont.truetype(FONT_B if b else FONT_R, s)


def clock(t):
    s = int(math.floor(7.5 * 3600 + t + 1e-6))
    return "%d時%02d分%02d秒" % (s // 3600, s % 3600 // 60, s % 60)


class Cam:
    def __init__(self, path):
        j = json.load(open(path, encoding="utf-8"))
        self.name, self.w, self.h = j["name"], j["width"], j["height"]
        self.v, self.p = j["worldToCameraMatrix"], j["projectionMatrix"]
        self.sx, self.sy = SW / self.w, H / self.h

    def px(self, x, z, y=FLOOR_Y):
        P = (x, y, z, 1.0)
        c = [sum(self.v[r][k] * P[k] for k in range(4)) for r in range(4)]
        q = [sum(self.p[r][k] * c[k] for k in range(4)) for r in range(4)]
        if q[3] <= 1e-6:
            return None
        u = (q[0] / q[3] + 1) / 2 * self.w * self.sx
        v = (1 - q[1] / q[3]) / 2 * self.h * self.sy
        return u, v

    def ppm(self, x, z):
        a, b = self.px(x - 0.5, z), self.px(x + 0.5, z)
        return math.hypot(b[0] - a[0], b[1] - a[1])


def ring(d, cam, x, z, r_m, min_px, col, width):
    r = max(r_m, min_px / max(cam.ppm(x, z), 1e-3))
    pts = [cam.px(x + r * math.cos(a), z + r * math.sin(a)) for a in [i * 2 * math.pi / 48 for i in range(49)]]
    if any(p is None for p in pts):
        return
    d.line(pts, fill=col, width=width, joint="curve")


def square(d, cam, x, z, r_m, min_px, col, width):
    r = max(r_m, min_px / max(cam.ppm(x, z), 1e-3))
    pts = [cam.px(x - r, z - r), cam.px(x + r, z - r), cam.px(x + r, z + r), cam.px(x - r, z + r), cam.px(x - r, z - r)]
    d.line(pts, fill=col, width=width)
    return pts


def overlay(im, cam, ev, t, fr, robot_xz):
    d = ImageDraw.Draw(im)
    if cam.name in WIDE and robot_xz is not None:
        ring(d, cam, robot_xz[0], robot_xz[1], 1.2, 15, (40, 40, 40), 6)
        ring(d, cam, robot_xz[0], robot_xz[1], 1.2, 15, (255, 255, 255), 3)
    for e in ev:
        te = float(e["t_s"])
        if te > t + 1e-6 or t - te > SHOW_S:
            continue
        k = int(e["type"])
        fade = 1.0 - (t - te) / SHOW_S * 0.7
        x, z = float(e["x"]), float(e["z"])
        layer = Image.new("RGBA", im.size, (0, 0, 0, 0))
        mark(ImageDraw.Draw(layer), cam, e, k, x, z, fr)
        alpha = layer.split()[3].point(lambda v: int(v * fade))
        layer.putalpha(alpha)
        im.paste(layer, (0, 0), layer)


def mark(d, cam, e, k, x, z, fr):
    a = 255
    if True:
        if k == 6:
            p = square(d, cam, x, z, 0.8, 22, (255, 230, 0, a), 5)
            d.line([p[0], p[2]], fill=OVER_COL + (a,), width=6)
            d.line([p[1], p[3]], fill=OVER_COL + (a,), width=6)
            return
        if k == 2 and e.get("note") == "速いまま":
            square(d, cam, x, z, 0.65, 18, FAST_COL + (a,), 4)
            p = square(d, cam, x, z, 0.85, 25, FAST_COL + (a,), 3)
            d.text((max(q[0] for q in p) + 3, min(q[1] for q in p)), "速", font=fr, fill=FAST_COL + (a,))
            return
        ring(d, cam, x, z, 0.6 if k != 4 else 0.85, 12 if k != 4 else 18, COLORS[k] + (a,), 4)


def legend(im):
    d = ImageDraw.Draw(im)
    f = font(False, 24)
    items = ["患者・付き添い", "杖の人", "車いす", "スタッフ", "ストレッチャー", "ロボット"]
    x0, y0, x = 24, 24, 44
    widths = [int(d.textlength(s, font=f)) for s in items]
    total = sum(widths) + len(items) * 52 + 20
    d.rectangle([x0, y0, x0 + total, y0 + 56], fill=(242, 241, 238))
    y = y0 + 28
    for s, wdt in zip(items, widths):
        if s == "患者・付き添い":
            d.ellipse([x, y - 11, x + 22, y + 11], fill=YELLOW)
        elif s == "杖の人":
            d.ellipse([x, y - 11, x + 22, y + 11], fill=YELLOW)
            d.line([x + 25, y - 9, x + 25, y + 12], fill=DARK, width=4)
        elif s == "車いす":
            d.rectangle([x - 1, y - 13, x + 23, y + 13], fill=DARK)
            d.ellipse([x + 4, y - 7, x + 18, y + 7], fill=YELLOW)
        elif s == "スタッフ":
            d.ellipse([x, y - 11, x + 22, y + 11], fill=BLUE)
        elif s == "ストレッチャー":
            d.rectangle([x - 2, y - 8, x + 18, y + 8], fill=(222, 223, 225), outline=(150, 150, 150))
            d.ellipse([x + 19, y - 6, x + 31, y + 6], fill=BLUE)
        else:
            d.ellipse([x, y - 11, x + 22, y + 11], fill=WHITE, outline=(60, 60, 60), width=2)
        d.text((x + 36, y), s, font=f, fill=INK, anchor="lm")
        x += wdt + 52


def counts(ev, t):
    c = {k: 0 for k in range(1, 8)}
    for e in ev:
        te = float(e["t_s"])
        if REC_FROM - 1e-6 <= te <= t + 1e-6:
            c[int(e["type"])] += 1
            if e["type"] == "2" and e.get("note") == "速いまま":
                c[7] += 1
    return c


def panel(im, row, ev, t, scheme, camname, summ):
    d = ImageDraw.Draw(im)
    d.rectangle([SW, 0, W, H], fill=PANEL_BG)
    x0, xr = SW + 40, W - 40
    f_s, f_m, f_mb, f_big, f_num = font(False, 24), font(False, 27), font(True, 29), font(True, 54), font(True, 46)
    d.text((x0, 24), "病院 h0・%s・%.1fm/秒・種%d" % (summ["plan"], summ["speed_mps"], summ["seed"]), font=f_s, fill=GRAY)
    d.text((x0, 56), "色 %s" % SCHEME_JA[scheme], font=f_s, fill=GRAY)
    d.text((x0, 88), "カメラ %s" % CAM_JA[camname], font=f_s, fill=GRAY)
    d.text((x0, 122), clock(t), font=f_big, fill=INK)
    y = 206
    d.text((x0, y), "ロボット・スタッフがしていること", font=f_mb, fill=INK)
    y += 52
    d.text((x0, y), "ロボット 1台", font=f_m, fill=INK)
    y += 44
    cur = row["leg_dir"]
    for key, ja in STATE:
        on = key == cur
        d.text((x0 + 8, y), ja, font=f_m, fill=INK if on else (150, 150, 148))
        if on:
            d.ellipse([xr - 40, y + 2, xr - 4, y + 38], fill=WHITE, outline=(80, 80, 80), width=3)
            if key in ("行き", "帰り", "移る") and row["leg_from"]:
                d.text((xr - 52, y + 20), "%s→%s" % (row["leg_from"], row["leg_to"]), font=f_s, fill=GRAY, anchor="rm")
        y += 40
    y += 10
    d.text((x0, y), "スタッフ", font=f_m, fill=INK)
    y += 44
    rs = int(row["rescue_staff"])
    d.text((x0 + 8, y), "ロボットの助け", font=f_m, fill=INK if rs else (150, 150, 148))
    for i in range(rs):
        d.rectangle([xr - 36 - i * 44, y + 3, xr - 4 - i * 44, y + 35], fill=BLUE)
    y += 56
    d.line([x0, y, xr, y], fill=LINE, width=2)
    y += 18
    d.text((x0, y + 8), "大廊下の人（x0〜70）", font=f_m, fill=INK)
    d.text((xr, y + 28), row["hall_people"], font=f_num, fill=INK, anchor="rm")
    y += 70
    d.line([x0, y, xr, y], fill=LINE, width=2)
    y += 16
    s0 = int(7.5 * 3600 + REC_FROM)
    d.text((x0, y), "撮り始め（%d:%02d:%02d）からの問題" % (s0 // 3600, s0 % 3600 // 60, s0 % 60), font=f_mb, fill=INK)
    y += 50
    c = counts(ev, t)
    for k in range(1, 6):
        d.ellipse([x0 + 6, y + 6, x0 + 30, y + 30], outline=COLORS[k], width=4)
        d.text((x0 + 44, y + 18), "%d %s" % (k, TYPES[k]), font=f_m, fill=INK, anchor="lm")
        d.text((xr, y + 18), str(c[k]), font=font(True, 34), fill=INK, anchor="rm")
        y += 46
    y += 6
    # 印の見方（速いままのひやり・止まりきれなかった・白の輪）
    d.rectangle([x0 + 9, y + 5, x0 + 27, y + 23], outline=FAST_COL, width=3)
    d.rectangle([x0 + 4, y, x0 + 32, y + 28], outline=FAST_COL, width=2)
    d.text((x0 + 44, y + 14), "うち速いままのひやり %d" % c[7], font=f_s, fill=INK, anchor="lm")
    y += 38
    d.rectangle([x0 + 5, y + 1, x0 + 31, y + 27], outline=(255, 210, 0), width=4)
    d.line([x0 + 9, y + 5, x0 + 27, y + 23], fill=OVER_COL, width=5)
    d.line([x0 + 9, y + 23, x0 + 27, y + 5], fill=OVER_COL, width=5)
    d.text((x0 + 44, y + 14), "止まりきれなかった %d" % c[6], font=f_s, fill=INK, anchor="lm")
    y += 40
    note = "輪＝問題の起きた所（10秒）" + ("・白い輪＝ロボット" if camname in WIDE else "")
    d.text((x0, min(y + 4, H - 40)), note, font=font(False, 21), fill=GRAY)


def compose(run, rowd, ev, summ, cams):
    src = Image.open(os.path.join(run, "look139", rowd["file"])).convert("RGB").resize((SW, H), Image.LANCZOS)
    im = Image.new("RGB", (W, H), PANEL_BG)
    im.paste(src, (0, 0))
    t = float(rowd["t_s"])
    cam = cams[rowd["cam"]]
    overlay(im, cam, ev, t, font(True, 22), (float(rowd["robot_x"]), float(rowd["robot_z"])))
    legend(im)
    panel(im, rowd, ev, t, rowd["scheme"], rowd["cam"], summ)
    return im


def load(run):
    rows = list(csv.DictReader(open(os.path.join(run, "look139", "look_frames.csv"), encoding="utf-8")))
    ev = list(csv.DictReader(open(os.path.join(run, "events.csv"), encoding="utf-8")))
    summ = json.load(open(os.path.join(run, "summary.json"), encoding="utf-8"))
    cams = {n: Cam(os.path.join(run, "look139", "cam_%s.json" % n)) for n in CAM_ORDER}
    return rows, ev, summ, cams


def main():
    mode, run = sys.argv[1], sys.argv[2]
    rows, ev, summ, cams = load(run)
    if mode == "stills":
        out = os.path.join(run, "look139", "panel")
        os.makedirs(out, exist_ok=True)
        for r in rows:
            if r["kind"] != "still":
                continue
            im = compose(run, r, ev, summ, cams)
            f = os.path.join(out, "still_%s_%s.png" % (r["scheme"], r["cam"]))
            im.save(f)
            print(f)
    elif mode == "video":
        camname, dst = sys.argv[3], sys.argv[4]
        tmp = tempfile.mkdtemp(prefix="pan139_")
        vr = [r for r in rows if r["kind"] == "vid" and r["cam"] == camname]
        for i, r in enumerate(vr):
            compose(run, r, ev, summ, cams).save(os.path.join(tmp, "c%05d.png" % i))
        # 0.2 秒ごとのコマを 25 コマ/秒で流す＝店の時刻の 5 倍
        subprocess.check_call([FFMPEG, "-y", "-loglevel", "error", "-framerate", "25", "-i", os.path.join(tmp, "c%05d.png"),
                               "-c:v", "libx264", "-pix_fmt", "yuv420p", "-crf", "18", dst])
        print(dst, len(vr), "コマ", len(vr) / 25.0, "秒")
    elif mode == "sheets":
        out = sys.argv[3]
        os.makedirs(out, exist_ok=True)
        pdir = os.path.join(run, "look139", "panel")
        fh, fl = font(True, 40), font(True, 30)
        t = float([r for r in rows if r["kind"] == "still"][0]["t_s"])
        head = "依頼139 病院の立体の見た目・%s・%.1fm/秒・種%d・%s" % (summ["plan"], summ["speed_mps"], summ["seed"], clock(t))
        # カメラごとに色3案を横に並べる
        cw, ch, top = 1280, 720, 130
        for i, cn in enumerate(CAM_ORDER):
            sheet = Image.new("RGB", (cw * 3 + 40, ch + top + 20), (255, 255, 255))
            d = ImageDraw.Draw(sheet)
            d.text((20, 18), head + "　カメラ " + CAM_JA[cn], font=fh, fill=INK)
            for j, s in enumerate("ABC"):
                im = Image.open(os.path.join(pdir, "still_%s_%s.png" % (s, cn))).resize((cw, ch), Image.LANCZOS)
                x = 10 + j * (cw + 10)
                sheet.paste(im, (x, top))
                d.text((x, top - 44), SCHEME_JA[s], font=fl, fill=INK)
            f = os.path.join(out, "compare_cam%d_%s.png" % (i + 1, cn))
            sheet.save(f)
            print(f)
        # 12 枚の一覧（横＝色3案・縦＝カメラ4つ）
        cw, ch, top, left = 960, 540, 130, 260
        sheet = Image.new("RGB", (left + cw * 3 + 40, top + (ch + 12) * 4 + 10), (255, 255, 255))
        d = ImageDraw.Draw(sheet)
        d.text((20, 18), head + "　色3案×カメラ4つ", font=fh, fill=INK)
        for j, s in enumerate("ABC"):
            d.text((left + j * (cw + 10), top - 46), SCHEME_JA[s], font=fl, fill=INK)
        for i, cn in enumerate(CAM_ORDER):
            y = top + i * (ch + 12)
            for k, line in enumerate(CAM_JA[cn].split("・")):
                d.text((16, y + 20 + k * 42), line, font=font(True, 28), fill=INK)
            for j, s in enumerate("ABC"):
                im = Image.open(os.path.join(pdir, "still_%s_%s.png" % (s, cn))).resize((cw, ch), Image.LANCZOS)
                sheet.paste(im, (left + j * (cw + 10), y))
        f = os.path.join(out, "overview_12.png")
        sheet.save(f)
        print(f)


if __name__ == "__main__":
    main()