import time
import numpy as np

import board
import busio
import digitalio
from adafruit_servokit import ServoKit
import adafruit_ads1x15.ads1015 as ADS
from adafruit_ads1x15.analog_in import AnalogIn

if __name__ == "__main__":
    mux_io = np.empty(4, dtype=object)
    mux_io[0] = digitalio.DigitalInOut(board.D17)
    mux_io[1] = digitalio.DigitalInOut(board.D27)
    mux_io[2] = digitalio.DigitalInOut(board.D22)
    mux_io[3] = digitalio.DigitalInOut(board.D23)

    mux_io = np.empty(4, dtype=object)
    for ii, io in mux_io:
        io.switch_to_output()
        mux[ii] = io.value

    i2c = busio.I2C(board.SCL, board.SDA)
    adc = ADS.ADS1015(i2c)
    adc_mux = AnalogIn(adc, ADS.P0)

    def get_reflectivity(chan):
        chan = int(chan)
        global mux
        global adc_mux
        mux = 1-np.array(list(f"{chan:04b}"), dtype=int)
        return adc_mux.voltage

    while True:
        for ii in range(1):
            print(f"{get_reflectivity(ii):1.2f}\t", end="")
        print()

    # servos = ServoKit(channels=16).continuous_servo
    # servos[0].throttle = 0
    # servos[1].throttle = 0
    # servos[2].throttle = 0
    # time.sleep(1)
    # servos[0].throttle = 20
    # servos[1].throttle = 20
    # servos[2].throttle = 20
    # time.sleep(1)
    # servos[0].throttle = 0
    # servos[1].throttle = 0
    # servos[2].throttle = 0