from .Raspbot_Lib import Raspbot

# Motor ID assignments (from Raspbot_Lib comments)
_MOTOR_FL = 0  # L1 — front left
_MOTOR_RL = 1  # L2 — rear left
_MOTOR_FR = 2  # R1 — front right
_MOTOR_RR = 3  # R2 — rear right

_DRIVER_MAX = 255


class MotorController:
    """
    Thin adapter between the ROS 2 motor controller node and Raspbot_Lib.

    Speed convention: the node passes values in the range
    [-max_speed, max_speed] (default max_speed = 1.0).  This class scales
    them linearly to the driver's 0-255 range, so max_speed should match
    whatever normalised unit you use (1.0 → full throttle).
    """

    def __init__(self, max_speed: float = 1.0):
        self._max_speed = max_speed
        self._bot: Raspbot | None = None
        # Shadow register — Raspbot_Lib has no read-back, so we track state.
        self._speeds = [0.0, 0.0, 0.0, 0.0]  # [FL, FR, RL, RR]

    # ------------------------------------------------------------------
    # Lifecycle
    # ------------------------------------------------------------------

    def initialize(self) -> None:
        """Create the I2C connection and ensure all motors are stopped."""
        self._bot = Raspbot()
        self._stop_all_motors()

    def shutdown(self) -> None:
        """Stop motors and release the driver handle."""
        if self._bot is not None:
            self._stop_all_motors()
            self._bot = None

    # ------------------------------------------------------------------
    # Motor control
    # ------------------------------------------------------------------

    def set_speed(
        self,
        front_left: float,
        front_right: float,
        rear_left: float,
        rear_right: float,
    ) -> None:
        """
        Set all four wheel speeds simultaneously.

        Parameters
        ----------
        front_left, front_right, rear_left, rear_right:
            Desired speeds in the range [-max_speed, max_speed].
            Positive = forward, negative = backward.
        """
        self._require_initialized()
        mapping = [
            (_MOTOR_FL, front_left),
            (_MOTOR_RL, rear_left),
            (_MOTOR_FR, front_right),
            (_MOTOR_RR, rear_right),
        ]
        for motor_id, speed in mapping:
            self._bot.Ctrl_Muto(motor_id, self._to_driver_units(speed))

        self._speeds = [front_left, front_right, rear_left, rear_right]

    def stop_all(self) -> None:
        """Immediately stop all motors."""
        self._require_initialized()
        self._stop_all_motors()
        self._speeds = [0.0, 0.0, 0.0, 0.0]

    def get_speeds(self) -> list[float]:
        """
        Return the last commanded wheel speeds as [FL, FR, RL, RR].

        The Raspbot driver provides no encoder/tachometer read-back, so this
        returns the most recently written values.
        """
        return list(self._speeds)

    # ------------------------------------------------------------------
    # Helpers
    # ------------------------------------------------------------------

    def _to_driver_units(self, speed: float) -> int:
        """Scale a speed value to the driver's integer range [-255, 255]."""
        if self._max_speed == 0:
            return 0
        scaled = (speed / self._max_speed) * _DRIVER_MAX
        return int(max(-_DRIVER_MAX, min(_DRIVER_MAX, scaled)))

    def _stop_all_motors(self) -> None:
        for motor_id in (_MOTOR_FL, _MOTOR_RL, _MOTOR_FR, _MOTOR_RR):
            self._bot.Ctrl_Muto(motor_id, 0)

    def _require_initialized(self) -> None:
        if self._bot is None:
            raise RuntimeError(
                "MotorController has not been initialized. Call initialize() first."
            )