"""Bbox-only visual servoing: turn a person ``Detection`` into a velocity.

The Go2 has no metric depth in this stack, so :class:`VisualServo` servos
purely on the detection bounding box:

* **Yaw** is a P(ID) law on the *normalised horizontal offset* of the box
  centroid (centre the target).
* **Forward** is a proportional law on how much shorter the box is than the
  ``stop_height_ratio`` setpoint (a box that fills more of the frame is
  closer), throttled down when the heading error is large so the dog turns to
  face the person before charging.

Velocity convention (matches the rest of GoWelcome / Unitree
``SportClient.Move``): ``vx`` forward+, ``vyaw`` CCW/left+ (rad/s). This module
only produces ``vx`` and ``vyaw``; lateral ``vy`` stays zero in APPROACH.

Pure module: imports only :mod:`math`, :mod:`config`, and
:mod:`gowelcome.types` -- no cv2, no SDK -- so it imports and unit-tests
off-robot.
"""

from __future__ import annotations

import math
from typing import Tuple

from config import ServoConfig
from gowelcome.types import Detection

from gowelcome.control.pid import PIDController


class VisualServo:
    """Pixel-offset visual servo producing ``(vx, vyaw, arrived)``.

    Holds a single yaw :class:`~gowelcome.control.pid.PIDController` built from
    the :class:`~config.ServoConfig`; the forward channel is a stateless
    proportional law. Call :meth:`reset` when re-acquiring a target so stale
    integral/derivative state does not leak across approaches.
    """

    def __init__(self, cfg: ServoConfig) -> None:
        """Build the yaw PID from ``cfg``.

        Args:
            cfg: Servo tuning block (gains, deadband, sign, limits, stop size).
        """
        self.cfg = cfg
        # The PID also carries the deadband so that, on the tick a target
        # crosses *into* the band, its derivative term is suppressed (the input
        # is pre-zeroed below, so the PID sees error 0 -> inside its own
        # deadband -> no derivative kick). The pre-zeroing of the input is what
        # additionally suppresses the proportional term inside the band.
        self.yaw_pid = PIDController(
            kp=cfg.kp_yaw,
            ki=cfg.ki_yaw,
            kd=cfg.kd_yaw,
            output_limits=(-cfg.max_yaw_rate, cfg.max_yaw_rate),
            deadband=cfg.yaw_deadband,
        )

    def reset(self) -> None:
        """Reset the internal yaw PID state (integral + previous error)."""
        self.yaw_pid.reset()

    def compute(
        self, target: Detection, frame_w: int, frame_h: int, dt: float
    ) -> Tuple[float, float, bool]:
        """Compute one servoing step toward ``target``.

        Pixel-offset P(ID) controller, bbox-only (no depth).

        Args:
            target: The person bounding box to servo toward.
            frame_w: Frame width in pixels (for the horizontal offset).
            frame_h: Frame height in pixels (for the height-ratio distance
                proxy).
            dt: Timestep in seconds (fed to the yaw PID derivative/integral).

        Returns:
            ``(vx, vyaw, arrived)`` where ``vx`` is forward velocity (m/s,
            never negative in APPROACH), ``vyaw`` is yaw rate (rad/s, CCW+),
            and ``arrived`` is ``True`` once the box fills at least
            ``cfg.stop_height_ratio`` of the frame height.
        """
        cfg = self.cfg

        # --- Yaw: centre the target horizontally. -------------------------
        norm_err = target.horizontal_offset(frame_w)  # -1..1, + = right of centre
        norm_err_eff = 0.0 if abs(norm_err) < cfg.yaw_deadband else norm_err
        vyaw = cfg.yaw_sign * self.yaw_pid.update(norm_err_eff, dt)

        # --- Forward: approach until the box fills the frame vertically. ---
        height_ratio = target.height_ratio(frame_h)
        arrived = height_ratio >= cfg.stop_height_ratio

        if arrived:
            vx = 0.0
        else:
            # Box smaller than the stop size -> positive forward command.
            forward_raw = cfg.kp_forward * (cfg.stop_height_ratio - height_ratio)
            # Throttle forward when heading error is large (don't charge sideways).
            scaling = math.exp(-cfg.forward_heading_falloff * abs(norm_err))  # 0..1
            vx = forward_raw * scaling
            # Never reverse in APPROACH; clamp to the configured max.
            vx = max(0.0, min(cfg.max_forward, vx))
            # Below min_forward, treat as stopped-forward (turning still allowed).
            if vx < cfg.min_forward:
                vx = 0.0

        return (vx, vyaw, arrived)