"""
kinematics.py — Differential / skid-steer kinematics for SaltyRover (Issue #110).

Supports two drive variants selectable at runtime:

  2-wheel (differential drive)
  ────────────────────────────
  Standard unicycle model: one left motor, one right motor.

    v_left  = linear_x - angular_z * track_width / 2
    v_right = linear_x + angular_z * track_width / 2

  4-wheel (skid steer)
  ─────────────────────
  Left-side motors share the same speed; right-side motors share the same speed.
  The kinematic formula is identical to 2-wheel — the rover simply applies
  the same left/right speeds to front and rear motors on each side.

    v_left_front  = v_left
    v_left_rear   = v_left
    v_right_front = v_right
    v_right_rear  = v_right

  This gives accurate straight-line and point-turn behaviour.  Skid losses
  during cornering are handled by the ESC/hardware; the Jetson does not
  model them.

Output
──────
  Wheel speeds are returned in m/s, then clipped to ±max_wheel_speed and
  converted to ESC PWM duty-cycle (µs) by the caller.

Pure module — no ROS2 dependency; fully unit-testable.
"""

import math


# ─── Wheel-speed computation ────────────────────────────────────────────────

def two_wheel_speeds(
    linear_x: float,
    angular_z: float,
    track_width: float,
) -> tuple:
    """
    Unicycle → differential drive.

    Parameters
    ----------
    linear_x   : forward velocity (m/s)
    angular_z  : yaw rate (rad/s, +ve = CCW / left turn)
    track_width: centre-to-centre wheel separation (m)

    Returns
    -------
    (v_left, v_right) in m/s
    """
    half = angular_z * track_width / 2.0
    return linear_x - half, linear_x + half


def four_wheel_speeds(
    linear_x: float,
    angular_z: float,
    track_width: float,
) -> tuple:
    """
    Unicycle → skid-steer 4-wheel drive.

    Returns
    -------
    (v_left_front, v_left_rear, v_right_front, v_right_rear) in m/s
    """
    v_left, v_right = two_wheel_speeds(linear_x, angular_z, track_width)
    return v_left, v_left, v_right, v_right


# ─── m/s → ESC PWM ─────────────────────────────────────────────────────────

def speed_to_pwm(
    speed_ms: float,
    max_speed_ms: float,
    pwm_neutral_us: int,
    pwm_range_us: int,
) -> int:
    """
    Convert wheel speed (m/s) to ESC PWM pulse width (µs).

    Linear mapping:
        pwm = neutral + (speed / max_speed) * range

    Parameters
    ----------
    speed_ms      : desired wheel speed (m/s); negative = reverse
    max_speed_ms  : speed at full deflection (m/s)
    pwm_neutral_us: centre / brake pulse width (µs), typically 1500
    pwm_range_us  : half-range from neutral to full (µs), typically 500

    Returns
    -------
    PWM pulse width in µs, clamped to [neutral - range, neutral + range].
    """
    if max_speed_ms <= 0.0:
        return pwm_neutral_us
    normalised = max(-1.0, min(1.0, speed_ms / max_speed_ms))
    pwm = pwm_neutral_us + normalised * pwm_range_us
    lo  = pwm_neutral_us - pwm_range_us
    hi  = pwm_neutral_us + pwm_range_us
    return int(round(max(lo, min(hi, pwm))))


# ─── Variant-aware speed computation ────────────────────────────────────────

def compute_wheel_speeds(
    linear_x: float,
    angular_z: float,
    track_width: float,
    four_wheel: bool,
    max_speed_ms: float,
) -> tuple:
    """
    Compute (and clip) wheel speeds for the selected drive variant.

    Returns a tuple of clipped m/s values:
      2-wheel → (v_left, v_right)
      4-wheel → (v_left_front, v_left_rear, v_right_front, v_right_rear)

    Clipping is symmetric around zero at ±max_speed_ms.
    """
    def clip(v):
        return max(-max_speed_ms, min(max_speed_ms, v))

    if four_wheel:
        vfl, vlr, vrf, vrr = four_wheel_speeds(linear_x, angular_z, track_width)
        return clip(vfl), clip(vlr), clip(vrf), clip(vrr)
    else:
        vl, vr = two_wheel_speeds(linear_x, angular_z, track_width)
        return clip(vl), clip(vr)


# ─── Odometry helper ────────────────────────────────────────────────────────

def odometry_from_wheel_speeds(
    v_left: float,
    v_right: float,
    track_width: float,
) -> tuple:
    """
    Estimate linear and angular velocity from left/right wheel speeds.

    Returns (linear_x, angular_z) — the inverse of two_wheel_speeds.
    """
    linear_x  = (v_left + v_right) / 2.0
    angular_z = (v_right - v_left) / track_width if track_width > 0.0 else 0.0
    return linear_x, angular_z
