# Initialization and Commands for DFRobot Macqueen
# Collected and inferred by Ido Gendel, 2020
# MU Editor (tested on version 1.0.2)
# Share and enjoy!

from microbit import *
from utime import *
from neopixel import *

# This is critical for IR reception!
pin16.set_pull(pin16.NO_PULL)
# Required for communication with motor chip
i2c.init()
# Pixel 0=Front/Left, 1=B/L, 2=B/R, 3=F/R
# Usage: pixels[n] = (R, G, B) / pixels.clear(), pixels.show()
pixels = NeoPixel(pin15, 4)

# Motor: 0 = left, 2 = right
# Direction: 0 = Forward, 1 = Backward
# Speed: 0 stop to 255 full
def sendMotorCmd(motor, direction, speed):
    i2c.write(0x10, bytes([motor, direction, speed]))


# Servo number is 1 or 2 (as marked on robot)
# Angle 0 - 180 inclusive
def sendServoCmd(servo, angle):
    addr = 0x13 + servo
    i2c.write(0x10, bytes([addr, angle]))


def frontLights(right, left):
    pin12.write_digital(right)
    pin8.write_digital(left)


def horn(frequency):
    if frequency == 0:
        pin0.write_digital(0)
        return
    else:
        pin0.set_analog_period_microseconds(1000000 // frequency)
        pin0.write_analog(512)


# Returns (right, left) reading
def getLineSensorsReading():
    return (pin14.read_digital(), pin13.read_digital())


def getRange():
    # Trigger
    pin1.write_digital(1)
    sleep_us(20)
    pin1.write_digital(0)
    # Wait for Echo start
    while pin2.read_digital() == 0:
        pass
    # Measure Echo
    t0 = ticks_us()
    while pin2.read_digital() == 1:
        pass
    # Result is returned in cm
    return (ticks_us() - t0) / 58.77


def getRawIRReading():
    return pin16.read_digital()

# Demo code from here on

display.show(Image.HAPPY)
pixels[0] = (0, 30, 0)
pixels[1] = (50, 0, 0)
pixels[2] = (50, 0, 0)
pixels[3] = (0, 30, 0)
pixels.show()

while True:
    if 0 == getRawIRReading():
        frontLights(1, 1)
        sleep(0.5)
    else:
        frontLights(0, 0)