from machine import Pin, PWM, ADC
import time

# ---------------- LEDs ------------------
led1 = Pin(2, Pin.OUT)   # Floor 1
led2 = Pin(3, Pin.OUT)   # Floor 2

# ---------------- RGB LED ----------------
red = Pin(10, Pin.OUT)
green = Pin(11, Pin.OUT)
blue = Pin(12, Pin.OUT)

# ---------------- Buttons ----------------
btn1 = Pin(14, Pin.IN, Pin.PULL_DOWN)
btn2 = Pin(15, Pin.IN, Pin.PULL_DOWN)

# ---------------- Servo ----------------
servo = PWM(Pin(16))
servo.freq(50)

# ---------------- Potentiometer ----------------
pot = ADC(26)

# ---------------- FUNCTIONS ----------------

def set_rgb(r, g, b):
    red.value(r)
    green.value(g)
    blue.value(b)

def open_door():
    servo.duty_u16(6000)   # adjust if needed

def close_door():
    servo.duty_u16(2000)   # adjust if needed

def go_to_floor(floor):
    # MOVING
    set_rgb(0, 0, 1)  # blue
    led1.value(0)
    led2.value(0)

    time.sleep(2)  # simulate movement

    # ARRIVAL
    if floor == 1:
        led1.value(1)
    elif floor == 2:
        led2.value(1)

    # DOOR OPEN
    set_rgb(1, 0, 0)  # red
    open_door()
    time.sleep(2)

    # DOOR CLOSE
    close_door()

    # IDLE
    set_rgb(0, 1, 0)  # green


# ---------------- START STATE ----------------
set_rgb(0, 1, 0)  # idle green
close_door()

# ---------------- MAIN LOOP ----------------
while True:

    if btn1.value() == 1:
        go_to_floor(1)
        time.sleep(0.5)

    if btn2.value() == 1:
        go_to_floor(2)
        time.sleep(0.5)

    # optional: read potentiometer (debug)
    print("Pot:", pot.read_u16())

    time.sleep(0.1)