# import necessary libraries
from microbit import *
import gc
from machine import time_pulse_us

# define here your Joy Car Mainboard Revision
joycar_rev = 1.3

# initialize I2C interface for the Joy Car Mainboard
i2c.init(freq=400000, sda=pin20, scl=pin19)

# initialize PWM controller
i2c.write(0x70, b'\x00\x01')
i2c.write(0x70, b'\xE8\xAA')

# the deceleration of a motor bias can be used to compensate for different motor speeds
biasR = 0  # deceleration of the right motor in percent
biasL = 0  # deceleration of the left motor in percent

# control motors using the PWM controller
# PWM0 and PWM1 for the left motor and PWM2 and PWM3 for the right motor
def drive(PWM0, PWM1, PWM2, PWM3):
    # The scale function is used to rescale the bias variables for
    # the calculation of the motor speed
    def scale(num, in_min, in_max, out_min, out_max):
        return (num - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

    # Scaling of the deceleration value to the value in percent
    PWM0 = int(PWM0 * (scale(biasR, 0, 100, 100, 0) / 100))
    PWM1 = int(PWM1 * (scale(biasR, 0, 100, 100, 0) / 100))
    PWM2 = int(PWM2 * (scale(biasL, 0, 100, 100, 0) / 100))
    PWM3 = int(PWM3 * (scale(biasL, 0, 100, 100, 0) / 100))

    # transmit value for PWM channel (0-255) to PWM controller
    # 0x70 is the I2C address of the controller.
    # the byte with the PWM value is added to the byte for the channel
    i2c.write(0x70, b'\x02' + bytes([PWM0]))
    i2c.write(0x70, b'\x03' + bytes([PWM1]))
    i2c.write(0x70, b'\x04' + bytes([PWM2]))
    i2c.write(0x70, b'\x05' + bytes([PWM3]))

# get all sensor data
def fetchSensorData():
    # Since the zfill function is not included in micro:bit Micropython,
    # it must be inserted as a function
    def zfill(s, width):
        return '{:0>{w}}'.format(s, w=width)

    # Read hexadecimal data and convert to binary
    data = "{0:b}".format(ord(i2c.read(0x38, 1)))
    # fill in the data to 8 digits if necessary
    data = zfill(data, 8)
    # declare bol_data_dict as dictionary
    bol_data_dict = {}
    # Counter for the loop that enters the data from data into bol_data_dict
    bit_count = 7
    # Transfer the data from data to bol_data_dict
    for i in data:
        if i == "0":
            bol_data_dict[bit_count] = False
            bit_count -= 1
        else:
            bol_data_dict[bit_count] = True
            bit_count -= 1

    # after main board revision 1.3, the speed sensors are on separate pins
    if joycar_rev >= 1.3:
        bol_data_dict[8], bol_data_dict[9] = bol_data_dict[0], bol_data_dict[1]
        bol_data_dict[0] = bool(pin14.read_digital())
        bol_data_dict[1] = bool(pin15.read_digital())

    # bit 0 = SpeedLeft, bit 1 = SpeedRight, bit 2 = LineTrackerLeft,
    # bit 3 = LineTrackerMiddle, bit 4 = LineTrackerRight,
    # bit 5 = ObstclLeft, bit 6 = ObstclRight, bit 7 = free pin(7)
    # (bit 8 = free (pin0) bit 9 = free (pin1)) - just with revision 1.3 or newer
    return bol_data_dict

# define pins for ultrasonic sensor
trigger = pin8
echo = pin12

# initialize pins for ultrasonic sensor
trigger.write_digital(0)
echo.read_digital()

# method to calculate distance from ultrasonic sensor
def get_distance():
    # collect garbage
    gc.collect()
    # set short impulse onto the trigger pin
    trigger.write_digital(1)
    trigger.write_digital(0)
    # meassure time until echo pin is high
    duration = time_pulse_us(echo, 1)
    # calculate distance
    distance = ((duration / 1000000) * 34300) / 2
    # return the distance rounded to 2 decimal digits
    return round(distance, 2)

# setup pins for servomotor
pin1.set_analog_period(10)
pin13.set_analog_period(10)

# method to change position to servo motors
def servo(channel, position):
    # method to scale from 0-180 (°) to 100-200 (us)
    def scale(num, in_min, in_max, out_min, out_max):
        # Rückgabe des auf eine ganze Zahl gerundeten Werts
        return (round((num - in_min) * (out_max - out_min) /
                (in_max - in_min) + out_min))
    # check if position is in range
    if position < 0 and position > 180:
        return "position not in range"
    # send position to selected channel
    if channel == 1:
        pin1.write_analog(scale(position, 0, 180, 100, 200))
    elif channel == 2:
        pin13.write_analog(scale(position, 0, 180, 100, 200))

while True:
    # get data from IO expander
    sensor_data = fetchSensorData()

    # check if both sensors detect an obstacle
    if sensor_data[5] is False and sensor_data[6] is False:
        # evade obstacle
        drive(255, 40, 40, 255)
        sleep(500)
    # check if obstacle is detected on the left
    elif sensor_data[5] is False and sensor_data[6] is True:
        # evade obstacle
        drive(255, 40, 0, 0)
        sleep(500)
    # check if obstacle is detected on the right
    elif sensor_data[6] is False and sensor_data[5] is True:
        # evade obstacle
        drive(0, 0, 255, 40)
        sleep(500)
    else:
        # drive forwards
        drive(40, 255, 40, 255)

    # check if there is more than 20cm otherwise do this rotine
    if get_distance() < 20:
        # stop the JoyCar
        drive(0, 0, 0, 0)
        sleep(500)
        # set servomotor to the far right an dmeasure distance
        servo(1, 0)
        sleep(500)
        distance_right = get_distance()
        # set servomotor to the far left an dmeasure distance
        servo(1, 180)
        sleep(500)
        distance_left = get_distance()
        # set servomotor back in the middle
        servo(1, 90)
        sleep(500)
        # if on the left side is less space than on the right
        if distance_left < distance_right:
            # turn right
            drive(255, 0, 0, 255)
            sleep(500)
        # if on the right side is less space than on the left
        else:
            # turn left
            drive(0, 255, 255, 0)
            sleep(500)