import time
from machine import Pin, PWM

# Pin Assignments
pir1 = Pin(4, Pin.IN)
pir2 = Pin(5, Pin.IN)

led_red = Pin(21, Pin.OUT)     # Red LED
led_yellow = Pin(19, Pin.OUT)  # Yellow LED
led_green = Pin(18, Pin.OUT)   # Green LED

servo = PWM(Pin(13), freq=50)

# Servo Positions
ANGLE_CLOSED = 25  # 0 degrees (Gate Closed)
ANGLE_OPEN = 75    # 90 degrees (Gate Open)

# Increased duration for each LED stage (13 seconds)
STEP_TIME = 13

def set_status(red=False, yellow=False, green=False):
    """Ensures ONLY one LED is active at a time."""
    led_red.value(1 if red else 0)
    led_yellow.value(1 if yellow else 0)
    led_green.value(1 if green else 0)

def open_gate_servo():
    servo.duty(ANGLE_OPEN)

def close_gate_servo():
    servo.duty(ANGLE_CLOSED)

# -------------------------------------------------------------
# Initial State: RED LED ON & Gate CLOSED
# -------------------------------------------------------------
close_gate_servo()
set_status(red=True)
print("[SYSTEM] System Initialized: RED LED ON & GATE CLOSED.")

# Main Control Loop
while True:
    if pir1.value() == 1 or pir2.value() == 1:
        print("[SYSTEM] Motion detected! Starting RED -> YELLOW -> GREEN sequence...")
        
        # 1. Red Phase: Gate Closes / Remains Closed
        close_gate_servo()
        set_status(red=True)
        print(f"[STAGE 1 - RED] Red LED ON | Gate CLOSED | Duration: {STEP_TIME}s")
        time.sleep(STEP_TIME)
        
        # 2. Yellow Phase: Gate Opens & Yellow LED stays ON
        open_gate_servo()
        set_status(yellow=True)
        print(f"[STAGE 2 - YELLOW] Yellow LED ON | Gate OPEN | Duration: {STEP_TIME}s")
        time.sleep(STEP_TIME)
        
        # 3. Green Phase: Gate Stays Open & Green LED stays ON
        set_status(green=True)
        print(f"[STAGE 3 - GREEN] Green LED ON | Gate OPEN | Duration: {STEP_TIME}s")
        time.sleep(STEP_TIME)
        
        # Hold Green as long as motion is still continuously detected
        while pir1.value() == 1 or pir2.value() == 1:
            time.sleep(0.5)
            
        # Return to default RED state & CLOSE gate
        close_gate_servo()
        set_status(red=True)
        print("[SYSTEM] Sequence completed. Resetting to RED LED & GATE CLOSED.")
        
    time.sleep(0.1)