"""依頼110 F-3: 上の端が切れている日に, 本線のホモグラフィ (上の2点を (±310, −89) とする) で測った位置が
どれだけずれるかを計算する (読むだけ).

正しい対応: 上の2点は「マットの左右の縁 x=±310 の, 方眼で数えた z (f_days.csv の image_top_z_mm)」.
  1920×1080 のコマがない日 (384×216 だけの日) は, 下の辺の幅と高さの比から z を推し量る
  (1920×1080 の切れている日で係数を合わせる. 粗い見積もり).
  左右の上の点の z は同じとした (依頼107 D のカメラ合わせでは左右で約 4mm 違った).
本線の対応: 上の2点を (±310, −89).
ずれ = 本線で出る位置 − 本当の位置 (x, z mm. z は + が手前 = 根元から遠い向き).
出力: f_shift.csv (日 × 点) / f_ztop.csv (日ごとの上の端の z)
"""
import csv
from pathlib import Path

import cv2
import numpy as np

OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所/F_上カメラの上の端")
MAIN_TOP_Z = -89.0
POINTS = [  # 名前, x, z (本線のマット座標 mm. Unity の x・z と同じ向き)
    ("教えた位置5 (T5)", -70.0, 253.0),
    ("π0.5 のブロックのあたり (依頼107 B)", -55.0, 248.0),
    ("白箱の中心 (依頼107 B)", -204.0, 236.0),
    ("教えた位置1 (T1)", 161.6, 212.8),
    ("マットの中央", 0.0, 136.0),
    ("手前の中央", 0.0, 330.0),
    ("手前の左", 250.0, 330.0),
    ("手前の右", -250.0, 330.0),
    ("奥の中央", 0.0, 0.0),
]


def H_of(c, ztop_l, ztop_r):
    src = np.array(c, np.float32)
    dst = np.array([[310, ztop_l], [-310, ztop_r], [-310, 361], [310, 361]], np.float32)
    return cv2.getPerspectiveTransform(src, dst)


def apply(H, p):
    return cv2.perspectiveTransform(np.array([[p]], np.float32), H)[0][0]


def main():
    days = [r for r in csv.DictReader(open(OUT / "f_days.csv")) if r.get("status") == "切れている"]
    # 1920×1080 の日で, 高さ/幅 の比から z を出す係数を合わせる
    def ratio_est(r):
        w = float(r["LR_u"]) - float(r["LL_u"]); h = float(r["LL_v"]) - 0.0
        return h * 620.0 / w
    fit = [(ratio_est(r), 361 - float(r["image_top_z_mm"])) for r in days if r.get("image_top_z_mm")]
    fit = [(a, b) for a, b in fit if b < 440]          # 7月のわずかに切れた日 (上の端がほぼマットの縁) は外す
    k = float(np.mean([b / a for a, b in fit]))
    resid = [b - k * a for a, b in fit]
    zrows, srows = [], []
    for r in days:
        c = [(float(r[f"{n}_u"]), float(r[f"{n}_v"])) for n in ("UL", "UR", "LR", "LL")]
        if r.get("image_top_z_mm"):
            zt, how = float(r["image_top_z_mm"]), "方眼を数えた (1920×1080)"
        else:
            zt, how = 361 - k * ratio_est(r), f"幅と高さの比から推し量った (係数 {k:.3f}・合わせた{len(fit)}日の残り最大 {max(abs(x) for x in resid):.1f}mm)"
        note = ""
        if r.get("other_cut"):
            note = f"上のほかに {r['other_cut']} も画像の端で切れている (下の2隅も本当の角ではない)"
        zrows.append({"source": r["source"], "day": r["day"], "kind": r["kind"], "image_top_z_mm": round(zt, 1),
                      "top_shift_vs_main_mm": round(zt - MAIN_TOP_Z, 1), "how": how, "note": note})
        if note:
            continue
        Hm = H_of(c, MAIN_TOP_Z, MAIN_TOP_Z)
        Ht = H_of(c, zt, zt)
        Ht_inv = np.linalg.inv(Ht)
        for name, x, z in POINTS:
            px = apply(Ht_inv, (x, z))
            xm, zm = apply(Hm, px)
            srows.append({"source": r["source"], "day": r["day"], "image_top_z_mm": round(zt, 1), "point": name,
                          "true_x_mm": x, "true_z_mm": z, "main_x_mm": round(float(xm), 1), "main_z_mm": round(float(zm), 1),
                          "dx_mm": round(float(xm - x), 1), "dz_mm": round(float(zm - z), 1),
                          "dist_mm": round(float(np.hypot(xm - x, zm - z)), 1), "pixel_u": round(float(px[0]), 1),
                          "pixel_v": round(float(px[1]), 1), "in_image": bool(px[1] >= 0)})
    for name, rows in (("f_ztop.csv", zrows), ("f_shift.csv", srows)):
        keys = []
        for r in rows:
            keys += [kk for kk in r if kk not in keys]
        with open(OUT / name, "w", newline="") as f:
            w = csv.DictWriter(f, fieldnames=keys); w.writeheader(); w.writerows(rows)
    print("k", round(k, 4), "fit days", len(fit), "resid", [round(x, 1) for x in resid])
    for r in zrows:
        print(r)
    for r in srows:
        if r["point"] in ("教えた位置5 (T5)", "マットの中央", "手前の中央", "奥の中央"):
            print(r["source"][:8], r["day"], r["image_top_z_mm"], r["point"], r["dx_mm"], r["dz_mm"])


if __name__ == "__main__":
    main()