from machine import Pin
from machine import PWM
from machine import Timer




def duty_to_u16(x):
    """Convert a percentage 0..100 into the range used by the PWM hardware of
    0..65535.   (2**16 - 1), the largest unsigned 16-bit number.

    The sign of _x_ is ignored and treated as always positive."""

    x = abs(x)  # ensure this is a positive number
    out = (65535 * x) // 100
    return out



class motor:
    """Class that describes how to control _some_ motor, since they all work
    the same way.  When creating a specific instance of the motor drive, it
    needs to be told which two pins it has control over, labeled _a_ and _b_
    here."""

    #
    # Initialization
    #
    def __init__(self, pin_a, pin_b, freq=1_000):
        self.pin_a = pin_a
        self.pin_b = pin_b
        self.a = PWM(Pin(pin_a, Pin.OUT), freq=freq, duty_u16=0)
        self.b = PWM(Pin(pin_b, Pin.OUT), freq=freq, duty_u16=0)
        self._running = False

    #
    # Basic operations
    #
    def coast(self):
        # see the DRV8837 motor driver chip datasheet
        self.a.duty_u16(0)  # always low
        self.b.duty_u16(0)
        self._running = False

    def brake(self):
        # see the DRV8837 motor driver chip datasheet
        self.a.duty_u16(65535)  # always high
        self.b.duty_u16(65535)
        self._running = False

    def stop(self, arg=None):
        """There are two ways to "stop", so uncomment the one that best fits
        your situation."""
        self.coast()
        #self.brake()

        if arg:
            arg.deinit()

    #
    # Actual motions.
    #
    # speed - percentage 0..100.  Negative values mean opposite direction.
    # howlong - time in seconds (currently ignored)
    #
    def forward(self, speed, howlong=None):
        speed_u16 = duty_to_u16(speed)
        self.a.duty_u16(speed_u16)
        self.b.duty_u16(0)
        self._running = True

        if howlong:
            t = Timer(mode=Timer.ONE_SHOT, period=howlong, callback=self.stop)

    def backward(self, speed, howlong=None):
        speed_u16 = duty_to_u16(speed)
        self.a.duty_u16(0)
        self.b.duty_u16(speed_u16)
        self._running = True

        if howlong:
            t = Timer(mode=Timer.ONE_SHOT, period=howlong, callback=self.stop)

    def go(self, speed, howlong=None):
        """Move forward or backwards according to the sign of speed."""
        if speed > 0:
            self.forward(speed, howlong)

        elif speed < 0:
            self.backward(-speed, howlong)

        elif speed == 0:
            self.stop()

        else:
            print(f"ERROR: Unknown speed: {speed}")

    def is_running(self):
        return self._running


# Replace the PINX and PINY with the pin numbers that match the wiring as shown
# in the "Pins of the Car" section in the documentation.
m1 = motor(19, 18)
m2 = motor(20, 21)
m3 = motor(6, 7)
m4 = motor(8, 9)


def stop():
    m1.stop()
    m2.stop()
    m3.stop()
    m4.stop()

def coast():
    m1.coast()
    m2.coast()
    m3.coast()
    m4.coast()

def brake():
    m1.brake()
    m2.brake()
    m3.brake()
    m4.brake()

def go(speed, howlong=None):
    m1.go(speed, howlong)
    m2.go(speed, howlong)
    m3.go(speed, howlong)
    m4.go(speed, howlong)

