"""Four motor drive control for IBT-2/BTS7960 motor drivers."""

from __future__ import annotations

import logging

from .config import AgentConfig

LOG = logging.getLogger(__name__)


class MotorController:
    """Safe motor controller.

    Hardware output is disabled unless `hardware_enabled` is true. This lets the
    agent be tested on a laptop or an unpowered Pi without moving GAR-BOT.
    """

    def __init__(self, config: AgentConfig):
        self.config = config
        self.hardware_enabled = config.hardware_enabled
        self._pigpio = None
        if self.hardware_enabled:
            try:
                import pigpio  # type: ignore

                self._pigpio = pigpio.pi()
                if not self._pigpio.connected:
                    raise RuntimeError("pigpio daemon is not connected")
                self._setup_pins()
            except Exception:
                LOG.exception("Motor hardware setup failed; disabling hardware")
                self.hardware_enabled = False

    def _setup_pins(self) -> None:
        assert self._pigpio is not None
        for pins in self.config.motor_pins.values():
            for gpio in (pins.rpwm, pins.lpwm, pins.r_en, pins.l_en):
                self._pigpio.set_mode(gpio, 1)
                self._pigpio.write(gpio, 0)

    def stop(self) -> None:
        self.drive(0, 0)

    def drive(self, throttle: float, turn: float) -> None:
        throttle = max(-1.0, min(1.0, throttle))
        turn = max(-1.0, min(1.0, turn))
        left = max(-1.0, min(1.0, throttle - turn))
        right = max(-1.0, min(1.0, throttle + turn))
        commands = {
            "front_left": left,
            "rear_left": left,
            "front_right": right,
            "rear_right": right,
        }
        LOG.info("drive throttle=%.2f turn=%.2f commands=%s", throttle, turn, commands)
        if not self.hardware_enabled or self._pigpio is None:
            return
        for motor, speed in commands.items():
            self._set_motor(motor, speed)

    def _set_motor(self, motor: str, speed: float) -> None:
        assert self._pigpio is not None
        pins = self.config.motor_pins[motor]
        pwm = int(abs(speed) * 255)
        self._pigpio.write(pins.r_en, 1)
        self._pigpio.write(pins.l_en, 1)
        if speed >= 0:
            self._pigpio.set_PWM_dutycycle(pins.rpwm, pwm)
            self._pigpio.set_PWM_dutycycle(pins.lpwm, 0)
        else:
            self._pigpio.set_PWM_dutycycle(pins.rpwm, 0)
            self._pigpio.set_PWM_dutycycle(pins.lpwm, pwm)

    def close(self) -> None:
        self.stop()
        if self._pigpio is not None:
            self._pigpio.stop()
