# microbit-module: maqueen@0.1.0
"""Pure MicroPython driver for DFRobot Maqueen Lite V4 and V5.

Made for the BBC micro:bit Python Editor: https://python.microbit.org/
This module uses only modules included with micro:bit MicroPython.
Register map derived from DFRobot/pxt-maqueen 1.7.17 (MIT licensed).
"""

from microbit import i2c, pin1, pin2, pin8, pin12, pin13, pin14, pin15, pin16, sleep
import machine
import neopixel
import utime

_I2C_ADDRESS = 0x10


def _byte(value):
    value = int(value)
    if value < 0:
        return 0
    if value > 255:
        return 255
    return value


def _motor_register(motor):
    if motor == 0:
        return 0
    if motor == 1:
        return 2
    raise ValueError("motor must be LEFT, RIGHT, or ALL")


def _write(*values):
    i2c.write(_I2C_ADDRESS, bytearray(values))


def _read_register(register, length):
    i2c.write(_I2C_ADDRESS, bytearray([register]))
    return i2c.read(_I2C_ADDRESS, length)


def _ultrasonic_raw():
    # Reading first selects input mode and its default pull-down resistor.
    pin2.read_digital()
    pin1.write_digital(0)
    utime.sleep_us(2)
    pin1.write_digital(1)
    utime.sleep_us(10)
    pin1.write_digital(0)
    return machine.time_pulse_us(pin2, 1, 29000)


def _ultrasonic_cm_once():
    pulse = _ultrasonic_raw()
    if pulse < 0:
        return 500
    return (pulse + 29) // 59


def _ultrasonic_cm():
    # Median filter: measure three times and return the middle distance.
    # This removes one unusually short or long noisy measurement.
    first = _ultrasonic_cm_once()
    sleep(60)
    second = _ultrasonic_cm_once()
    sleep(60)
    third = _ultrasonic_cm_once()

    # Sorting puts the noisy low and high values on either side.
    distances = [first, second, third]
    distances.sort()
    shortest, middle, longest = distances
    return middle


class NECReceiver:
    """Small blocking NEC infrared decoder for the receiver on pin 16.

    read() waits for a packet and returns its command byte (0-255), or None on
    timeout/bad data. Repeat packets return the most recent command.
    """

    def __init__(self, pin=pin16):
        self.pin = pin
        self.last_command = None
        try:
            pin.set_pull(pin.NO_PULL)
        except AttributeError:
            pass

    def _pulse(self, level, timeout_us):
        return machine.time_pulse_us(self.pin, level, timeout_us)

    def read(self, timeout_ms=100):
        timeout_ms = max(1, int(timeout_ms))
        started = utime.ticks_ms()
        while utime.ticks_diff(utime.ticks_ms(), started) < timeout_ms:
            remaining = timeout_ms - utime.ticks_diff(utime.ticks_ms(), started)
            mark = self._pulse(0, max(1000, remaining * 1000))
            if not 8000 <= mark <= 10000:
                continue
            space = self._pulse(1, 5500)
            if 2000 <= space <= 2600:  # NEC repeat packet
                self._pulse(0, 1000)
                return self.last_command
            if not 4000 <= space <= 5000:
                continue

            raw = 0
            valid = True
            for bit in range(32):
                mark = self._pulse(0, 1200)
                space = self._pulse(1, 2500)
                if not 350 <= mark <= 800 or not 300 <= space <= 2000:
                    valid = False
                    break
                if space > 1000:
                    raw |= 1 << bit
            if not valid:
                continue

            address = raw & 0xFF
            address_inverse = (raw >> 8) & 0xFF
            command = (raw >> 16) & 0xFF
            command_inverse = (raw >> 24) & 0xFF
            if (address ^ address_inverse) != 0xFF:
                continue
            if (command ^ command_inverse) != 0xFF:
                continue
            self.last_command = command
            return command
        return None


class _Common:
    LEFT = 0
    RIGHT = 1
    ALL = 2
    FORWARD = 0
    BACKWARD = 1
    SERVO_1 = 0
    SERVO_2 = 1

    def __init__(self):
        self._pixels = None
        self._ir = None

    def connected(self):
        """Return True when the Maqueen answers on the I2C bus."""
        try:
            return _I2C_ADDRESS in i2c.scan()
        except OSError:
            return False

    def motor_run(self, motor, direction, speed):
        speed = _byte(speed)
        direction = 1 if direction else 0
        if motor == self.ALL:
            _write(0, direction, speed)
            _write(2, direction, speed)
        else:
            _write(_motor_register(motor), direction, speed)

    def motor(self, motor, speed):
        """Run one/both motors with signed speed from -255 to 255."""
        speed = int(speed)
        direction = self.BACKWARD if speed < 0 else self.FORWARD
        self.motor_run(motor, direction, abs(speed))

    def arcade_drive(self, throttle, steering):
        """Drive using signed throttle and steering values from -255 to 255.

        Positive throttle moves forward. Positive steering turns right.
        """
        throttle = max(-255, min(255, int(throttle)))
        steering = max(-255, min(255, int(steering)))
        left_speed = max(-255, min(255, throttle + steering))
        right_speed = max(-255, min(255, throttle - steering))
        self.motor(self.LEFT, left_speed)
        self.motor(self.RIGHT, right_speed)

    def motor_stop(self, motor=ALL):
        self.motor_run(motor, self.FORWARD, 0)

    def servo_run(self, servo, angle):
        if servo not in (self.SERVO_1, self.SERVO_2):
            raise ValueError("servo must be SERVO_1 or SERVO_2")
        angle = max(0, min(180, int(angle)))
        _write(20 + servo, angle)

    def ultrasonic(self):
        """Return a filtered distance in cm; 500 means no echo in range.

        The median of three measurements removes most isolated false echoes.
        """
        return _ultrasonic_cm()

    def ultrasonic_raw(self):
        """Return the unrounded echo duration in microseconds.

        A result of -2 means no echo started before the timeout; -1 means the
        echo did not finish before the timeout.
        """
        return _ultrasonic_raw()

    def read_version(self):
        length = _read_register(50, 1)[0]
        if not 0 < length <= 32:
            return ""
        data = _read_register(51, length)
        return "".join(chr(value) for value in data)

    def pixels(self):
        """Return the four WS2812/NeoPixel ambient lights on pin 15."""
        if self._pixels is None:
            self._pixels = neopixel.NeoPixel(pin15, 4)
        return self._pixels

    def pixels_fill(self, red, green, blue):
        strip = self.pixels()
        color = (_byte(red), _byte(green), _byte(blue))
        for index in range(4):
            strip[index] = color
        strip.show()

    def pixels_off(self):
        self.pixels_fill(0, 0, 0)

    def ir_read(self, timeout_ms=100):
        """Read an NEC remote command from pin 16, or return None."""
        if self._ir is None:
            self._ir = NECReceiver()
        return self._ir.read(timeout_ms)


class Maqueen(_Common):
    """Maqueen Lite V1-V4 (the MakeCode extension calls this "V4")."""

    LINE_LEFT = 13
    LINE_RIGHT = 14
    LED_LEFT = 8
    LED_RIGHT = 12

    def read_patrol(self, sensor):
        if sensor in (self.LINE_LEFT, self.LEFT):
            return pin13.read_digital()
        if sensor in (self.LINE_RIGHT, self.RIGHT):
            return pin14.read_digital()
        raise ValueError("sensor must be LINE_LEFT or LINE_RIGHT")

    def write_led(self, led, on):
        if led in (self.LED_LEFT, self.LEFT):
            pin8.write_digital(1 if on else 0)
        elif led in (self.LED_RIGHT, self.RIGHT):
            pin12.write_digital(1 if on else 0)
        else:
            raise ValueError("led must be LED_LEFT or LED_RIGHT")


class MaqueenV5(_Common):
    """Maqueen Lite V5."""

    LINE_LEFT = 1
    LINE_MIDDLE = 2
    LINE_RIGHT = 3

    RGB_LEFT = 0
    RGB_RIGHT = 1
    RGB_ALL = 2

    RED = 1
    GREEN = 2
    YELLOW = 3
    BLUE = 4
    PURPLE = 5
    CYAN = 6
    WHITE = 7
    BLACK = 8

    ALKALINE = 1
    LITHIUM = 0

    def i2c_init(self, timeout_ms=5000):
        """Reset V5 control firmware and wait until it answers.

        Return True when ready, False after timeout. Use timeout_ms=None to
        wait forever, matching the MakeCode I2CInit block.
        """
        started = utime.ticks_ms()
        try:
            _write(70, 1)
        except OSError:
            pass
        sleep(100)
        while True:
            try:
                if _read_register(50, 1)[0]:
                    return True
            except OSError:
                pass
            if timeout_ms is not None:
                if utime.ticks_diff(utime.ticks_ms(), started) >= timeout_ms:
                    return False
            sleep(100)

    def patrolling(self, on):
        """Enable or disable the V5 controller's built-in line following."""
        _write(71, 1 if on else 0)

    def patrol_speed(self, grade):
        """Set built-in line-following speed grade (firmware register 72).

        This register exists in pxt-maqueen, but its block/function is
        commented out in the current v1.7.17 source.
        """
        grade = int(grade)
        if grade < 1 or grade > 3:
            raise ValueError("grade must be 1, 2, or 3")
        _write(72, grade)

    def read_patrol(self, sensor):
        if sensor not in (self.LINE_LEFT, self.LINE_MIDDLE, self.LINE_RIGHT):
            raise ValueError("invalid V5 line sensor")
        state = _read_register(29, 1)[0]
        return 1 if state & (1 << (3 - sensor)) else 0

    def read_patrol_data(self, sensor):
        registers = (0, 32, 34, 36)
        if sensor not in (self.LINE_LEFT, self.LINE_MIDDLE, self.LINE_RIGHT):
            raise ValueError("invalid V5 line sensor")
        data = _read_register(registers[sensor], 2)
        return (data[0] << 8) | data[1]

    def set_rgb_led(self, light, color):
        if color < self.RED or color > self.BLACK:
            raise ValueError("invalid car-light color")
        if light in (self.RGB_LEFT, self.RGB_ALL):
            _write(11, color)
        if light in (self.RGB_RIGHT, self.RGB_ALL):
            _write(12, color)
        if light not in (self.RGB_LEFT, self.RGB_RIGHT, self.RGB_ALL):
            raise ValueError("invalid car light")

    def set_rgb_blink(self, light, flashes, grade, color):
        self.set_rgb_led(light, color)
        flashes = _byte(flashes)
        grade = int(grade)
        if grade < 1 or grade > 5:
            raise ValueError("grade must be from 1 to 5")
        if light in (self.RGB_LEFT, self.RGB_ALL):
            _write(14, grade)
            _write(13, flashes)
        if light in (self.RGB_RIGHT, self.RGB_ALL):
            _write(16, grade)
            _write(15, flashes)

    def set_rgb_change(self, light, grade):
        grade = int(grade)
        if grade < 1 or grade > 5:
            raise ValueError("grade must be from 1 to 5")
        if light in (self.RGB_LEFT, self.RGB_ALL):
            _write(17, grade)
        if light in (self.RGB_RIGHT, self.RGB_ALL):
            _write(18, grade)
        if light not in (self.RGB_LEFT, self.RGB_RIGHT, self.RGB_ALL):
            raise ValueError("invalid car light")

    def set_rgb_off(self, light=RGB_ALL):
        self.set_rgb_led(light, self.BLACK)

    def read_light_intensity(self, light):
        if light == self.RGB_LEFT:
            register = 41
        elif light == self.RGB_RIGHT:
            register = 43
        else:
            raise ValueError("light must be RGB_LEFT or RGB_RIGHT")
        data = _read_register(register, 2)
        return (data[0] << 8) | data[1]

    def get_battery_data(self, battery_type=ALKALINE):
        if battery_type not in (self.ALKALINE, self.LITHIUM):
            raise ValueError("invalid battery type")
        _write(45, battery_type)
        sleep(50)
        level = _read_register(46, 1)[0]
        return min(level, 100)