from machine import Pin, time_pulse_us
import utime

# Define the GPIO pins for the ultrasonic sensors
TRIG_US1 = 3
ECHO_US1 = 2
TRIG_US2 = 5
ECHO_US2 = 4
TRIG_US3 = 7
ECHO_US3 = 6

# Define pico rx and tx pins
PICO_RX = 14
PICO_TX = 15

# Create pin objects
trig_us1 = Pin(TRIG_US1, Pin.OUT)
echo_us1 = Pin(ECHO_US1, Pin.IN)
trig_us2 = Pin(TRIG_US2, Pin.OUT)
echo_us2 = Pin(ECHO_US2, Pin.IN)
trig_us3 = Pin(TRIG_US3, Pin.OUT)
echo_us3 = Pin(ECHO_US3, Pin.IN)
pico_rx = Pin(PICO_RX, Pin.IN)
pico_tx = Pin(PICO_TX, Pin.OUT)

def measure_distance(trig, echo):
    trig.low()
    utime.sleep_us(2)
    trig.high()
    utime.sleep_us(10)
    trig.low()
    duration = time_pulse_us(echo, 1, 30000)
    distance = (duration / 2) * 0.0343
    return distance

def send_pulses(pulses, delay_ms=167):
    for _ in range(pulses):
        pico_tx.high()
        utime.sleep_ms(delay_ms)
        pico_tx.low()
        utime.sleep_ms(delay_ms)

def receive_pulses(expected_pulses, timeout=5000):
    received_pulses = 0
    start_time = utime.ticks_ms()
    while utime.ticks_diff(utime.ticks_ms(), start_time) < timeout:
        if pico_rx.value() == 1:
            received_pulses += 1
            while pico_rx.value() == 1:
                pass  # Wait for the pulse to end
        if received_pulses == expected_pulses:
            return True
    return False

while True:
    start_time = utime.ticks_ms()
    
    distance_us1 = measure_distance(trig_us1, echo_us1)
    distance_us2 = measure_distance(trig_us2, echo_us2)
    distance_us3 = measure_distance(trig_us3, echo_us3)
    
    print("US1: {:.2f} cm, US2: {:.2f} cm, US3: {:.2f} cm".format(distance_us1, distance_us2, distance_us3))
    
    if distance_us1 < 200:
        print("Object detected")
        send_pulses(3)
        if receive_pulses(3):
            print("Start collecting")
            if receive_pulses(3):
                print("Start collecting")
            else:
                send_pulses(3)
        else:
            send_pulses(3)
    
    if distance_us2 < 300:
        print("Collision detected")
        send_pulses(5)
        if receive_pulses(5):
            print("Turning left")
            if receive_pulses(5):
                print("Turning left")
            else:
                send_pulses(5)
        else:
            send_pulses(5)
    
    if distance_us3 < 200:
        print("Capacity full")
        send_pulses(7)
        if receive_pulses(7):
            print("Start collecting")
            if receive_pulses(7):
                print("Start collecting")
            else:
                send_pulses(7)
        else:
            send_pulses(7)
    
    elapsed_time = utime.ticks_diff(utime.ticks_ms(), start_time)
    if elapsed_time < 1000:
        utime.sleep_ms(1000 - elapsed_time)