from machine import Pin
import time

# Setup RGB LED pins (common cathode)
red = Pin(0, Pin.OUT)
green = Pin(1, Pin.OUT)
blue = Pin(2, Pin.OUT)

# Dictionary to map events to RGB states
# Each value is a (red, green, blue) tuple: 1 = ON, 0 = OFF
event_colours = {
    "idle": (0, 0, 0),          # all off
    "start": (1, 1, 0),         # yellow (red + green)
    "obstacle": (1, 0, 0),      # red
    "navigating": (0, 0, 1),    # blue
    "goal_reached": (0, 1, 0),  # green
    "error": (1, 0, 1),         # magenta
}

# Function to set colour
def set_colour(rgb):
    red.value(not rgb[0])    # invert for common cathode
    green.value(not rgb[1])
    blue.value(not rgb[2])

# Example event sequence
events = ["idle", "start", "navigating", "obstacle", "navigating", "goal_reached", "idle"]

# Cycle through events
for event in events:
    print("Event:", event)
    set_colour(event_colours[event])
    time.sleep(1)