"""
driver
TB6612FNG モータードライバを制御するモジュール
差動2輪駆動で左右のモーターを制御する
"""

from common import config

try:
    import RPi.GPIO as GPIO
except Exception:
    GPIO = None


class MotorDriver:
    """TB6612FNG を介して左右のモーターを制御するクラス"""

    def __init__(self) -> None:
        self._pwm_a: object = None
        self._pwm_b: object = None
        self._ready: bool = False

    def start(self) -> None:
        """GPIO を初期化して PWM を開始する"""
        if GPIO is None:
            print("GPIO 未検出: モーター無効（非 RPi 環境）")
            return

        GPIO.setmode(GPIO.BOARD)

        # モーター A（左）
        GPIO.setup(config.MA_IN1, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(config.MA_IN2, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(config.MA_PWM, GPIO.OUT, initial=GPIO.LOW)

        # モーター B（右）
        GPIO.setup(config.MB_IN1, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(config.MB_IN2, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(config.MB_PWM, GPIO.OUT, initial=GPIO.LOW)

        self._pwm_a = GPIO.PWM(
            config.MA_PWM, config.MOTOR_PWM_FREQ,
        )
        self._pwm_b = GPIO.PWM(
            config.MB_PWM, config.MOTOR_PWM_FREQ,
        )
        self._pwm_a.start(0)
        self._pwm_b.start(0)
        self._ready = True
        print("モーター初期化完了")

    @staticmethod
    def _clamp(
        value: float, low: float, high: float,
    ) -> float:
        """値を指定範囲に制限する"""
        return max(low, min(high, value))

    def _apply_one_motor(
        self, in1: int, in2: int, pwm: object,
        speed: float,
    ) -> None:
        """1つのモーターに速度を適用する

        Args:
            in1: IN1 の GPIO ピン番号
            in2: IN2 の GPIO ピン番号
            pwm: PWM オブジェクト
            speed: -1.0 ~ +1.0 の速度値
        """
        if speed > 0:
            GPIO.output(in1, GPIO.LOW)
            GPIO.output(in2, GPIO.HIGH)
        elif speed < 0:
            GPIO.output(in1, GPIO.HIGH)
            GPIO.output(in2, GPIO.LOW)
        else:
            GPIO.output(in1, GPIO.LOW)
            GPIO.output(in2, GPIO.LOW)
        pwm.ChangeDutyCycle(abs(speed) * 100.0)

    def set_drive(
        self, throttle: float, steer: float,
    ) -> None:
        """throttle と steer からモーターを駆動する

        Args:
            throttle: 前後方向 (-1.0 ~ +1.0)
            steer: 左右方向 (-1.0 ~ +1.0)
        """
        if not self._ready:
            return

        throttle = self._clamp(throttle, -1.0, 1.0)
        steer = self._clamp(steer, -1.0, 1.0)

        # ステアリング方向の補正
        if config.STEER_REVERSED:
            steer = -steer

        # 差動2輪: 左右の速度を計算
        left = self._clamp(throttle + steer, -1.0, 1.0)
        right = self._clamp(throttle - steer, -1.0, 1.0)

        # モーター配線の極性補正
        if config.MOTOR_LEFT_REVERSED:
            left = -left
        if config.MOTOR_RIGHT_REVERSED:
            right = -right

        self._apply_one_motor(
            config.MA_IN1, config.MA_IN2,
            self._pwm_a, left,
        )
        self._apply_one_motor(
            config.MB_IN1, config.MB_IN2,
            self._pwm_b, right,
        )

    def stop(self) -> None:
        """モーターを停止する"""
        self.set_drive(0.0, 0.0)

    def cleanup(self) -> None:
        """GPIO リソースを解放する"""
        if not self._ready:
            return
        self.stop()
        self._pwm_a.stop()
        self._pwm_b.stop()
        GPIO.cleanup()
        self._ready = False