import time
import board
import digitalio
import busio
import pwmio
import adafruit_hcsr04


#UART setup
#                 UART0 TX    UART0 RX
uart = busio.UART(board.GP16, board.GP17, baudrate = 9600, receiver_buffer_size = 1024)

#declaración ultrasonico izquierdo
trig_pin1 = board.GP10
echo_pin1= board.GP11
ultrasonic1 = adafruit_hcsr04.HCSR04(trig_pin1, echo_pin1)

#declaración ultrasonico enmedio
trig_pin2=board.GP12
echo_pin2=board.GP13
ultrasonic2 = adafruit_hcsr04.HCSR04(trig_pin2, echo_pin2)

#declaración ultrasonico derecho
trig_pin3=board.GP14
echo_pin3=board.GP15
ultrasonic3 = adafruit_hcsr04.HCSR04(trig_pin3, echo_pin3)

switch_pin = digitalio.DigitalInOut(board.GP2)  
switch_pin.direction = digitalio.Direction.INPUT
switch_pin.pull = digitalio.Pull.DOWN

m_i = pwmio.PWMOut(board.GP0, frequency = 1000)
m_d= pwmio.PWMOut(board.GP1, frequency = 1000)

def bluetooth():
    mi=0
    md=0
    def send_data(uart, data):
        uart.write(bytes(data + '\n', 'utf-8')) #Send data as bytes


    def receive_data(uart):
        if uart.in_waiting > 0:
            received_bytes = uart.read(uart.in_waiting)    #Read all available bytes
            received_string = str(received_bytes, 'utf-8') #Convert bytes to string  
            return received_string.strip()                 #Remove any whitespace characters
        return None

    #Main loop

    while True:
        #Receive data
        received = receive_data(uart)
        
        if received:
            print("Received: ", received)
            if received == "IZ":
                mi = 65535
                md = 5000
            elif received == "DER":
                mi = 5000
                md= 65535
            elif received == "ADE":
                mi = 65535
                md = 65535
            elif received == "STO":
                mi = 0
                md = 0
            else:
                mi= 0
                md= 0
                
            m_i.duty_cycle = mi
            m_d.duty_cycle = md
            time.sleep(0.25)
        if switch_pin.value:
        break;

def distance ():
    mi=0
    md=0
    
    while True:
        try:
            distance1 = ultrasonic1.distance #distancia sensor derecha 
            distance2 = ultrasonic2.distance #distancia sensor izquierda
            distance3 = ultrasonic3.distance #distancia sensor enmedio

            print("Distancia sensor  derecha: {:.1f} cm".format(distance1))
            print("Distancia sensor  izquierda: {:.1f} cm".format(distance2))
            print("Distancia sensor centro: {:.1f} cm".format(distance3))
            print("\n")

            #detecta algo a la derecha
            if distance1<distance2 and distance1<distance3:
                mi = 5000 
                md = 65535

            #detecta algo a la izquierda
            elif distance2 <distance3 and distance2<distance1:
                mi = 65535 
                md = 5000
            
            #detecta algo en medio 
            elif distance3 <distance2 and distance3<distance1:
                mi = 65535
                md = 65535
            
            #no detecta nada
            elif distance1 >50 and distance2 > 50 and distance3:
                mi = 0
                md = 0

            m_i.duty_cycle = mi
            m_d.duty_cycle = md
            break
            
        except RuntimeError:
            print("Recargando...!")
            pass

while True:
    if switch_pin.value:
        print("1")
        bluetooth()  # Run code A
    else:
        print("0")
        distance()  # Run code B

    time.sleep(0.4)