import time
import board
import digitalio

# ================= LED =================
red = digitalio.DigitalInOut(board.GP6)
yellow = digitalio.DigitalInOut(board.GP7)
green = digitalio.DigitalInOut(board.GP8)

red.direction = digitalio.Direction.OUTPUT
yellow.direction = digitalio.Direction.OUTPUT
green.direction = digitalio.Direction.OUTPUT

# ================= SWITCH =================
sw1 = digitalio.DigitalInOut(board.GP19)
sw1.direction = digitalio.Direction.INPUT
sw1.pull = digitalio.Pull.UP

sw2 = digitalio.DigitalInOut(board.GP20)
sw2.direction = digitalio.Direction.INPUT
sw2.pull = digitalio.Pull.UP

# ================= 7-SEG BCD =================
bcd_pins = []
for p in [board.GP2, board.GP3, board.GP4, board.GP5]:
    pin = digitalio.DigitalInOut(p)
    pin.direction = digitalio.Direction.OUTPUT
    bcd_pins.append(pin)

s72 = digitalio.DigitalInOut(board.GP1)
s71 = digitalio.DigitalInOut(board.GP0)

s71.direction = digitalio.Direction.OUTPUT
s72.direction = digitalio.Direction.OUTPUT
s71.value = True
s72.value = True

# ================= STATE =================
state = 1
# ================= FUNCTIONS =================

def send_bcd(value):
    for i in range(4):
        bcd_pins[i].value = (value >> i) & 1

def latch(pin):
    pin.value = False
    time.sleep(0.001)
    pin.value = True
    time.sleep(0.001)

def display_number(num):
    tens = num // 10
    ones = num % 10

    send_bcd(tens)
    latch(s72)

    send_bcd(ones)
    latch(s71)

def all_off():
    red.value = False
    yellow.value = False
    green.value = False

def debounce(sw):
    if not sw.value:
        time.sleep(0.05)
        if not sw.value:
            return True
    return False

# ================= EMERGENCY CHECK =================

def check_emergency():
    global state
    if debounce(sw2):
        state = 4

# ================= COUNTDOWN =================

def countdown(t):
    global state
  
    for i in range(t, -1, -1):  #Countdown 改 （0，t+1)

        check_emergency()
        if state == 4:
            return

        print("TIME:", i)
        display_number(i)

        for j in range(10):
            check_emergency()
            if state == 4:
                return
            time.sleep(0.1)


# ================= STATES =================

def state1():
    global state
    all_off()

    # red blinking idle
    red.value = True
    time.sleep(0.2)

    if debounce(sw1):
        state = 2

def state2():
    global state
    all_off()
    yellow.value = True

    countdown(5)

    if state != 4:
        state = 3

def state3():
    global state
    all_off()
    green.value = True

    countdown(5)

    if state != 4:
        state = 1

def state4():
    global state
    all_off()

    print("EMERGENCY MODE")

    # emergency countdown
    for i in range(5, -1, -1):

        display_number(i)

        red.value = True
        time.sleep(0.5)
        red.value = False
        time.sleep(0.5)

        time.sleep(1)

    state = 1


# ================= MAIN LOOP =================

while True:

    check_emergency()

    if state == 1:
        state1()

    elif state == 2:
        state2()

    elif state == 3:
        state3()

    elif state == 4:
        state4()