"""依頼230：229 の真上のコマ（Closeup230.cs が動かない真上のカメラで撮った frames/yield_top）に、床の目印・通った跡・印・左上の字を重ねて
closeup230_yield_top.mp4（音なし）と静止画 3 枚を作り、確かめ（check230_top.txt）を書く。
目印・跡は Unity のカメラで写した位置（top230.csv の画面の位置・top230_cam.json の写し方）に 2 倍の大きさで描いて縮める。
時刻は動画の秒（台本の秒 − video_from）。
使い方: python3 make230.py <Unity の出力フォルダ> <台本> <動画を置くフォルダ>"""
import csv, json, math, os, re, subprocess, sys, tempfile
from PIL import Image, ImageDraw, ImageFont

FF = "/opt/homebrew/Caskroom/miniforge/base/envs/ros_env/bin/ffmpeg"
D, SC, VD = sys.argv[1:4]
os.makedirs(VD, exist_ok=True)
C = json.load(open(SC))["shots"][0]["c229"]
CAM = json.load(open(os.path.join(D, "top230_cam.json")))
FPS, W, H, S = 30, 1920, 1080, 2          # S：描く倍率（2 倍で描いて縮める）
f0, f1 = int(round(C["video_from"] * FPS)), int(round(C["video_to"] * FPS))
n = f1 - f0 + 1
vt = lambda f: (f - f0) / FPS             # 動画の秒

# ---- 決めた値（1920×1080 の画面での px）
FONT_PATH = "/System/Library/Fonts/ヒラギノ角ゴシック W6.ttc"
HUD_PX, LINE_LABEL_PX, MARK_LABEL_PX = 70, 36, 40
COL = {
    "center": (120, 120, 120),       # 廊下の中心の線（細い実線・灰色）
    "x028": (21, 101, 192),          # 車いすの始めの線 +0.28（破線・青）
    "x042": (142, 36, 170),          # 寄せたあとの線 +0.42（破線・紫）
    "lane": (46, 173, 107),          # ハコブくんのふだんの線 −0.33（破線・ハコブくんの輪の光に近い緑）
    "chair": (239, 108, 0),          # 車いすの通った跡（実線・橙）
    "robot": (0, 105, 92),           # ハコブくんの通った跡（実線・深い青緑）
}
LW = {"center": 3, "dash": 5, "trail": 10, "mark_r": 13, "mark_edge": 3, "leader": 3}
DASH, GAP = 28, 16

# ---- 床の点 → 画面（Unity のカメラで写した 3 点から。正射影なので一次式）
ox, oy = CAM["px_origin"]; xx, xy = CAM["px_per_x"]; zx, zy = CAM["px_per_z"]
def to_px(x, z): return (ox + xx * x + zx * z, oy + xy * x + zy * z)
def from_px(px, py):
    det = xx * zy - zx * xy; dx, dy = px - ox, py - oy
    return ((dx * zy - zx * dy) / det, (xx * dy - dx * xy) / det)   # (x, z)

rows = {int(r["frame"]): r for r in csv.DictReader(open(os.path.join(D, "frames230.csv")))}
top = {int(r["frame"]): r for r in csv.DictReader(open(os.path.join(D, "top230.csv")))}
ev = open(os.path.join(D, "events230.txt"), encoding="utf-8").read()
t_trig = float(re.search(r"([\d.]+) 秒 決まりに当たった", ev).group(1))
t_stop = float(re.search(r"([\d.]+) 秒 止まった（", ev).group(1))
t_sh0, t_sh1 = C["shift"]["t0"], C["shift"]["t1"]
# 印：(どれ, 台本の秒, 字の頭)。出す時刻 = その秒のコマ
MARKS = [("chair", t_sh0, "寄せ始め"), ("chair", t_sh1, "寄せ終わり"), ("robot", t_trig, "寄り始め"), ("robot", t_stop, "止まった")]
mark_f = [int(round(t * FPS)) for _, t, _ in MARKS]

def font(px): return ImageFont.truetype(FONT_PATH, px * S)
FL, FM, FH = font(LINE_LABEL_PX), font(MARK_LABEL_PX), font(HUD_PX)
def sc(p): return (p[0] * S, p[1] * S)

def text_box(d, xy, s, f, fg, anchor, pad=8, bg=(255, 255, 255, 235), edge=None):
    x, y = xy
    bb = d.textbbox((x, y), s, font=f, anchor=anchor)
    d.rounded_rectangle([bb[0] - pad * S, bb[1] - pad * S, bb[2] + pad * S, bb[3] + pad * S], radius=6 * S, fill=bg, outline=edge, width=2 * S if edge else 0)
    d.text((x, y), s, font=f, fill=fg, anchor=anchor)

def dashed(d, p0, p1, col, w):
    (x0, y0), (x1, y1) = sc(p0), sc(p1)
    L = math.hypot(x1 - x0, y1 - y0); ux, uy = (x1 - x0) / L, (y1 - y0) / L
    s = 0.0
    while s < L:
        e = min(L, s + DASH * S)
        d.line([(x0 + ux * s, y0 + uy * s), (x0 + ux * e, y0 + uy * e)], fill=col, width=w * S)
        s += (DASH + GAP) * S

def down(layer): return layer.resize((W, H), Image.LANCZOS)

# ---- 動かない層：中心の線・目印の線と字
z_lo, z_hi = from_px(0, 0)[1], from_px(W, 0)[1]
LINES = {"center": 0.0, "x028": 0.28, "x042": 0.42, "lane": -0.33}
line_py = {k: to_px(x, z_lo)[1] for k, x in LINES.items()}
st = Image.new("RGBA", (W * S, H * S), (0, 0, 0, 0)); d = ImageDraw.Draw(st)
d.line([sc(to_px(0.0, z_lo)), sc(to_px(0.0, z_hi))], fill=COL["center"] + (255,), width=LW["center"] * S)
for k in ("x028", "x042", "lane"):
    dashed(d, to_px(LINES[k], z_lo), to_px(LINES[k], z_hi), COL[k] + (255,), LW["dash"])
# 字：始め +0.28（線の上）と寄せたあと +0.42（線の下）は左の端（車いすが来るのは右の端なので、入ってくる所と寄せる所を隠さない）、ハコブくんのふだん −0.33 は右の端（ハコブくんが行かない側）
text_box(d, (sc((14, line_py["x028"] - 8))), "始め +0.28", FL, COL["x028"], "lb", edge=COL["x028"])
text_box(d, (sc((14, line_py["x042"] + 8))), "寄せたあと +0.42", FL, COL["x042"], "lt", edge=COL["x042"])
text_box(d, (sc((W - 14, line_py["lane"] - 8))), "ハコブくんのふだん −0.33", FL, COL["lane"], "rb", edge=COL["lane"])
static = down(st)

# ---- 印の層（時刻になったら出す）。字は車いすの印は廊下の下、ハコブくんの印は廊下の上へ引き出す
rail_lo_py = to_px(0.875, 0)[1]; rail_hi_py = to_px(-0.875, 0)[1]
def mark_layer(shown):
    L = Image.new("RGBA", (W * S, H * S), (0, 0, 0, 0)); d = ImageDraw.Draw(L)
    nrob = 0
    for i, (who, t, head) in enumerate(MARKS):
        if not shown[i]: continue
        r = top[mark_f[i]]
        px, py = (float(r["axle_px"]), float(r["axle_py"])) if who == "chair" else (float(r["robot_px"]), float(r["robot_py"]))
        if who == "chair": ly = rail_lo_py + 40 + (0 if head == "寄せ始め" else 0); anchor = "mt"
        else: ly = rail_hi_py - 40 - (60 if head == "止まった" else 0); anchor = "mb"
        d.line([sc((px, py)), sc((px, ly))], fill=COL[who] + (255,), width=LW["leader"] * S)
        R = LW["mark_r"] * S; e = LW["mark_edge"] * S
        d.ellipse([px * S - R - e, py * S - R - e, px * S + R + e, py * S + R + e], fill=(255, 255, 255, 255))
        d.ellipse([px * S - R, py * S - R, px * S + R, py * S + R], fill=COL[who] + (255,))
        text_box(d, sc((px, ly)), f"{head} {vt(mark_f[i]):.1f}秒", FM, COL[who], anchor, edge=COL[who])
    return down(L)

# ---- 左上の字
def num_ok(s):
    try: v = float(s)
    except ValueError: return None
    return v if math.isfinite(v) and abs(v) < 1e6 else None
def hud_lines(f):
    r = rows[f]
    cx = num_ok(r["chair_x"]); g = num_ok(r["gap_m"])
    return [f"{vt(f):.1f} 秒",
            "車いすの横の位置 " + (f"x {cx * 100:+.1f} cm" if cx is not None else "—"),
            "ハコブくんと車いす・人のすき間 " + (f"{g:.2f} m" if g is not None else "—")]

# ---- コマを作る
tmp = tempfile.mkdtemp(prefix="m230_", dir="/tmp")
trail = Image.new("RGBA", (W * S, H * S), (0, 0, 0, 0)); td = ImageDraw.Draw(trail)
prev = {"chair": None, "robot": None}
shown = [False] * len(MARKS); mcache = mark_layer(shown)
chk = {"trail_err_m": {"chair": 0.0, "robot": 0.0}, "mark_first": [None] * len(MARKS), "hud_bad": 0, "hud_dash": {1: 0, 2: 0}, "hud_texts": []}
stills = {}
for k, f in enumerate(range(f0, f1 + 1)):
    r = top[f]
    for who, kx, ky, wx, wz in (("chair", "axle_px", "axle_py", "axle_x", "axle_z"), ("robot", "robot_px", "robot_py", "robot_x", "robot_z")):
        p = (round(float(r[kx]) * S) / S, round(float(r[ky]) * S) / S)   # 描く点（2 倍の画面の整数の点）
        bx, bz = from_px(*p)
        chk["trail_err_m"][who] = max(chk["trail_err_m"][who], math.hypot(bx - float(r[wx]), bz - float(r[wz])))
        if prev[who] is not None:
            td.line([sc(prev[who]), sc(p)], fill=COL[who] + (255,), width=LW["trail"] * S)
        R = LW["trail"] * S / 2
        td.ellipse([p[0] * S - R, p[1] * S - R, p[0] * S + R, p[1] * S + R], fill=COL[who] + (255,))   # 線のつなぎ目を丸く
        prev[who] = p
    changed = False
    for i in range(len(MARKS)):
        if not shown[i] and f >= mark_f[i]: shown[i] = True; changed = True; chk["mark_first"][i] = vt(f)
    if changed: mcache = mark_layer(shown)
    im = Image.open(os.path.join(D, "frames", "yield_top", f"f_{f:05d}.png")).convert("RGBA")
    im.alpha_composite(down(trail)); im.alpha_composite(static); im.alpha_composite(mcache)
    lines = hud_lines(f)
    for i in (1, 2):
        if lines[i].endswith("—"): chk["hud_dash"][i] += 1
    if any(re.search(r"\d{5,}|nan|inf", s, re.I) for s in lines): chk["hud_bad"] += 1
    if k % 30 == 0: chk["hud_texts"].append(" / ".join(lines))
    hl = Image.new("RGBA", (W * S, H * S), (0, 0, 0, 0)); hd = ImageDraw.Draw(hl)
    x0, y0, lh = 30, 22, 88
    wmax = max(hd.textbbox((0, 0), s, font=FH)[2] for s in lines) / S
    hd.rounded_rectangle([x0 * S - 8 * S, y0 * S - 6 * S, (x0 + wmax + 28) * S, (y0 + lh * 3 + 6) * S], radius=10 * S, fill=(255, 255, 255, 235))
    for i, s in enumerate(lines): hd.text(((x0 + 10) * S, (y0 + i * lh) * S), s, font=FH, fill=(25, 25, 25))
    im.alpha_composite(down(hl))
    out = im.convert("RGB"); out.save(os.path.join(tmp, f"v_{k:05d}.png"))
    for name, ff in (("s1_shift_start", mark_f[0]), ("s2_shift_end", mark_f[1]), ("s3_last", f1)):
        if f == ff: stills[name] = out.copy()
vid = os.path.join(VD, "closeup230_yield_top.mp4")
subprocess.run([FF, "-y", "-loglevel", "error", "-framerate", "30", "-i", os.path.join(tmp, "v_%05d.png"), "-c:v", "libx264", "-pix_fmt", "yuv420p", "-crf", "18", "-r", "30", vid], check=True)
sd = os.path.join(VD, "stills"); os.makedirs(sd, exist_ok=True)
for name, im in stills.items(): im.save(os.path.join(sd, name + ".png"))

# ---- 確かめ
o = []
o.append(f"コマ {n}（{f0}〜{f1}）・動画 {n / FPS:.3f} 秒")
o.append(f"写し方：画面の点 = ({ox:.4f}, {oy:.4f}) + x×({xx:.4f}, {xy:.4f}) + z×({zx:.4f}, {zy:.4f}) px（1 m = {math.hypot(zx, zy):.3f} px・1 px = {1000 / math.hypot(zx, zy):.2f} mm）")
o.append(f"写す範囲：z {z_lo:.3f}〜{z_hi:.3f}（{z_hi - z_lo:.3f} m）・x {from_px(0, 0)[0]:.3f}〜{from_px(0, H)[0]:.3f}（{from_px(0, H)[0] - from_px(0, 0)[0]:.3f} m）")
for k2, x in LINES.items():
    py = line_py[k2]; back = from_px(W / 2, py)[0]
    o.append(f"目印の線 {k2}：x {x:+.2f} → 画面の y {py:.2f} px（画面から戻した x {back:+.5f}）")
o.append(f"手すり（内側の面 x ±0.84）：画面の y {to_px(-0.84, 0)[1]:.2f}〜{to_px(0.84, 0)[1]:.2f} px（{to_px(0.84, 0)[1] - to_px(-0.84, 0)[1]:.1f} px ＝ 画面の高さの {(to_px(0.84, 0)[1] - to_px(-0.84, 0)[1]) / H * 100:.1f}%）")
o.append(f"通った跡の点と本当の位置のずれの最大：車いすの後ろの車軸の真ん中 {chk['trail_err_m']['chair'] * 100:.3f} cm・ハコブくんの根 {chk['trail_err_m']['robot'] * 100:.3f} cm")
for (who, t, head), fv in zip(MARKS, chk["mark_first"]):
    o.append(f"印 {who} {head}：台本 {t:.4f} 秒（動画 {t - C['video_from']:.3f} 秒）→ 出たコマの動画の秒 {fv:.3f}")
o.append(f"左上の字：壊れた値のコマ {chk['hud_bad']}・車いすの横の位置が「—」のコマ {chk['hud_dash'][1]}・すき間が「—」のコマ {chk['hud_dash'][2]}")
o += ["  " + s for s in chk["hud_texts"]]
# 画面の中か（2＝全部・1＝一部・0＝外）
def spans(key):
    out, cur = [], None
    for f in range(f0, f1 + 1):
        v = int(top[f][key])
        if cur is None or cur[0] != v: cur = [v, vt(f), vt(f)]; out.append(cur)
        else: cur[2] = vt(f)
    return " ／ ".join(f"{['外', '一部', '全部'][v]} {a:.3f}〜{b:.3f}" for v, a, b in out)
for key, nm in (("chair_in", "車いす"), ("robot_in", "ハコブくん"), ("nurse_in", "大城さん")):
    o.append(f"{nm}が画面の中か：{spans(key)}")
a, b = int(round((t_sh0 - 1.0) * FPS)), int(round(t_sh1 * FPS))
o.append(f"寄せ始めの 1 秒前〜寄せ終わり（動画 {vt(a):.3f}〜{vt(b):.3f}）の車いす：全部が画面の中でないコマ {sum(1 for f in range(a, b + 1) if int(top[f]['chair_in']) != 2)}")
open(os.path.join(D, "check230_top.txt"), "w", encoding="utf-8").write("\n".join(o) + "\n")
print("\n".join(o)); print("動画", vid)
