"""
_wheel_odom.py — Differential drive wheel odometry kinematics (no ROS2 deps).

Algorithm
---------
Given incremental left/right wheel displacements (metres) since the last update:

    d_center = (d_left + d_right) / 2
    d_theta  = (d_right − d_left) / wheel_base

Pose is integrated using the midpoint Euler method (equivalent to the exact
ICC solution for a circular arc, but simpler and numerically robust):

    mid_theta = theta + d_theta / 2
    x        += d_center · cos(mid_theta)
    y        += d_center · sin(mid_theta)
    theta    += d_theta

theta is kept in (−π, π] after every step.

Int32 rollover
--------------
<<<<<<< HEAD
ESP32 BALANCE encoder counters are int32 and wrap at ±2^31.  `unwrap_delta` handles
=======
ESP32-S3 encoder counters are int32 and wrap at ±2^31.  `unwrap_delta` handles
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
this by detecting jumps larger than half the int32 range and adjusting by the
full range:

    if  delta >  2^30 : delta -= 2^31
    if  delta < -2^30 : delta += 2^31

This correctly handles up to ½ of the full int32 range per message interval —
safely above any realistic tick rate at 50 Hz.

Public API
----------
  WheelOdomState(x, y, theta)
  unwrap_delta(current, prev)        -> int
  ticks_to_meters(ticks, radius, ticks_per_rev) -> float
  integrate_odom(x, y, theta, d_left_m, d_right_m, wheel_base) -> (x, y, theta)
  normalize_angle(theta)             -> float
  quat_from_yaw(yaw)                 -> (qx, qy, qz, qw)
"""

from __future__ import annotations

import math
from typing import NamedTuple, Tuple


# ── Data types ────────────────────────────────────────────────────────────────

class WheelOdomState(NamedTuple):
    """Current dead-reckoning pose estimate."""
    x:     float   # metres (world frame)
    y:     float   # metres (world frame)
    theta: float   # radians, kept in (−π, π]


# ── Int32 rollover handling ───────────────────────────────────────────────────

_INT32_HALF = 2 ** 31   # half of the full int32 value range (2^32) — detection threshold
_INT32_FULL = 2 ** 32   # full int32 value range — adjustment amount


def unwrap_delta(current: int, prev: int) -> int:
    """
    Compute signed tick delta handling int32 counter rollover.

    Parameters
    ----------
    current : current tick count (int32, may have wrapped)
    prev    : previous tick count

    Returns
    -------
    Signed delta ticks, correct even across the ±2^31 rollover boundary.
    """
    delta = int(current) - int(prev)
    if delta > _INT32_HALF:
        delta -= _INT32_FULL
    elif delta < -_INT32_HALF:
        delta += _INT32_FULL
    return delta


# ── Unit conversion ───────────────────────────────────────────────────────────

def ticks_to_meters(delta_ticks: int, wheel_radius: float, ticks_per_rev: int) -> float:
    """
    Convert encoder tick count to linear wheel displacement in metres.

    Parameters
    ----------
    delta_ticks  : signed tick increment
    wheel_radius : effective wheel radius (metres)
    ticks_per_rev: encoder ticks per full wheel revolution

    Returns
    -------
    Linear displacement (metres); positive = forward.
    """
    if ticks_per_rev <= 0:
        return 0.0
    return delta_ticks * (2.0 * math.pi * wheel_radius) / ticks_per_rev


# ── Pose integration ──────────────────────────────────────────────────────────

def normalize_angle(theta: float) -> float:
    """Wrap angle to (−π, π]."""
    return math.atan2(math.sin(theta), math.cos(theta))


def integrate_odom(
    x:          float,
    y:          float,
    theta:      float,
    d_left_m:   float,
    d_right_m:  float,
    wheel_base: float,
) -> Tuple[float, float, float]:
    """
    Integrate differential drive kinematics using the midpoint Euler method.

    Parameters
    ----------
    x, y, theta  : current pose (metres, metres, radians)
    d_left_m     : left  wheel displacement since last update (metres)
    d_right_m    : right wheel displacement since last update (metres)
    wheel_base   : centre-to-centre wheel separation (metres)

    Returns
    -------
    (x, y, theta) updated pose
    """
    d_center = (d_left_m + d_right_m) / 2.0
    d_theta  = (d_right_m - d_left_m) / wheel_base

    mid_theta = theta + d_theta / 2.0
    new_x     = x + d_center * math.cos(mid_theta)
    new_y     = y + d_center * math.sin(mid_theta)
    new_theta = normalize_angle(theta + d_theta)

    return new_x, new_y, new_theta


# ── Quaternion helper ─────────────────────────────────────────────────────────

def quat_from_yaw(yaw: float) -> Tuple[float, float, float, float]:
    """
    Convert a yaw angle (rotation about Z) to a unit quaternion.

    Parameters
    ----------
    yaw : rotation angle in radians

    Returns
    -------
    (qx, qy, qz, qw)
    """
    half = yaw / 2.0
    return (0.0, 0.0, math.sin(half), math.cos(half))
