src/poolcoach_cv/camera.py

"""Camera từ homography + bù độ cao tâm bi — BG31 (13/08/2026).

Production hoá kết luận BG30 (bb9_diag30_offset_fit.csv, commit f763db7):
track/detections là ảnh của TÂM BI (cao R trên vải) chiếu qua homography
MẶT VẢI z=0, nên vị trí map lệch khỏi chân tâm bi một vec-tơ hướng RA XA
chân camera: X' = P + k·(P − C_xy), k = R/(h − R). Mô hình KHỚP trên nhóm
offset ≥ 15mm (dư 2.9mm, 10 cú pilot; camera decompose chụm h = 2.410
± 0.021 m, f ≈ 2236 px).

- ``decompose_homography`` — port NGUYÊN thuật toán ``decompose_h`` từ
  ``scripts/broadcast/diag_cushion_offset.py`` (BG30 bước 2): ước camera
  từ MỘT homography ảnh→bàn, tự ước f từ ràng buộc trực giao r1⊥r2 và
  ‖r1‖=‖r2‖, principal point danh định giữa ảnh.
- ``HeightCompensation`` — bù bằng nghiệm ngược ĐÓNG của mô hình trên:
  P = (X' + k·C_xy)/(1 + k) — chính công thức đã kiểm offline ở
  ``scripts/broadcast/diag_height_compensation.py`` (BG30 bước 3).
  Camera lấy từ decompose homography CỦA CHÍNH CLIP, không dùng hằng
  chung; decompose thất bại / camera vô lý → KHÔNG bù (identity) + meta
  ``on=False`` — thà không bù còn hơn bù bằng camera rác (BRIEF 31).

Thuần numpy — venv app import được (nếp poolcoach_cv, unit test không cần
cv2/torch).
"""

from __future__ import annotations

import math

import numpy as np

# Biên "camera vô lý" cho fallback — SIẾT ở BG32 (13/08/2026): biên rộng
# BG31 (h 0.5–20m, f 200–20000px) đã cho lọt ca rác cú 11 smoke lần 1
# (h=0.686m, f=12767px) → bù bằng camera rác. Căn cứ biên mới: 19 decompose
# lành trải h 2.33–2.51m / f 2155–3012px — chừa dư ~2 lần mỗi phía để không
# chặn oan camera broadcast lạ, nhưng ca rác trên phải bị chặn.
CAM_H_MIN_M = 1.5
CAM_H_MAX_M = 6.0
CAM_F_MIN_PX = 800.0
CAM_F_MAX_PX = 8000.0


def decompose_homography(H: np.ndarray,
                         pp: tuple[float, float] = (960.0, 540.0)
                         ) -> dict | None:
    """Ước camera từ MỘT homography ảnh→bàn, không cần offset field.

    G = H⁻¹ (bàn→ảnh) ≅ K[r1 r2 t]; tự ước f từ ràng buộc r1⊥r2 và
    ‖r1‖=‖r2‖ (principal point danh định ``pp`` — giữa ảnh). Trả
    ``{f_px, cx, cy, h}`` hệ bàn CỦA CÚ, hoặc None nếu nghiệm f không
    dương. Port nguyên từ ``diag_cushion_offset.decompose_h`` (BG30).
    """
    G = np.linalg.inv(np.asarray(H, dtype=np.float64))
    G = G / G[2, 2]
    a, b = G[:, 0].copy(), G[:, 1].copy()
    a[0] -= pp[0] * a[2]
    a[1] -= pp[1] * a[2]
    b[0] -= pp[0] * b[2]
    b[1] -= pp[1] * b[2]
    # sau khi khử principal point: m_i = (u_i/f, v_i/f, w_i) — hai ràng buộc
    # trên f²: u1u2 + v1v2 + f²w1w2 = 0 ; (u1²+v1²) − (u2²+v2²) = f²(w2²−w1²)
    f2 = []
    den = a[2] * b[2]
    if abs(den) > 1e-15:
        f2.append(-(a[0] * b[0] + a[1] * b[1]) / den)
    den = b[2] ** 2 - a[2] ** 2
    if abs(den) > 1e-15:
        f2.append(((a[0] ** 2 + a[1] ** 2) - (b[0] ** 2 + b[1] ** 2)) / den)
    f2 = [v for v in f2 if v > 1e3]
    if not f2:
        return None
    f = math.sqrt(float(np.mean(f2)))
    Kinv = np.array([[1 / f, 0, -pp[0] / f], [0, 1 / f, -pp[1] / f],
                     [0, 0, 1.0]])
    M = Kinv @ G
    lam = 2.0 / (np.linalg.norm(M[:, 0]) + np.linalg.norm(M[:, 1]))
    r1, r2, tvec = lam * M[:, 0], lam * M[:, 1], lam * M[:, 2]
    r3 = np.cross(r1, r2)
    Rm = np.stack([r1, r2, r3], axis=1)
    cam = -Rm.T @ tvec  # X_cam = Rm·X_table + t ⇒ C_table = −Rmᵀ t
    if cam[2] < 0:      # nghiệm λ âm — lật dấu (camera phải ở TRÊN mặt bàn)
        cam = -cam
    return {"f_px": round(f, 1), "cx": float(cam[0]), "cy": float(cam[1]),
            "h": float(cam[2])}


class HeightCompensation:
    """Bù độ cao tâm bi cho toạ độ đã map qua homography mặt vải.

    ``on=True``: ``apply`` giải ngược P = (X' + k·C_xy)/(1 + k) với
    k = R/(h − R) — camera (C_xy, h) từ decompose homography của chính
    clip. ``on=False`` (fallback): ``apply`` là identity, ``meta()`` mang
    cờ ``on: False`` (+ ``reason``) để JSON job khai rõ track sinh ra
    dưới quy ước CHƯA bù (BRIEF 31 bước 1.4 — không áp hồi tố, hai thời
    kỳ phân biệt bằng chính cờ này).
    """

    def __init__(self, on: bool, cam_xy=None, h_m: float | None = None,
                 f_px: float | None = None, reason: str | None = None,
                 ball_r: float = 0.0) -> None:
        self.on = bool(on)
        self.cam_xy = (None if cam_xy is None
                       else np.asarray(cam_xy, dtype=np.float64))
        self.h_m = h_m
        self.f_px = f_px
        self.reason = reason
        self._k = (ball_r / (h_m - ball_r)) if self.on else 0.0

    @classmethod
    def from_homography(cls, H: np.ndarray, image_wh: tuple[float, float],
                        ball_r: float) -> "HeightCompensation":
        """Dựng bộ bù từ homography ảnh→bàn CỦA CHÍNH CLIP.

        ``image_wh`` = (rộng, cao) pixel của frame mà corners được chấm —
        principal point danh định giữa ảnh (quy ước decompose BG30).
        Mọi đường thất bại đều trả bộ bù ``on=False`` kèm ``reason``,
        không ném — bù là tầng phụ, không được giết analyze.
        """
        w_px, h_px = float(image_wh[0]), float(image_wh[1])
        if not (w_px > 0 and h_px > 0):
            return cls(False, reason="không biết kích thước frame")
        try:
            dec = decompose_homography(np.asarray(H, dtype=np.float64),
                                       pp=(w_px / 2.0, h_px / 2.0))
        except np.linalg.LinAlgError:
            return cls(False, reason="homography suy biến (không nghịch đảo "
                                     "được)")
        if dec is None:
            return cls(False, reason="decompose thất bại (nghiệm f² không "
                                     "dương)")
        h_m, f_px = float(dec["h"]), float(dec["f_px"])
        cx, cy = float(dec["cx"]), float(dec["cy"])
        if not all(math.isfinite(v) for v in (h_m, f_px, cx, cy)):
            return cls(False, reason="decompose ra giá trị không hữu hạn")
        if not (CAM_H_MIN_M <= h_m <= CAM_H_MAX_M):
            return cls(False, cam_xy=(cx, cy), h_m=round(h_m, 3),
                       f_px=round(f_px, 1),
                       reason=f"h = {h_m:.2f} m ngoài "
                              f"[{CAM_H_MIN_M:g}, {CAM_H_MAX_M:g}] m")
        if not (CAM_F_MIN_PX <= f_px <= CAM_F_MAX_PX):
            return cls(False, cam_xy=(cx, cy), h_m=round(h_m, 3),
                       f_px=round(f_px, 1),
                       reason=f"f = {f_px:.0f} px ngoài "
                              f"[{CAM_F_MIN_PX:g}, {CAM_F_MAX_PX:g}] px")
        return cls(True, cam_xy=(cx, cy), h_m=h_m, f_px=f_px, ball_r=ball_r)

    def apply(self, pts: np.ndarray) -> np.ndarray:
        """X' (toạ độ map mặt vải, (2,) hay (N,2)) → chân tâm bi thật P."""
        pts = np.asarray(pts, dtype=np.float64)
        if not self.on:
            return pts
        return (pts + self._k * self.cam_xy) / (1.0 + self._k)

    def meta(self) -> dict:
        """Khối ``height_comp`` cho JSON job (BRIEF 31 bước 1.4).

        BG32 thêm ``cx_m``/``cy_m`` (chân camera trên hệ bàn) — optional
        như ``h_m``/``f_px``, có mặt cả khi fallback vì camera ngoài biên
        (để truy vết decompose rác + tích luỹ ca cho câu hỏi apex).
        """
        out: dict = {"on": self.on}
        if self.h_m is not None:
            out["h_m"] = round(float(self.h_m), 3)
        if self.f_px is not None:
            out["f_px"] = round(float(self.f_px), 1)
        if self.cam_xy is not None:
            out["cx_m"] = round(float(self.cam_xy[0]), 3)
            out["cy_m"] = round(float(self.cam_xy[1]), 3)
        if self.reason is not None:
            out["reason"] = self.reason
        return out