"""
diff_drive_odom.py — Pure differential drive odometry kinematics (Issue #646).

No ROS2 dependencies — pure Python, fully unit-testable.

Computes x, y, theta, v_lin, v_ang from per-wheel electrical RPM using exact
arc integration.  Designed to be wrapped by a ROS2 node.

Motor convention
────────────────
  eRPM → mechanical RPM = eRPM / motor_poles  (VESC reports electrical RPM)
  Mechanical RPM → wheel surface velocity (m/s) = RPM/60 × 2π × wheel_radius

  If invert_right=True the right eRPM sign is flipped before use (handles
  physically-mirrored motor mounting where positive eRPM on both motors would
  otherwise produce counter-rotation rather than forward motion).

Kinematics
──────────
  v_left  = wheel surface velocity of left wheel (m/s)
  v_right = wheel surface velocity of right wheel (m/s)
  v_lin   = (v_right + v_left) / 2          forward velocity (m/s)
  v_ang   = (v_right - v_left) / wheel_sep  yaw rate (rad/s, CCW positive)

Integration uses the exact arc formula to reduce heading error accumulation:
  If |v_ang| > ε:
    r      = v_lin / v_ang
    dtheta = v_ang * dt
    x     += r * (sin(theta + dtheta) - sin(theta))
    y     -= r * (cos(theta + dtheta) - cos(theta))
    theta += dtheta
  Else (straight line):
    x     += v_lin * cos(theta) * dt
    y     += v_lin * sin(theta) * dt

Heading is normalised to (-π, π] after each update.
"""

from __future__ import annotations

import math


class DiffDriveOdometry:
    """
    Differential drive odometry from dual VESC CAN motor RPM feedback.

    Thread model: single-threaded — caller must serialise access.
    """

    def __init__(
        self,
        wheel_radius: float = 0.10,
        wheel_separation: float = 0.35,
        motor_poles: int = 7,
        invert_right: bool = True,
    ) -> None:
        """
        Parameters
        ----------
        wheel_radius     : wheel radius (m)
        wheel_separation : distance between left and right wheel contact points (m)
        motor_poles      : BLDC motor pole pairs (VESC eRPM / motor_poles = mechanical RPM)
        invert_right     : negate right eRPM before computing velocity (mirrored mount)
        """
        self.wheel_radius     = wheel_radius
        self.wheel_separation = wheel_separation
        self.motor_poles      = motor_poles
        self.invert_right     = invert_right

        # Odometry state (map/odom frame)
        self.x:     float = 0.0
        self.y:     float = 0.0
        self.theta: float = 0.0

        # Current velocities (body frame)
        self.v_lin: float = 0.0   # linear  (m/s, forward positive)
        self.v_ang: float = 0.0   # angular (rad/s, CCW positive)

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

    def erpm_to_wheel_vel(self, erpm: float) -> float:
        """Convert electrical RPM to wheel surface velocity (m/s)."""
        mech_rpm = erpm / self.motor_poles
        return (mech_rpm / 60.0) * (2.0 * math.pi) * self.wheel_radius

    # ── Kinematics update ─────────────────────────────────────────────────────

    def update(
        self,
        erpm_left: float,
        erpm_right: float,
        dt: float,
    ) -> tuple[float, float]:
        """
        Integrate one differential drive step.

        Parameters
        ----------
        erpm_left  : left motor electrical RPM  (positive = forward)
        erpm_right : right motor electrical RPM (positive = forward before invert)
        dt         : elapsed time (s); must be > 0

        Returns
        -------
        (v_lin, v_ang) — linear (m/s) and angular (rad/s) velocities after update
        """
        if dt <= 0.0:
            return self.v_lin, self.v_ang

        if self.invert_right:
            erpm_right = -erpm_right

        v_left  = self.erpm_to_wheel_vel(erpm_left)
        v_right = self.erpm_to_wheel_vel(erpm_right)

        v_lin = (v_right + v_left) / 2.0
        v_ang = (v_right - v_left) / self.wheel_separation

        # Exact arc integration
        if abs(v_ang) > 1e-6:
            r_turn  = v_lin / v_ang
            dtheta  = v_ang * dt
            self.x      += r_turn * (math.sin(self.theta + dtheta) - math.sin(self.theta))
            self.y      -= r_turn * (math.cos(self.theta + dtheta) - math.cos(self.theta))
            self.theta  += dtheta
        else:
            self.x += v_lin * math.cos(self.theta) * dt
            self.y += v_lin * math.sin(self.theta) * dt

        # Normalise heading to (-π, π]
        self.theta = (self.theta + math.pi) % (2.0 * math.pi) - math.pi

        self.v_lin = v_lin
        self.v_ang = v_ang

        return v_lin, v_ang

    def reset(self, x: float = 0.0, y: float = 0.0, theta: float = 0.0) -> None:
        """Reset odometry state to given pose."""
        self.x     = x
        self.y     = y
        self.theta = theta
        self.v_lin = 0.0
        self.v_ang = 0.0
