from machine import Pin
import time

# Define LED pins
red_leds = [Pin(26, Pin.OUT), Pin(19, Pin.OUT), Pin(32, Pin.OUT), Pin(17, Pin.OUT)]
yellow_leds = [Pin(25, Pin.OUT), Pin(18, Pin.OUT), Pin(3, Pin.OUT), Pin(16, Pin.OUT)]  # Swapped 16 and 3
green_leds = [Pin(33, Pin.OUT), Pin(5, Pin.OUT), Pin(21, Pin.OUT), Pin(15, Pin.OUT)]

# Define sensor pins
trigger_pins = [Pin(13, Pin.OUT), Pin(27, Pin.OUT), Pin(22, Pin.OUT), Pin(2, Pin.OUT)]
echo_pins = [Pin(12, Pin.IN), Pin(14, Pin.IN), Pin(23, Pin.IN), Pin(4, Pin.IN)]

# Define number of signals/sensors
num_signals = 4

# Light durations
smart_green_duration = 10  # seconds
smart_yellow_duration = 2  # seconds

# Function to read distance from ultrasonic sensor
def read_distance(trigger_pin, echo_pin):
    trigger_pin.off()
    time.sleep_us(2)
    trigger_pin.on()
    time.sleep_us(10)
    trigger_pin.off()

    # Wait for the echo signal to go high, with timeout
    timeout_start = time.ticks_ms()
    while echo_pin.value() == 0:
        if time.ticks_diff(time.ticks_ms(), timeout_start) > 100:  # 100ms timeout
            return -1  # Return -1 if timeout occurs (indicates no reading)

    start_time = time.ticks_us()

    # Wait for the echo signal to go low, with timeout
    timeout_start = time.ticks_ms()
    while echo_pin.value() == 1:
        if time.ticks_diff(time.ticks_ms(), timeout_start) > 100:  # 100ms timeout
            return -1  # Return -1 if timeout occurs

    end_time = time.ticks_us()
    duration = time.ticks_diff(end_time, start_time)
    distance = (duration * 0.034) / 2  # Speed of sound is 0.034 cm/µs
    return distance

# Function to turn off all LEDs
def reset_leds():
    for i in range(num_signals):
        red_leds[i].off()
        yellow_leds[i].off()
        green_leds[i].off()

# Function to turn on red lights for all signals except the active one
def activate_red_lights(active_index):
    for i in range(num_signals):
        if i != active_index:
            red_leds[i].on()
        else:
            red_leds[i].off()

# Function to operate in smart signal mode
def smart_signal_mode(distances, previous_min_index):
    valid_distances = [dist if dist != -1 else float('inf') for dist in distances]

    # Check if all distances are invalid or show no traffic
    if all(d == float('inf') for d in valid_distances):
        print("No valid readings from any sensor. Cycling to the next signal.")
        next_index = (previous_min_index + 1) % num_signals
        reset_leds()
        activate_red_lights(next_index)
        green_leds[next_index].on()
        time.sleep(smart_green_duration)
        return next_index

    # Find the signal with the closest vehicle (minimum valid distance)
    min_index = valid_distances.index(min(valid_distances))
    
    # Check if the current active signal has traffic
    if previous_min_index != -1 and valid_distances[previous_min_index] < 50:  # Threshold for "traffic"
        print(f"Signal {previous_min_index + 1} remains active due to traffic.")
        reset_leds()
        activate_red_lights(previous_min_index)
        green_leds[previous_min_index].on()
        time.sleep(smart_green_duration)
        return previous_min_index

    # Transition to a new signal if no traffic at the current signal
    if min_index != previous_min_index:
        print(f"Transitioning to Signal {min_index + 1} due to closer vehicle.")
        if previous_min_index != -1:
            green_leds[previous_min_index].off()  # Turn off green LED
            yellow_leds[previous_min_index].on()  # Turn on yellow LED
            time.sleep(smart_yellow_duration)
            yellow_leds[previous_min_index].off()  # Turn off yellow LED

    reset_leds()
    activate_red_lights(min_index)
    green_leds[min_index].on()
    time.sleep(smart_green_duration)
    return min_index

# Main loop
previous_min_index = -1  # Tracks the last active signal in smart mode

while True:
    distances = []

    # Read distances from all sensors
    for i in range(num_signals):
        time.sleep_ms(50)  # Delay to prevent interference
        distance = read_distance(trigger_pins[i], echo_pins[i])
        if distance == -1:
            print(f"Sensor {i + 1}: No Reading or Timeout")
            distances.append(-1)  # No reading
        else:
            print(f"Sensor {i + 1}: Distance = {distance:.2f} cm")
            distances.append(distance)

    # Operate in smart signal mode
    print("Operating in Smart Signal Mode")
    previous_min_index = smart_signal_mode(distances, previous_min_index)

    time.sleep(1)  # Small delay for readability