import machine
import micropython
import time
from utime import sleep

micropython.alloc_emergency_exception_buf(100)

BUTTON_COUNT = 2
BUTTON_GPIO_START = 14
BUTTON_DEBOUNCE_DELAY_ms = 50
THRESHOLD_ms = 700

LED_COUNT = 2
LED_GPIO_START = 2

class Debouncer:
    inputs = []   # Noisy inputs.
    outputs = []  # Debounced outputs.
    risers = []   # Inputs that rose since they were last debounced.
    fallers = []  # Inputs that fell since they were last debounced.
    delay = 0     # Debounce delay.
    previousTimestamp = time.ticks_ms()

    def __init__(self, inputs, delay):
        self.inputs = inputs      # Assign with reference, i.e. pointer. These will be updated by the ISR.
        self.outputs = inputs[:]  # Assign without reference, i.e. copy.
        self.risers = [False] * len(inputs)
        self.fallers = [False] * len(inputs)
        self.delay = delay

    def Update(self):
        currentTimestamp = time.ticks_ms()
        sampledInputs = self.inputs[:]
        if sampledInputs[:] == self.outputs[:]:
            self.previousTimestamp = currentTimestamp
        elif currentTimestamp - self.previousTimestamp > self.delay:
            for i in range(len(self.inputs)):
                self.risers[i] = sampledInputs[i] == 1 and self.outputs[i] == 0
                self.fallers[i] = sampledInputs[i] == 0 and self.outputs[i] == 1
            self.outputs = sampledInputs[:]
            #print(f"inputs  = {sampledInputs}")
            #print(f"outputs = {self.outputs}")
            #print(f"risers  = {self.risers}")
            #print(f"fallers = {self.fallers}")
            self.previousTimestamp = currentTimestamp
            return
        self.risers = [False] * len(self.inputs)
        self.fallers = [False] * len(self.inputs)

# Get the pin ID from its description and convert it to an index.
#def PinId(pin):
#    return int(str(pin)[8:10]) - BUTTON_GPIO_START

# Get the pin ID from its index in the button array
def PinId(pin):
    return buttons.index(pin)

# Interrupt Service Routine (ISR) to update the inputs array with the button values.
def ISR_Button(pin):
    inputs[PinId(pin)] = pin.value()

# Array of button values which will be updated by the ISR.
inputs = []

# Initialise the buttons as pull-ups with an ISR that is triggered on rising or falling edges.
# Initialise the inputs with the button values.
buttons = []
for i in range(BUTTON_COUNT):
    button = machine.Pin(
        BUTTON_GPIO_START + i, machine.Pin.IN, machine.Pin.PULL_UP
    )
    button.irq(trigger=machine.Pin.IRQ_RISING | machine.Pin.IRQ_FALLING, handler=ISR_Button)
    buttons.append(button)
    inputs.append(button.value())

# Initialise the output pins for the LEDs.
outputPins = []
for i in range(LED_COUNT):
    pin = machine.Pin(LED_GPIO_START + i, machine.Pin.OUT)
    outputPins.append(pin)

# Create the debouncer that points to the inputs. The inputs will be updated by the ISR.
debouncer = Debouncer(inputs, BUTTON_DEBOUNCE_DELAY_ms)

time.sleep(0.1)  # Wait for USB to become ready

print("Hello, Pi Pico!")

previousTimestamp = time.ticks_ms()

STATE_WAITING_FOR_PRESS = 1
STATE_WAITING_FOR_THRESHOLD = 2
STATE_WAITING_FOR_RELEASE = 3

state = STATE_WAITING_FOR_PRESS

while True:
    # INPUT LOGIC
    # Update the debouncer.
    debouncer.Update()
    risersOccurred = debouncer.risers != [False] * BUTTON_COUNT
    fallersOccurred = debouncer.fallers != [False] * BUTTON_COUNT
    currentTimestamp = time.ticks_ms()

    # OUTPUT LOGIC
    # Update the state of the LEDs with the debouncer outputs.
    for i in range(LED_COUNT):
        outputPins[i].value(debouncer.outputs[i])

    # STATE LOGIC
    if state == STATE_WAITING_FOR_PRESS:
        if fallersOccurred:
            # Reset the threshold timer.
            previousTimestamp = currentTimestamp
            state = STATE_WAITING_FOR_THRESHOLD
            print("Fallers occured. Waiting for threshold.")    

    elif state == STATE_WAITING_FOR_THRESHOLD:
        if risersOccurred:
            state = STATE_WAITING_FOR_RELEASE
            print("Risers occurred. Waiting for release.")
        elif fallersOccurred:
            # Reset the threshold timer.
            previousTimestamp = currentTimestamp
        elif currentTimestamp - previousTimestamp > THRESHOLD_ms:
            print("New function.")
            if debouncer.outputs[0] == 0 and debouncer.outputs[1] == 0:
                print("Tear")
            elif debouncer.outputs[0] == 0 and debouncer.outputs[1] == 1:
                print("Calibrate")
            elif debouncer.outputs[0] == 1 and debouncer.outputs[1] == 0:
                print("Cancel")
            
            state = STATE_WAITING_FOR_RELEASE
            print("Waiting for release.")

    elif state == STATE_WAITING_FOR_RELEASE:
        if debouncer.outputs == [1] * BUTTON_COUNT:
            print("All buttons released.")
            state = STATE_WAITING_FOR_PRESS
            print("Waiting for press.")

    sleep(0.001)