"""YuNet face gates for Rally follow — reject ceiling/stand/noise."""
from __future__ import annotations

import numpy as np

from rally_follow import FaceGate, accept_yunet_face, select_yunet_face


def _face(
    *,
    x=400.0,
    y=200.0,
    w=80.0,
    h=100.0,
    score=0.93,
    re=None,
    le=None,
    nose=None,
    rm=None,
    lm=None,
) -> np.ndarray:
    """YuNet 15-vector: box + 5 landmarks + score, in 960x540 detect space."""
    if re is None:
        re = (x + 0.30 * w, y + 0.38 * h)
    if le is None:
        le = (x + 0.70 * w, y + 0.38 * h)
    if nose is None:
        nose = (x + 0.50 * w, y + 0.55 * h)
    if rm is None:
        rm = (x + 0.35 * w, y + 0.78 * h)
    if lm is None:
        lm = (x + 0.65 * w, y + 0.78 * h)
    row = np.zeros(15, dtype=np.float32)
    row[0:4] = (x, y, w, h)
    row[4:6] = re
    row[6:8] = le
    row[8:10] = nose
    row[10:12] = rm
    row[12:14] = lm
    row[14] = score
    return row


FW, FH = 960, 540


def test_good_face_accepted():
    assert accept_yunet_face(_face(), FW, FH) is True


def test_tiny_blob_rejected():
    # Live FP: 25x34 @ 0.805, nose not between the eyes.
    row = _face(x=512, y=250, w=25, h=34, score=0.805,
                re=(528.5, 264), le=(532.1, 263), nose=(536.1, 271),
                rm=(529.7, 277), lm=(532.1, 278))
    assert accept_yunet_face(row, FW, FH) is False


def test_low_score_rejected():
    assert accept_yunet_face(_face(score=0.61), FW, FH) is False


def test_ceiling_band_rejected():
    row = _face(x=400, y=2, w=80, h=100)
    assert accept_yunet_face(row, FW, FH) is False


def test_stand_band_rejected():
    row = _face(x=400, y=470, w=80, h=100)
    assert accept_yunet_face(row, FW, FH) is False


def test_nose_not_between_eyes_rejected():
    row = _face(re=(430, 238), le=(450, 238), nose=(470, 255))
    assert accept_yunet_face(row, FW, FH) is False


def test_upside_down_landmarks_rejected():
    row = _face(
        re=(424, 270), le=(456, 270), nose=(440, 240),
        rm=(428, 220), lm=(452, 220),
    )
    assert accept_yunet_face(row, FW, FH) is False


def test_select_skips_large_invalid_for_smaller_valid():
    junk = _face(x=10, y=2, w=80, h=100, score=0.99)  # ceiling
    good = _face(x=400, y=200, w=80, h=100, score=0.88)
    picked = select_yunet_face(np.stack([junk, good]), FW, FH)
    assert picked is not None
    assert float(picked[0]) == 400.0


def test_select_empty():
    assert select_yunet_face(None, FW, FH) is None
    assert select_yunet_face(np.zeros((0, 15)), FW, FH) is None


def test_face_gate_needs_confirm():
    gate = FaceGate(need=3, max_jump=0.18)
    box = (400, 200, 80, 100)
    assert gate.update(box, 960, 540) is None
    assert gate.update(box, 960, 540) is None
    assert gate.update(box, 960, 540) == box


def test_face_gate_jump_resets():
    gate = FaceGate(need=3, max_jump=0.18)
    a = (400, 200, 80, 100)
    b = (800, 400, 80, 100)
    assert gate.update(a, 960, 540) is None
    assert gate.update(a, 960, 540) is None
    assert gate.update(b, 960, 540) is None  # new target
    assert gate.update(b, 960, 540) is None
    assert gate.update(b, 960, 540) == b


def test_face_gate_loss_clears():
    gate = FaceGate(need=2, max_jump=0.18)
    box = (400, 200, 80, 100)
    gate.update(box, 960, 540)
    assert gate.update(box, 960, 540) == box
    assert gate.update(None, 960, 540) is None
    assert gate.update(box, 960, 540) is None


def test_profile_face_accepted():
    """Head turned: eyes cluster, nose not between them — still a face."""
    row = _face(
        x=400, y=200, w=80, h=100, score=0.90,
        re=(418, 238), le=(426, 237),
        nose=(410, 255),
        rm=(416, 278), lm=(428, 277),
    )
    assert accept_yunet_face(row, FW, FH) is True


def test_head_hold_keeps_box_on_roi_motion():
    from rally_follow import HeadHold
    hold = HeadHold(hold_s=1.0)
    box = (400, 200, 80, 100)
    t = 10.0
    assert hold.update(box, moving=False, now=t) == box
    assert hold.update(None, moving=True, now=t + 0.3) == box
    assert hold.update(None, moving=False, now=t + 0.4) == box  # coast
    assert hold.update(None, moving=False, now=t + 2.0) is None


def test_roi_motion_ignores_outside_last_box():
    from rally_follow import roi_motion
    prev = np.zeros((540, 960), dtype=np.uint8)
    now = prev.copy()
    now[10:40, 10:40] = 200  # motion far from the face
    assert roi_motion(prev, now, (400, 200, 80, 100)) is False
    now2 = prev.copy()
    now2[210:260, 420:460] = 200
    assert roi_motion(prev, now2, (400, 200, 80, 100)) is True


def test_stale_hold_freezes_does_not_keep_panning():
    """Last image-space box is not a pan target — that derails as the camera moves."""
    from rally_follow import lock_action, steer_speeds
    left = (80, 400, 160, 200)
    pan, tilt = steer_speeds(left, 1920, 1080, deadband=0.14)
    assert pan == -1
    assert lock_action(confirmed=left, held=left) == "steer"
    assert lock_action(confirmed=None, held=left) == "freeze"
    assert lock_action(confirmed=None, held=None) == "lost"
    cx = 1920 / 2 - 40
    mid = (int(cx - 80), 440, 160, 200)
    pan, tilt = steer_speeds(mid, 1920, 1080, deadband=0.14)
    assert pan == 0 and tilt == 0