tests/test_height_comp.py

"""Unit hình học + fallback bù độ cao tâm bi — BG31 bước 1.5 (13/08/2026).

Camera GIẢ dựng tường minh (K với f/pp biết trước, pose nhìn xuống bàn,
độ cao h biết trước) → chiếu 4 góc bàn (z=0) ra pixel → TableHomography
(ĐÚNG code production) → chiếu TÂM BI (z=R) ra pixel → map + bù phải khôi
phục đúng chân tâm bi trên mặt vải, sai số < 1mm (BRIEF 31 bước 1.5).
Đối chứng không-bù phải lệch RÕ (mm hai chữ số ở xa camera) — test không
được rỗng nghĩa.

BG32 (13/08/2026) siết biên sanity (h ∈ [1.5, 6]m, f ∈ [800, 8000]px) và
thêm ``cx_m``/``cy_m`` vào cờ — thêm test tái tạo ĐÚNG ca rác cú 11 smoke
lần 1 (h=0.686/f=12767 từng lọt biên rộng BG31) phải ra off.

Thuần numpy — không cần cv2/torch (camera.py giữ nếp poolcoach_cv).
"""

from __future__ import annotations

import sys
from pathlib import Path

import numpy as np
import pytest

from poolcoach_cv.camera import (CAM_F_MAX_PX, CAM_F_MIN_PX, CAM_H_MAX_M,
                                 CAM_H_MIN_M, HeightCompensation,
                                 decompose_homography)
from poolcoach_cv.homography import TableHomography

# hệ bàn broadcast (poolcoach_cv.broadcast — không import cả module cho gọn)
W, L = 1.27, 2.54
BALL_R = 0.028575
IMG_WH = (1920.0, 1080.0)


def make_projector(cam_xy=(0.6, -2.5), h=2.4, f=2200.0, wh=IMG_WH):
    """Camera pinhole giả: trả hàm chiếu điểm 3D hệ bàn → pixel.

    Pose nhìn vào tâm bàn (đúng dáng camera broadcast: đặt ngoài băng ngắn,
    cao h trên mặt vải). K không skew, pixel vuông, pp giữa ảnh — đúng các
    giả định của decompose BG30.
    """
    c = np.array([cam_xy[0], cam_xy[1], h], dtype=np.float64)
    z = np.array([W / 2, L / 2, 0.0]) - c
    z /= np.linalg.norm(z)
    x = np.cross(z, np.array([0.0, 0.0, 1.0]))
    x /= np.linalg.norm(x)
    y = np.cross(z, x)
    rot = np.stack([x, y, z])            # hàng = trục camera trong hệ bàn

    def project(pts3):
        p = (np.atleast_2d(np.asarray(pts3, dtype=np.float64)) - c) @ rot.T
        return np.stack([f * p[:, 0] / p[:, 2] + wh[0] / 2,
                         f * p[:, 1] / p[:, 2] + wh[1] / 2], axis=1)

    return project


def table_h(project) -> TableHomography:
    corners_px = project([[0, 0, 0], [W, 0, 0], [W, L, 0], [0, L, 0]])
    return TableHomography(corners_px, table_w=W, table_l=L)


def ball_grid():
    xs = np.linspace(0.05, W - 0.05, 5)
    ys = np.linspace(0.05, L - 0.05, 7)
    return np.array([[x, y, BALL_R] for x in xs for y in ys])


# ------------------------------------------------------------- decompose

def test_decompose_khoi_phuc_dung_camera_gia():
    """f/h/C biết trước → decompose phải trả lại gần đúng (toán đóng,
    không nhiễu — sai lệch chỉ còn số học)."""
    project = make_projector(cam_xy=(0.6, -2.5), h=2.4, f=2200.0)
    th = table_h(project)
    dec = decompose_homography(th.H, pp=(IMG_WH[0] / 2, IMG_WH[1] / 2))
    assert dec is not None
    assert abs(dec["h"] - 2.4) < 1e-3
    assert abs(dec["f_px"] - 2200.0) < 0.5
    assert abs(dec["cx"] - 0.6) < 1e-3
    assert abs(dec["cy"] - (-2.5)) < 1e-3


@pytest.mark.parametrize("cam_xy,h,f", [
    ((0.6, -2.5), 2.4, 2200.0),      # dáng BG30 (hệ cú, camera phía y<0)
    ((0.635, 5.05), 2.41, 2236.0),   # đúng số decompose pilot (hệ chuẩn)
    ((-0.8, 3.6), 1.6, 1400.0),      # camera lệch trái, thấp hơn
])
def test_map_cong_bu_khoi_phuc_duoi_1mm(cam_xy, h, f):
    """Gate BRIEF 31 bước 1.5: điểm 3D biết trước, chiếu qua camera giả →
    map + bù khôi phục đúng vị trí mặt vải < 1mm; không bù phải lệch rõ."""
    project = make_projector(cam_xy=cam_xy, h=h, f=f)
    th = table_h(project)
    comp = HeightCompensation.from_homography(th.H, IMG_WH, BALL_R)
    assert comp.on, comp.reason

    balls = ball_grid()
    mapped = th.px_to_table(project(balls))          # quy ước CŨ (chưa bù)
    err_raw = np.linalg.norm(mapped - balls[:, :2], axis=1)
    err_comp = np.linalg.norm(comp.apply(mapped) - balls[:, :2], axis=1)
    # không bù: lệch cỡ k·(khoảng cách tới chân camera) — phải thấy rõ
    assert err_raw.max() > 0.005
    assert err_comp.max() < 0.001


def test_meta_mang_dung_h_f_cx_cy():
    project = make_projector(cam_xy=(0.6, -2.5), h=2.4, f=2200.0)
    comp = HeightCompensation.from_homography(table_h(project).H, IMG_WH,
                                              BALL_R)
    m = comp.meta()
    assert m["on"] is True
    assert abs(m["h_m"] - 2.4) < 1e-3
    assert abs(m["f_px"] - 2200.0) < 0.5
    # BG32: chân camera trên hệ bàn — optional, cùng nếp h_m/f_px
    assert abs(m["cx_m"] - 0.6) < 1e-3
    assert abs(m["cy_m"] - (-2.5)) < 1e-3
    assert "reason" not in m


# ------------------------------------------------------------- fallback

def test_homography_affine_khong_bu_apply_identity():
    """Fronto-parallel (nhìn thẳng từ trên): không có tín hiệu phối cảnh
    để ước f → decompose None → KHÔNG bù, apply là identity."""
    corners_px = np.array([[500.0, 300.0], [627.0, 300.0],
                           [627.0, 554.0], [500.0, 554.0]])
    th = TableHomography(corners_px, table_w=W, table_l=L)
    assert decompose_homography(th.H, pp=(960.0, 540.0)) is None
    comp = HeightCompensation.from_homography(th.H, IMG_WH, BALL_R)
    assert not comp.on
    assert "f²" in comp.reason or "f2" in comp.reason
    pts = np.array([[0.3, 0.4], [1.0, 2.0]])
    assert np.array_equal(comp.apply(pts), pts)
    m = comp.meta()
    assert m["on"] is False and "reason" in m


def test_camera_vo_ly_khong_bu_khai_ly_do():
    """Decompose chạy được nhưng h ngoài biên sanity → không bù, meta vẫn
    khai h/f (BG32: cả cx/cy) đo được để truy vết."""
    project = make_projector(cam_xy=(0.6, -20.0), h=CAM_H_MAX_M * 2, f=7000.0)
    comp = HeightCompensation.from_homography(table_h(project).H, IMG_WH,
                                              BALL_R)
    assert not comp.on
    assert "h =" in comp.reason
    m = comp.meta()
    assert m["on"] is False and m["h_m"] > CAM_H_MAX_M
    assert abs(m["cx_m"] - 0.6) < 1e-3
    assert abs(m["cy_m"] - (-20.0)) < 1e-2


def test_bien_bg32_chan_ca_rac_cu11_lan1():
    """Tái tạo ĐÚNG ca rác đã lọt biên rộng BG31 (cú 11 smoke lần 1:
    decompose h=0.686m, f=12767px → bù bằng camera rác). Biên BG32
    [1.5, 6]m phải chặn: off + reason h + meta khai đủ h/f/cx/cy để truy
    vết, apply là identity (thà không bù còn hơn bù bằng camera rác)."""
    project = make_projector(cam_xy=(0.6, -2.5), h=0.686, f=12766.8)
    comp = HeightCompensation.from_homography(table_h(project).H, IMG_WH,
                                              BALL_R)
    assert not comp.on
    assert "h =" in comp.reason
    m = comp.meta()
    assert m["on"] is False
    assert abs(m["h_m"] - 0.686) < 1e-3 and m["h_m"] < CAM_H_MIN_M
    assert abs(m["f_px"] - 12766.8) < 0.5
    assert abs(m["cx_m"] - 0.6) < 1e-3
    assert abs(m["cy_m"] - (-2.5)) < 1e-3
    pts = np.array([[0.3, 0.4], [1.0, 2.0]])
    assert np.array_equal(comp.apply(pts), pts)


@pytest.mark.parametrize("f", [700.0, 9000.0])
def test_f_ngoai_bien_khong_bu_du_h_lanh(f):
    """Biên BG32 là h VÀ f: h trong [1.5, 6]m nhưng f ngoài [800, 8000]px
    (hai phía) vẫn phải off, reason nói về f."""
    project = make_projector(cam_xy=(0.6, -2.5), h=2.4, f=f)
    comp = HeightCompensation.from_homography(table_h(project).H, IMG_WH,
                                              BALL_R)
    assert not comp.on
    assert "f =" in comp.reason
    m = comp.meta()
    assert m["on"] is False
    assert abs(m["h_m"] - 2.4) < 1e-3
    assert not (CAM_F_MIN_PX <= m["f_px"] <= CAM_F_MAX_PX)
    assert abs(m["cx_m"] - 0.6) < 1e-3       # nhánh f cũng khai chân camera
    assert abs(m["cy_m"] - (-2.5)) < 1e-3


def test_thieu_kich_thuoc_frame_khong_bu():
    project = make_projector()
    comp = HeightCompensation.from_homography(table_h(project).H, (0, 0),
                                              BALL_R)
    assert not comp.on
    assert "frame" in comp.reason


# ------------------------------------- adapter phía worker (bước 1.2)

def _cv_worker():
    sys.path.insert(0, str(Path(__file__).resolve().parents[1] / "scripts"))
    import cv_worker

    return cv_worker


def test_comp_homography_cls_ap_bu_va_ghi_meta():
    """Subclass patch vào bc.TableHomography: px_to_table trả toạ độ ĐÃ bù
    (<1mm), sink nhận meta height_comp — đúng những gì analyze_once ghi
    vào JSON job."""
    cw = _cv_worker()
    project = make_projector(cam_xy=(0.6, -2.5), h=2.4, f=2200.0)
    corners_px = project([[0, 0, 0], [W, 0, 0], [W, L, 0], [0, L, 0]])

    sink: dict = {}
    cls = cw.comp_homography_cls(IMG_WH, BALL_R, sink)
    th = cls(corners_px, table_w=W, table_l=L)
    assert sink["on"] is True
    assert abs(sink["h_m"] - 2.4) < 1e-3
    assert abs(sink["cx_m"] - 0.6) < 1e-3      # BG32: cx/cy vào JSON job
    assert abs(sink["cy_m"] - (-2.5)) < 1e-3

    balls = ball_grid()
    got = np.array([th.px_to_table(px) for px in project(balls)])
    err = np.linalg.norm(got - balls[:, :2], axis=1)
    assert err.max() < 0.001


def test_comp_homography_cls_fallback_van_map_nhu_cu():
    """Decompose fail → subclass map Y HỆT TableHomography gốc (không bù),
    sink khai on=False — thà không bù còn hơn bù bằng camera rác."""
    cw = _cv_worker()
    corners_px = np.array([[500.0, 300.0], [627.0, 300.0],
                           [627.0, 554.0], [500.0, 554.0]])
    sink: dict = {}
    cls = cw.comp_homography_cls(IMG_WH, BALL_R, sink)
    th = cls(corners_px, table_w=W, table_l=L)
    assert sink["on"] is False and "reason" in sink
    base = TableHomography(corners_px, table_w=W, table_l=L)
    px = np.array([560.0, 400.0])
    assert np.allclose(th.px_to_table(px), base.px_to_table(px))