"""
visual_servo.py — Image-based visual servo for final dock approach (Issue #158).

Control law
───────────
  Operates once the robot is within *servo_range_m* of the dock.

  Linear velocity (forward):
      v = clip( k_linear × (distance_m − target_distance_m),  0,  max_linear_ms )

  Angular velocity (yaw correction):
      ω = clip( −k_angular × yaw_rad,  −max_angular_rads,  +max_angular_rads )

  The linear term drives the robot toward the dock until it reaches
  target_distance_m.  The clip to [0, …] prevents reversing.

  The angular term zeroes the bearing error; sign convention:
    yaw_rad > 0  →  dock is to the robot's left  →  turn left (ω > 0).

Alignment declaration
─────────────────────
  `aligned` is True when both:
    |lateral_m|   < lateral_tol_m      (default ±5 mm)
    distance_m    < contact_distance_m (default 50 mm)

Pure module — no ROS2 dependency.
"""

from __future__ import annotations

import math
from dataclasses import dataclass
from typing import Optional

from saltybot_docking.dock_detector import DockPose


# ── Result ────────────────────────────────────────────────────────────────────

@dataclass
class VisualServoResult:
    linear_x:       float   # forward velocity (m/s); ≥ 0
    angular_z:      float   # yaw velocity (rad/s)
    distance_m:     float   # current distance to dock (m)
    lateral_error_m: float  # current lateral offset (m); +ve = dock is right
    aligned:        bool    # True when within contact tolerances


# ── VisualServo ───────────────────────────────────────────────────────────────

class VisualServo:
    """
    Proportional visual servo controller for dock final approach.

    Parameters
    ----------
    k_linear          : distance gain  (m/s per metre of error)
    k_angular         : yaw gain       (rad/s per radian of bearing error)
    target_distance_m : desired stop distance from dock face (m)
    lateral_tol_m     : lateral alignment tolerance (m)   [spec ±5 mm → 0.005]
    contact_distance_m: distance below which contact is assumed (m)
    max_linear_ms     : velocity ceiling (m/s)
    max_angular_rads  : yaw-rate ceiling (rad/s)
    """

    def __init__(
        self,
        k_linear:           float = 0.30,
        k_angular:          float = 0.80,
        target_distance_m:  float = 0.02,    # 20 mm — at the dock face
        lateral_tol_m:      float = 0.005,   # ±5 mm
        contact_distance_m: float = 0.05,    # 50 mm — within contact zone
        max_linear_ms:      float = 0.10,    # slow for precision approach
        max_angular_rads:   float = 0.30,
    ):
        self._k_lin   = float(k_linear)
        self._k_ang   = float(k_angular)
        self._target  = float(target_distance_m)
        self._lat_tol = max(1e-6, float(lateral_tol_m))
        self._contact = max(self._target, float(contact_distance_m))
        self._v_max   = max(0.0, float(max_linear_ms))
        self._w_max   = max(0.0, float(max_angular_rads))

    # ── Control step ─────────────────────────────────────────────────────────

    def update(self, dock_pose: DockPose) -> VisualServoResult:
        """
        Compute one velocity command from the current dock pose.

        Parameters
        ----------
        dock_pose : DockPose from ArucoDetector or IRBeaconDetector

        Returns
        -------
        VisualServoResult with cmd velocities and alignment flag
        """
        dist = dock_pose.distance_m
        yaw  = dock_pose.yaw_rad
        lat  = dock_pose.lateral_m

        # Linear: approach until target_distance; no reversing
        v_raw = self._k_lin * (dist - self._target)
        v = max(0.0, min(self._v_max, v_raw))

        # Angular: null yaw error; positive yaw → turn left (+ω)
        w_raw = -self._k_ang * yaw
        w = max(-self._w_max, min(self._w_max, w_raw))

        aligned = (abs(lat) < self._lat_tol) and (dist < self._contact)

        return VisualServoResult(
            linear_x=v,
            angular_z=w,
            distance_m=dist,
            lateral_error_m=lat,
            aligned=aligned,
        )

    def stop(self) -> VisualServoResult:
        """
        Zero-velocity command — used when dock pose is momentarily lost.
        """
        return VisualServoResult(
            linear_x=0.0,
            angular_z=0.0,
            distance_m=float("nan"),
            lateral_error_m=float("nan"),
            aligned=False,
        )
