"""Unit tests for :mod:`gowelcome.control.servoing` (VisualServo).

Constructs ``Detection`` objects directly and a ``ServoConfig`` from the
project config; asserts the velocity *signs and inequalities* rather than exact
floats, so the tests survive minor gain re-tuning. No cv2 / SDK / hardware.
"""

from __future__ import annotations

from config import ServoConfig, default_config
from gowelcome.control.servoing import VisualServo
from gowelcome.types import Detection


FRAME_W = 640
FRAME_H = 480
DT = 1.0 / 15.0


def _centered_box(frame_w: int, frame_h: int, h_px: int, w_px: int = 80) -> Detection:
    """A person box centred horizontally, with the given pixel height."""
    cx = frame_w / 2.0
    cy = frame_h / 2.0
    x1 = int(cx - w_px / 2)
    x2 = int(cx + w_px / 2)
    y1 = int(cy - h_px / 2)
    y2 = int(cy + h_px / 2)
    return Detection(label="person", conf=0.95, x1=x1, y1=y1, x2=x2, y2=y2)


def test_servoconfig_from_default_config():
    """ServoConfig is reachable from the top-level default config."""
    cfg = default_config()
    assert isinstance(cfg.servo, ServoConfig)
    assert cfg.servo.yaw_sign == -1.0  # contract default


def test_centered_far_target_drives_forward_with_small_yaw():
    """Centred + small box (far) -> vx > 0 and |vyaw| small (near zero)."""
    cfg = default_config().servo
    servo = VisualServo(cfg)
    # Small box -> far away -> below stop_height_ratio.
    far_box = _centered_box(FRAME_W, FRAME_H, h_px=60)  # ratio 0.125 << 0.50
    vx, vyaw, arrived = servo.compute(far_box, FRAME_W, FRAME_H, DT)
    assert not arrived
    assert vx > 0.0
    # Perfectly centred -> within the deadband -> essentially no turn.
    assert abs(vyaw) < 1e-6


def test_far_right_target_yaw_sign_is_correct():
    """Target far to the RIGHT (norm_err>0) with yaw_sign=-1 -> vyaw < 0."""
    cfg = default_config().servo
    assert cfg.yaw_sign == -1.0
    servo = VisualServo(cfg)
    # Push the box well to the right of centre (cx ~ 0.9 * frame_w).
    box = Detection(label="person", conf=0.9, x1=540, y1=200, x2=620, y2=300)
    assert box.horizontal_offset(FRAME_W) > 0.0  # confirm "right of centre"
    _vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT)
    assert vyaw < 0.0  # turn right (CW) to centre a right-side target


def test_far_left_target_yaw_sign_is_correct():
    """Target far to the LEFT (norm_err<0) with yaw_sign=-1 -> vyaw > 0."""
    cfg = default_config().servo
    servo = VisualServo(cfg)
    box = Detection(label="person", conf=0.9, x1=20, y1=200, x2=100, y2=300)
    assert box.horizontal_offset(FRAME_W) < 0.0  # left of centre
    _vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT)
    assert vyaw > 0.0  # turn left (CCW) to centre a left-side target


def test_arrived_when_box_fills_stop_height_ratio():
    """Box height >= stop_height_ratio of the frame -> arrived True and vx == 0."""
    cfg = default_config().servo
    servo = VisualServo(cfg)
    # Box taller than stop_height_ratio * frame_h (0.50 * 480 = 240).
    h_px = int(cfg.stop_height_ratio * FRAME_H) + 40
    near_box = _centered_box(FRAME_W, FRAME_H, h_px=h_px)
    assert near_box.height_ratio(FRAME_H) >= cfg.stop_height_ratio
    vx, _vyaw, arrived = servo.compute(near_box, FRAME_W, FRAME_H, DT)
    assert arrived is True
    assert vx == 0.0


def test_deadband_zeroes_yaw_near_centre():
    """A target inside the yaw deadband produces ~0 yaw."""
    cfg = default_config().servo
    servo = VisualServo(cfg)
    # Offset the centre by less than yaw_deadband (0.06) of the half-width.
    # half-width = 320; 0.06 * 320 ~= 19 px, so nudge by ~10 px.
    cx = FRAME_W / 2.0 + 10
    box = Detection(
        label="person", conf=0.9,
        x1=int(cx - 40), y1=200, x2=int(cx + 40), y2=300,
    )
    assert abs(box.horizontal_offset(FRAME_W)) < cfg.yaw_deadband
    _vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT)
    assert abs(vyaw) < 1e-6


def test_no_derivative_kick_crossing_into_deadband():
    """Crossing from off-centre INTO the deadband must not spike the yaw.

    Regression for the derivative-kick: with a non-zero kd, a target that was
    off-centre last tick and is centred this tick used to inject a one-tick
    derivative spike. The PID's own deadband (fed a pre-zeroed in-band error)
    must suppress it, so the centred tick yields ~0 yaw.
    """
    cfg = default_config().servo
    assert cfg.kd_yaw > 0.0  # the kick only exists with a derivative term
    servo = VisualServo(cfg)

    # Tick 1: clearly off-centre (outside the deadband) -> a real turn.
    off = Detection(label="person", conf=0.9, x1=560, y1=200, x2=620, y2=300)
    assert off.horizontal_offset(FRAME_W) > cfg.yaw_deadband
    _vx1, vyaw1, _ = servo.compute(off, FRAME_W, FRAME_H, DT)
    assert abs(vyaw1) > 0.0

    # Tick 2: now centred (inside the deadband) -> must settle to ~0, no kick.
    centred = _centered_box(FRAME_W, FRAME_H, h_px=120)
    assert abs(centred.horizontal_offset(FRAME_W)) < cfg.yaw_deadband
    _vx2, vyaw2, _ = servo.compute(centred, FRAME_W, FRAME_H, DT)
    assert abs(vyaw2) < 1e-6


def test_forward_throttled_off_axis():
    """A far box far off-axis should command less forward than the same box centred."""
    cfg = default_config().servo

    far_h = 60  # well below the stop ratio for both cases

    servo_centre = VisualServo(cfg)
    centred = _centered_box(FRAME_W, FRAME_H, h_px=far_h)
    vx_centre, _, _ = servo_centre.compute(centred, FRAME_W, FRAME_H, DT)

    servo_side = VisualServo(cfg)
    off_axis = Detection(label="person", conf=0.9, x1=580, y1=210,
                         x2=620, y2=210 + far_h)
    vx_side, _, _ = servo_side.compute(off_axis, FRAME_W, FRAME_H, DT)

    # Heading-error throttle (exp(-falloff*|err|)) makes the off-axis forward
    # speed no greater than the centred one.
    assert vx_side <= vx_centre