from machine import Pin, PWM


class Motor:
    def __init__(self, pwm_pin, in1_pin, in2_pin) -> None:
        self.pwm = pwm_pin
        self.in1 = in1_pin
        self.in2 = in2_pin


class MotorController:
    def __init__(self, left_motor, right_motor) -> None:
        self.left = left_motor
        self.right = right_motor
        self.speed = 65535 // 2

    def forward(self):
        """Forward move"""
        self.left.in1.value(1)
        self.left.in2.value(0)
        self.right.in2.value(1)
        self.right.in1.value(0)
        self.left.pwm.duty_u16(self.speed)
        self.right.pwm.duty_u16(self.speed)

    def reverse(self):
        """Reverse move"""
        self.left.in1.value(0)
        self.left.in2.value(1)
        self.right.in2.value(0)
        self.right.in1.value(1)
        self.left.pwm.duty_u16(self.speed)
        self.right.pwm.duty_u16(self.speed)

    def stop(self):
        """All stop"""
        self.left.pwm.duty_u16(0)
        self.right.pwm.duty_u16(0)

    def right_forward(self):
        """Right turn while moving forward"""
        self.left.in1.value(1)
        self.left.in2.value(0)
        self.right.in2.value(1)
        self.right.in1.value(0)
        self.left.pwm.duty_u16(self.speed)
        self.right.pwm.duty_u16(self.speed // 2)

    def right_spin(self):
        """Spins clockwise in place"""
        self.left.in1.value(1)
        self.left.in2.value(0)
        self.right.in2.value(0)
        self.right.in1.value(1)
        self.left.pwm.duty_u16(self.speed)
        self.right.pwm.duty_u16(self.speed // 2)

    def right_reverse(self):
        """Right turn while moving in reverse"""
        self.left.in1.value(0)
        self.left.in2.value(1)
        self.right.in2.value(0)
        self.right.in1.value(1)
        self.left.pwm.duty_u16(self.speed)
        self.right.pwm.duty_u16(self.speed // 2)

    def left_forward(self):
        """Right turn while moving forward"""
        self.left.in1.value(1)
        self.left.in2.value(0)
        self.right.in2.value(1)
        self.right.in1.value(0)
        self.left.pwm.duty_u16(self.speed // 2)
        self.right.pwm.duty_u16(self.speed)

    def left_spin(self):
        """Spins counter-clockwise in place"""
        self.left.in1.value(0)
        self.left.in2.value(1)
        self.right.in2.value(1)
        self.right.in1.value(0)
        self.left.pwm.duty_u16(self.speed)
        self.right.pwm.duty_u16(self.speed)

    def left_reverse(self):
        """Left turn while moving in reverse"""
        self.left.in1.value(0)
        self.left.in2.value(1)
        self.right.in2.value(0)
        self.right.in1.value(1)
        self.left.pwm.duty_u16(self.speed // 2)
        self.right.pwm.duty_u16(self.speed)

    @classmethod
    def from_config(cls, pin_config):
        return cls(
            Motor(
                # 100Hz duty cycle, range 0-65535
                pwm_pin=PWM(
                    Pin(pin_config["left"]["motor"]["pwm"], Pin.OUT),
                    freq=100,
                    duty_u16=0,
                ),
                in1_pin=Pin(pin_config["left"]["motor"]["in1"], Pin.OUT),
                in2_pin=Pin(pin_config["left"]["motor"]["in2"], Pin.OUT),
            ),
            Motor(
                # 100Hz duty cycle, range 0-65535
                pwm_pin=PWM(
                    Pin(pin_config["right"]["motor"]["pwm"], Pin.OUT),
                    freq=100,
                    duty_u16=0,
                ),
                in1_pin=Pin(pin_config["right"]["motor"]["in1"], Pin.OUT),
                in2_pin=Pin(pin_config["right"]["motor"]["in2"], Pin.OUT),
            ),
        )