import neopixel
import _thread
from machine import Pin, ADC
from time import sleep, sleep_us, time_ns


SOUND_SPEED = 0.034
CM_TO_INCH = 0.393701

# left sensor
p5_trigger = Pin(5, Pin.OUT)
p18_echo = Pin(18, Pin.IN)

# right sensor
p19_trigger = Pin(19, Pin.OUT)
p21_echo = Pin(21, Pin.IN)

# ledstrip single pixel
ledstrip = neopixel.NeoPixel(Pin(4), 1)

# distances list to hold each sensor distance
global distances
distances = [0, 0]

global ledcolor
ledcolor = [0,0,0]

# this is our measure function to measure the distance with our sensor
def measure(trig_pin : Pin, echo_pin : Pin) -> int:
        # "beep" for 10 microseconds
        trig_pin.off()
        sleep_us(2)
        trig_pin.on()
        sleep_us(10)
        trig_pin.off()

        # mark start time when the "beep" is over
        sent_pulse_time = time_ns()

        # do nothing while the "beep" didn't echo back from the "wall"
        while not echo_pin.value():
            pass
        
        # mark when we start get the the echo from the "wall"
        received_pulse_echo_time = time_ns() 

        # as long as the "beep" echos, update end time
        while echo_pin.value():
            received_pulse_echo_time = time_ns()

        # calculate the time difference
        duration = received_pulse_echo_time - sent_pulse_time

        # calculate the distance in cm using the speed of sound
        distanceCm = (duration / 2000) * SOUND_SPEED
        return distanceCm

def update_distance(trig_pin : Pin, echo_pin : Pin, distance_index : int) -> None:
    '''
    Using the sensor to measure distance in cm and update the distance list
    Where should we use it? 
    '''
    global distances
    distances[distance_index] = measure(trig_pin, echo_pin)
 
def run_ultrasonic_color_manipulation(trig_pin : Pin, echo_pin : Pin, color_index : int):
    '''
    This function should update the global variable "ledcolor" according to
    the supersonic sensor read. This function should update only one of the
    three color component (Red, Green, Blue).
    @param trig_pin: trigger pin of a sound sensor
    @param echo_pin: echo pin of the same sound sensor
    @param color_index: 0 = RED; 1 = GREEN; 2 = BLUE
    '''
    while True:
        color_index %= 3
        global ledcolor
        color_brightness = 0 # Change me to update the color when running! 
        # How can we do this every 0.1 seconds?
        ####### YOUR CODE FOR CALCULATING THE COLOR GOES HERE! #######
        
        ##############################################################
        ledcolor[color_index] = color_brightness


def main():
    global ledcolor
    # Those lines are running our sensone measure on background "in parallel". 
    # Don't touch those two
    _thread.start_new_thread(run_ultrasonic_color_manipulation, (p5_trigger, p18_echo, 0))
    _thread.start_new_thread(run_ultrasonic_color_manipulation, (p19_trigger, p21_echo, 1))

    # a loop to update our color using "distances" and "ledcolor"
    while True:
        sleep(0.5)
        # get the new color
        new_color = tuple(ledcolor)
        print(new_color)
        ########### YOUR'E LEDSTRIP ANIMATION CODE GOES HERE ############

        #################################################################
        ledstrip.write()

main()