from machine import Pin, time_pulse_us
import time

# Pins for Sensor 1
ECHO_PIN1 = Pin(27, Pin.IN)
TRIG_PIN1 = Pin(28, Pin.OUT)

# Pins for Sensor 2
ECHO_PIN2 = Pin(4, Pin.IN)
TRIG_PIN2 = Pin(6, Pin.OUT)

# Pins for Sensor 3
ECHO_PIN3 = Pin(20, Pin.IN)
TRIG_PIN3 = Pin(22, Pin.OUT)

LEDPIN1 = Pin(5, Pin.OUT)
LEDPIN2 = Pin(13, Pin.OUT)
LEDPIN3 = Pin(12, Pin.OUT)

def read_distance_cm(trig_pin, echo_pin):
    trig_pin.value(0)
    time.sleep_us(2)
    trig_pin.value(1)
    time.sleep_us(10)
    trig_pin.value(0)
    duration = time_pulse_us(echo_pin, 1, 20000)  # 20,000 us (20 ms) timeout
    if duration > 0:
        return duration * 0.034 / 2
    else:
        return -1  # Error condition, no echo received within timeout

def toggle_led(led, distance):
    if distance > 200:
        led.value(1)
    else:
        led.value(0)

while True:
    distance1 = read_distance_cm(TRIG_PIN1, ECHO_PIN1)
    distance2 = read_distance_cm(TRIG_PIN2, ECHO_PIN2)
    distance3 = read_distance_cm(TRIG_PIN3, ECHO_PIN3)

    is_nearby1 = distance1 > 200
    LEDPIN1.value(is_nearby1)

    is_nearby2 = distance2 > 200
    LEDPIN2.value(is_nearby2)

    is_nearby3 = distance3 > 200
    LEDPIN3.value(is_nearby3)

    print("Distance 1: {:.2f} cm".format(distance1))
    print("Distance 2: {:.2f} cm".format(distance2))
    print("Distance 3: {:.2f} cm".format(distance3))
    
    time.sleep(0.1)  # Sleep for 100ms (adjust as needed)