from machine import Pin, PWM
from utime import sleep

firstfloor = Pin(1, Pin.IN, Pin.PULL_UP)
secondfloor = Pin(5, Pin.IN, Pin.PULL_UP)
thirdfloor = Pin(9, Pin.IN, Pin.PULL_UP)
fourthfloor = Pin(13, Pin.IN, Pin.PULL_UP)

FIRSTFLOOR = 500000
SECONDFLOOR = 1150000
THIRDFLOOR = 1550000
FOURTHFLOOR = 1800000

led4=Pin(26, Pin.OUT)
led3=Pin(21, Pin.OUT)
led2=Pin(18, Pin.OUT)
led1=Pin(16, Pin.OUT)

sevensegment = [
    Pin(15, Pin.OUT),  # A
    Pin(12, Pin.OUT),  # B
    Pin(17, Pin.OUT),  # C
    Pin(19, Pin.OUT),  # D
    Pin(20, Pin.OUT),  # E
    Pin(14, Pin.OUT),  # F
    Pin(11, Pin.OUT)   # G
]

# 7-segment display layout
#       A
#      ---
#  F |  G  | B
#      ---
#  E |     | C
#      ---
#       D

FIRST_DIGIT= [[1, 0, 0, 1, 1, 1, 1]]

SECOND_DIGIT = [[0, 0, 1, 0, 0, 1, 0]]

THIRD_DIGIT = [[0, 0, 0, 0, 1, 1, 0]]

FOURTH_DIGIT = [[0, 0, 1, 1, 0, 0, 1]]

def reset():
    for pin in sevensegment:
        pin.value(1)


def display_first(digits, pins):
    for i in range(len(digits)):
        for j in range(len(pins)):
            pins[j].value(digits[i][j])

reset()

servo = PWM(Pin(27), freq=40, duty_ns=FIRSTFLOOR)
led1.on()

display_first(FIRST_DIGIT,sevensegment)

while True:

    if thirdfloor.value() == 0:
        if servo.duty_ns() == 1550034:
            led3.on()
        else:
            led1.off()
            sleep(2)
            servo.duty_ns(SECONDFLOOR)
            display_first(SECOND_DIGIT,sevensegment)
            led2.on()
            
            sleep(2)
            led2.off()
            sleep(2)
            servo.duty_ns(THIRDFLOOR)
            display_first(THIRD_DIGIT,sevensegment)
            led3.on()

    elif firstfloor.value() == 0:
        if servo.duty_ns() == 500146:
            led1.on()
        else:
            led3.off()
            sleep(2)
            servo.duty_ns(SECONDFLOOR)
            display_first(SECOND_DIGIT,sevensegment)
            led2.on()
            
            sleep(2)
            led2.off()
            sleep(2)
            servo.duty_ns(FIRSTFLOOR)
            display_first(FIRST_DIGIT,sevensegment)
            led1.on()



    # duty_ns = 1000000
    # print(duty_ns)
    # if servo.duty_ns == 1000000:
        # print(servo.duty_ns)
        # led1.value(1)
    # if firstfloor.value() == 1:
    #     led1.value(1)