from machine import Pin, time_pulse_us
import time

# Setup GPIO pins
TRIG_PIN = 15  # Trigger pin for ultrasonic sensor
ECHO_PIN = 14  # Echo pin for ultrasonic sensor
STOP_LIGHT_PIN = 13  # GPIO pin for the stop light LED

# Initialize the pins
trigger = Pin(TRIG_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)  # Initialize echo pin
stop_light = Pin(STOP_LIGHT_PIN, Pin.OUT)

def measure_distance():
    # Send a 10us pulse to trigger
    trigger.low()  # Ensure trigger is low initially
    time.sleep_us(2)  # Wait for 2us
    trigger.high()  # Trigger the ultrasonic sensor
    time.sleep_us(10)  # Keep the trigger high for 10us
    trigger.low()  # Set trigger back to low
    
    # Measure the duration of the echo signal
    duration = time_pulse_us(echo, 1)  # Measure time for echo to go high
    distance = duration * 0.0343 / 2  # Convert duration to distance in cm

    return distance

def control_stop_light(distance):
    print(f"Measured distance: {distance} cm")  # Print the distance for debugging
    
    # Turn on the stop light if the distance is less than the user-defined limit and not 30 cm
    if distance < user_distance and distance != 30:
        stop_light.high()  # Turn on the stop light
        print("Stop light ON")  # Debug message for stop light ON
    else:
        stop_light.low()  # Turn off the stop light
        print("Stop light OFF")  # Debug message for stop light OFF

# Main loop
try:
    time.sleep(0.1)  # Wait for USB to become ready
    print("Hello, Pi Pico!")

    while True:
        try:
            # Prompt user for distance limit
            user_distance = float(input("Enter the distance limit (in cm) for the stop light: "))
            break  # Break the loop if input is valid
        except ValueError:
            print("Invalid input. Please enter a number.")  # Notify user of invalid input

    while True:
        distance = measure_distance()
        control_stop_light(distance)

        time.sleep(0.5)  # Adjust the sleep time as needed

except KeyboardInterrupt:
    print("Program stopped.")
    stop_light.low()  # Ensure the stop light is off