# Write your code here :-)
# https://github.com/adafruit/Adafruit_CircuitPython_VL53L1X/blob/main/adafruit_vl53l1x.py
# https://github.com/adafruit/Adafruit_CircuitPython_BusDevice/blob/main/adafruit_bus_device/i2c_device.py
import time
import board
import busio
import digitalio
import adafruit_vl53l1x
from led import LED
from i2cperipheral import I2CPeripheral


fault = False
led = LED()
led.rainbow_cycle(0, halt='crit')
I2CP = None


try:
    i2c = busio.I2C(board.SCL, board.SDA, frequency=400000)
except Exception as err:
    print(err)
    fault = True


if not fault:
    sensors = {
        'down': {
            'pin': None,  # Remember this one will always be active so we set it first to get it off the default.
            'address': 0x27
        },
        'up':{
            'pin': board.D0,
            'address': 0x2b
        },
        'left': {
            'pin': board.D1,
            'address': 0x2d
        },
        'right': {
            'pin': board.D2,
            'address': 0x2f
        },
        'front': {
            'pin': board.D3,
            'address': 0x31
        },
        'back': {
            'pin': board.D6,
            'address': 0x33
        }
    }

    led.rainbow_cycle(0, halt='cool')
    try:
        for sensor in sensors:  # Configure pins and set them low.
            pin = sensors[sensor]['pin']
            if pin is not None:
                pin = digitalio.DigitalInOut(pin)
                pin.switch_to_output(value=False)
                sensors[sensor]['pin'] = pin
    except Exception as err:
        fault = True
        print(err)

# Configure devices.

def set_addr(addr, pin=None):
    if pin is not None:
        pin.value = True
    dev = adafruit_vl53l1x.VL53L1X(i2c)
    dev.set_address(addr)
    return dev

# Set addresses.

if not fault:
    led.rainbow_cycle(0, halt='warn')
    try:
        sensors['down']['device'] = set_addr(sensors['down']['address'])  # Set the first sensor's address as it's the only one not powered down.
    except Exception as err:
        fault = True
        print('configuring down pin', err)


if not fault:
    for sensor in sensors:
        device = sensors[sensor]
        pin = device['pin']
        address = device['address']
        if pin is not None:
            led.rainbow_cycle(0, halt='warn')
            try:
                lidar = set_addr(address, pin)
                device['device'] = lidar
            except Exception as err:
                led.rainbow_cycle(0, halt='fail')
                fault = True
                print('configuring pins', err)

# Check configuration.

if not fault:
    for sensor in sensors:
        led.rainbow_cycle(0, halt='err')
        print('checking', sensor)
        try:
            device = sensors[sensor]
            lidar = device['device']
            lidar.start_ranging()
            while not lidar.data_ready:
                pass
            reading = lidar.distance
            if reading is None:
                print('sensor failed to send data')
                break
            print(reading)
            lidar.stop_ranging()
        except Exception as err:
            fault = True
            print('configuration check', err)
        lidar.clear_interrupt()
        pin.deinit()

sensors = None

while not i2c.try_lock():
    pass
print('scanning bus', i2c.scan())

if not fault:
    try:
        i2c.unlock()
        i2c.deinit()
        # Become i2c slave here.
        I2CP = I2CPeripheral(board.SCL, board.SDA, (0x6e, 0x6f))
    except Exception as err:
        led.rainbow_cycle(0, halt='release')
        fault = True
        print('unlock', err)

if not fault:
    led.rainbow_cycle(0, halt='good')
    print('deviced ready to go, changing to peripheral')