#!/usr/bin/env python3
"""依頼140：寄りのカメラごとに、比べるための数を出す（見た目の好みではない）。

  python3 vis140.py <回のフォルダ> <出力.csv>

隠れの判定：カメラの位置から点までの線分が、動かない物の箱（look_static.csv）のうち「部屋の箱・壁」
（group rooms の全部・waiting の WallBack*/WallWest・world の高さ 0.5 m 超えのもの）を通るかで決める。
長いす・カウンター・座った人・人・ロボットは隠すものに入れない。画面の外（0≤u≤w・0≤v≤h の外）も「写っていない」に数える。
  1 見える廊下の割合：x9〜31・z27〜31 の大廊下の床（床の上の面＝高さ 0）を 0.1 m のマスに切り、マスの中心が写っている割合
  2 見える輪の数：静止画の時刻（8862 秒）の前 10 秒（8852〜8862 秒）に出ている輪（events.csv）の中心（床の上）が写っている数／全部
  3 ロボットが見える割合：8845〜8945 秒の 0.2 秒ごとのコマで、白い円柱の上の面の中心が写っているコマの割合
     （上の面の 8 つの縁の点も数え、すべて写っているコマの割合も出す。画面の中にいたコマ数と、画面の中で箱に隠れたコマ数も出す）
  4 人の見分け：範囲の真ん中 (20, 29) に立つカプセル（体：直径 0.4・高さ 1.59）が、パネル付きの絵（左 1360×1080）で何画素の高さか
     （カプセルの表面の点を画面に移したときの縦の幅）。あわせて足元から頭の上（1.59 m）の縦の線の長さ（画素）
"""
import csv, json, math, os, sys
import numpy as np

FLOOR_Y = 0.0   # 床の上の面（look_static.csv の Floor01〜04 の max_y）
ROBOT_H = 1.081
ROBOT_D = 0.549
STILL_T, SHOW_S = 8862.0, 10.0
CX, CZ = 20.0, 29.0
SW, SH = 1360, 1080


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 = np.array(j["worldToCameraMatrix"]), np.array(j["projectionMatrix"])
        self.pos = np.array(j["position_unity"])
        self.spec = j["h139Cam"].split(",")

    def uv(self, pts):
        pts = np.atleast_2d(pts)
        P4 = np.c_[pts, np.ones(len(pts))]
        q = (self.P @ (self.V @ P4.T)).T
        w = q[:, 3]
        u = (q[:, 0] / w + 1) / 2 * self.w
        v = (1 - q[:, 1] / w) / 2 * self.h
        return u, v, w

    def inframe(self, pts):
        u, v, w = self.uv(pts)
        return (w > 1e-6) & (u >= 0) & (u <= self.w) & (v >= 0) & (v <= self.h)


def load_boxes(run):
    path = os.path.join(run, "look139", "look_static.csv")
    lines = open(path, encoding="utf-8").read().splitlines()
    head = dict(kv.split("=") for kv in lines[0][2:].split())
    boxes, kinds = [], {"rooms": 0, "waiting_wall": 0, "world": 0}
    for r in csv.DictReader(lines[1:]):
        mn = [float(r["min_x"]), float(r["min_y"]), float(r["min_z"])]
        mx = [float(r["max_x"]), float(r["max_y"]), float(r["max_z"])]
        name = r["path"].split("/")[-1]
        g = r["group"]
        if g == "rooms":
            k = "rooms"
        elif g == "waiting" and name.startswith(("WallBack", "WallWest")):
            k = "waiting_wall"
        elif g == "world" and mx[1] - mn[1] > 0.5:
            k = "world"
        else:
            continue
        if r["axis_aligned"] != "1":
            raise SystemExit("軸にそろっていない箱があります: " + r["path"])
        boxes.append(mn + mx)
        kinds[k] += 1
    return np.array(boxes), kinds, float(head["robot_base_y"]), float(head["walker_base_y"])


def visible(cam, boxes, pts):
    """pts (N,3) の点がカメラから見えるか（画面の中・箱に隠れていない）"""
    pts = np.atleast_2d(pts).astype(float)
    ok = cam.inframe(pts)
    o = cam.pos
    d = pts - o                                   # (N,3)
    with np.errstate(divide="ignore", invalid="ignore"):
        inv = 1.0 / d
    bmn, bmx = boxes[:, :3], boxes[:, 3:]
    hid = np.zeros(len(pts), bool)
    for i in range(0, len(pts), 2000):
        dd = inv[i:i + 2000, None, :]             # (n,1,3)
        t1 = (bmn[None] - o) * dd
        t2 = (bmx[None] - o) * dd
        t1 = np.where(np.isnan(t1), -np.inf, t1); t2 = np.where(np.isnan(t2), np.inf, t2)
        tmin = np.max(np.minimum(t1, t2), axis=2)
        tmax = np.min(np.maximum(t1, t2), axis=2)
        hit = (tmax >= tmin) & (tmax > 1e-6) & (tmin < 1 - 1e-6)
        hid[i:i + 2000] = hit.any(axis=1)
    return ok & ~hid


def capsule_px(cam, base_y):
    pts = []
    r, h = 0.2, 1.59
    for a in np.linspace(0, 2 * math.pi, 48, endpoint=False):
        for y in np.linspace(r, h - r, 12):
            pts.append((CX + r * math.cos(a), base_y + y, CZ + r * math.sin(a)))
        for b in np.linspace(0, math.pi / 2, 8):
            pts.append((CX + r * math.cos(b) * math.cos(a), base_y + h - r + r * math.sin(b), CZ + r * math.cos(b) * math.sin(a)))
            pts.append((CX + r * math.cos(b) * math.cos(a), base_y + r - r * math.sin(b), CZ + r * math.cos(b) * math.sin(a)))
    u, v, _ = cam.uv(np.array(pts))
    sy = SH / cam.h
    ext = (v.max() - v.min()) * sy
    _, va, _ = cam.uv(np.array([(CX, base_y, CZ), (CX, base_y + h, CZ)]))
    return ext, abs(va[0] - va[1]) * sy


def main():
    run, out = sys.argv[1], sys.argv[2]
    boxes, kinds, rby, wby = load_boxes(run)
    ev = [e for e in csv.DictReader(open(os.path.join(run, "events.csv"), encoding="utf-8"))
          if STILL_T - SHOW_S - 1e-6 <= float(e["t_s"]) <= STILL_T + 1e-6]
    frames = list(csv.DictReader(open(os.path.join(run, "look139", "look_frames.csv"), encoding="utf-8")))
    names = sorted({r["cam"] for r in frames if r["cam"].startswith("n_")})
    xs = np.arange(9.05, 31.0, 0.1); zs = np.arange(27.05, 31.0, 0.1)
    gx, gz = np.meshgrid(xs, zs)
    floor = np.c_[gx.ravel(), np.full(gx.size, FLOOR_Y), gz.ravel()]
    rows = []
    for n in names:
        cam = Cam(os.path.join(run, "look139", "cam_%s.json" % n))
        a = visible(cam, boxes, floor)
        fr_in = cam.inframe(floor)
        ring_pts = np.array([(float(e["x"]), FLOOR_Y, float(e["z"])) for e in ev])
        rv = visible(cam, boxes, ring_pts)
        vf = [r for r in frames if r["kind"] == "vid" and r["cam"] == n]
        top = rby + ROBOT_H
        c_pts = np.array([(float(r["robot_x"]), top, float(r["robot_z"])) for r in vf])
        c_vis = visible(cam, boxes, c_pts)
        c_in = cam.inframe(c_pts)
        rim_all = np.ones(len(vf), bool)
        for k in range(8):
            ang = k * math.pi / 4
            p = c_pts + np.array([ROBOT_D / 2 * math.cos(ang), 0, ROBOT_D / 2 * math.sin(ang)])
            rim_all &= visible(cam, boxes, p)
        ext, axis = capsule_px(cam, wby)
        yaw, pitch, tx, tz, dist = (float(v) for v in cam.spec[1:6])
        rows.append({
            "cam": n, "yaw": yaw, "pitch": pitch, "tx": tx, "tz": tz, "dist": dist, "cam_h": round(float(cam.pos[1]), 2),
            "cam_x": round(float(cam.pos[0]), 2), "cam_z": round(float(cam.pos[2]), 2),
            "hall_visible_pct": round(100 * float(a.mean()), 1), "hall_inframe_pct": round(100 * float(fr_in.mean()), 1),
            "rings_visible": int(rv.sum()), "rings_total": len(ev),
            "rings_hidden_ids": " ".join(e["event_id"] for e, v in zip(ev, rv) if not v),
            "robot_top_center_visible_pct": round(100 * float(c_vis.mean()), 1), "robot_top_all_visible_pct": round(100 * float(rim_all.mean()), 1),
            "robot_frames": len(vf), "robot_inframe_frames": int(c_in.sum()), "robot_hidden_inframe_frames": int((c_in & ~c_vis).sum()), "person_px": round(float(ext), 1), "person_axis_px": round(float(axis), 1),
        })
    with open(out, "w", encoding="utf-8", newline="") as f:
        w = csv.DictWriter(f, fieldnames=list(rows[0].keys()))
        w.writeheader(); w.writerows(rows)
    print("隠すもの:", kinds, "robot_base_y", rby, "walker_base_y", wby, "輪", len(ev))
    for r in rows:
        print(r)


if __name__ == "__main__":
    main()
