import utime
utime.sleep(0.1) # Wait for USB to become ready
import machine
from machine import Pin, I2C
from ssd1306 import SSD1306_I2C

# Display
pix_res_x = 128
pix_res_y = 64

# Define the pins for the buttons
button_mode1 = machine.Pin(16, machine.Pin.IN, machine.Pin.PULL_UP)
button_mode2 = machine.Pin(17, machine.Pin.IN, machine.Pin.PULL_UP)

#DP Switch 1 (Der Input der auf 0V steht ist aktiv, der Input mit +5v ist inaktiv)
device1_state1 = machine.Pin(3, machine.Pin.IN, machine.Pin.PULL_DOWN)
device1_state2 = machine.Pin(4, machine.Pin.IN, machine.Pin.PULL_DOWN)

device2_state1 = machine.Pin(5, machine.Pin.IN, machine.Pin.PULL_DOWN)
device2_state2 = machine.Pin(6, machine.Pin.IN, machine.Pin.PULL_DOWN)
device3_state1 = machine.Pin(7, machine.Pin.IN, machine.Pin.PULL_DOWN)
device3_state2 = machine.Pin(8, machine.Pin.IN, machine.Pin.PULL_DOWN)

#USB Switch (Der Input der 0V steht ist aktiv, der Input mit +3.3v is inaktiv
device4_state1 = machine.Pin(9, machine.Pin.IN, machine.Pin.PULL_DOWN)
device4_state2 = machine.Pin(10, machine.Pin.IN, machine.Pin.PULL_DOWN)

# Define the pins for the outputs
switch1 = machine.Pin(11, machine.Pin.OUT, value=1)
switch2 = machine.Pin(12, machine.Pin.OUT, value=1)
switch3 = machine.Pin(13, machine.Pin.OUT, value=1)
switch4 = machine.Pin(14, machine.Pin.OUT, value=1)

# Define the variables to store the last button press time
button1_press = 0
button2_press = 0

# Define the variable to store the mode and status
mode = "1"
state_D1 = "3"
state_D2 = "3"
state_D3 = "3"
state_D4 = "3"

def init_i2c(scl_pin, sda_pin):
    # Initialize I2C device
    i2c_dev = I2C(1, scl=Pin(scl_pin), sda=Pin(sda_pin), freq=200000)
    i2c_addr = [hex(ii) for ii in i2c_dev.scan()]
    
    if not i2c_addr:
        print('No I2C Display Found')
        sys.exit()
    return i2c_dev

def display_text(oled):
    # Display text on the OLED
    oled.text("Mode", 5, 5)
    oled.text("DP", 45, 5)
    oled.text("H1", 65, 5)
    oled.text("H2", 85, 5)
    oled.text("USB",105, 5)
    oled.text(str(mode), 18, 20)
    oled.text(str(state_D1), 50, 20)
    oled.text(str(state_D2), 68, 20)
    oled.text(str(state_D3), 88, 20)
    oled.text(str(state_D4), 112, 20)
    oled.show()

# Define the function to print the message
def state(pin):
    global last_button1_press
    global last_button2_press

        
# Define the function to print the message
def state(pin):
    global mode
    global state_D1
    global state_D2
    global state_D3
    global state_D4
    global button1_press
    global button2_press
    current_time = utime.ticks_ms()
    if pin == button_mode1 and utime.ticks_diff(current_time, button1_press) > 100:
        print("**Mode 1 activated**")
        button1_press = current_time
        mode = "1"
    elif pin == button_mode2 and utime.ticks_diff(current_time, button2_press) > 100:
        print("**Mode 2 activated**")
        button2_press = current_time
        mode = "2"
    elif pin == device1_state2:
        state_D1 = "1"
    elif pin == device1_state1:
        state_D1 = "2"
    elif pin == device2_state1:
        state_D2 = "1"
    elif pin == device2_state2:
        state_D2 = "2"
    elif pin == device3_state1:
        state_D3 = "1"
    elif pin == device3_state2:
        state_D3 = "2"
    elif pin == device4_state2:
        state_D4 = "1"
    elif pin == device4_state1:
        state_D4 = "2" 

# Define the function to print the values of the variables
def print_variables():
    print("State:")
    print("Mode: **{}**".format(mode))
    print("Device 1: **{}**".format(state_D1))
    print("Device 2: **{}**".format(state_D2))
    print("Device 3: **{}**".format(state_D3))
    print("Device 4: **{}**".format(state_D4))

def toggle():
    if state_D1 != mode:
        switch1.value(True)
        utime.sleep_ms(500)
        switch1.value(False)
    if state_D2 != mode:
        switch2.value(True)
        utime.sleep_ms(500)
        switch2.value(False)
    if state_D3 != mode:
        switch3.value(True)
        utime.sleep_ms(500)
        switch3.value(False)
    if state_D4 != mode:
        switch4.value(True)
        utime.sleep_ms(500)
        switch4.value(False)

# Define the interrupt handlers for the buttons
button_mode1.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
button_mode2.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
device1_state1.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
device1_state2.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
device2_state1.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
device2_state2.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
device3_state1.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
device3_state2.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
device4_state1.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)
device4_state2.irq(trigger=machine.Pin.IRQ_FALLING, handler=state)

# Keep the program running
while True:
    i2c_dev = init_i2c(scl_pin=19, sda_pin=18)
    oled = SSD1306_I2C(pix_res_x, pix_res_y, i2c_dev)
    display_text(oled)
    print_variables()
    toggle()
    utime.sleep_ms(1000)