from machine import Pin
from time import sleep
import rp2

# rows = [18, 19, 20, 21]
# outcols = [0, 1, 2, 3, 4, 5, 6, 7]
incols = [8, 9, 10, 11, 12, 13, 14, 15]
in_pins = [ Pin(pin_name, mode=Pin.IN, pull=Pin.PULL_UP) for pin_name in incols ]

@rp2.asm_pio(out_init=[rp2.PIO.OUT_LOW]*8, sideset_init=[rp2.PIO.OUT_HIGH]*4)
def ledswitches():
    mov(x, null)    .side(0b1111)      # initialize x to all LEDs off
    mov(y, null)            .side(0b1111)      # initialize y to stash last switch state
    wrap_target()
    pull(noblock)           .side(0b1111)      # Grab LED state from ouput buffer or X
    mov(x, osr)             .side(0b1111)      # Keep most recent pull data stashed in X, for recycling by noblock
    out(pins, 8)            .side(0b1110)      # Shift 8 bits from OSR to Row 1 LEDs
    in_(pins, 8)            .side(0b1110)      # Shift 8 bits to ISR from Row 1 Switches
    out(pins, 8)            .side(0b1101)      # Shift 8 bits from OSR to Row 2 LEDs
    in_(pins, 8)            .side(0b1101)      # Shift 8 bits to ISR from Row 2 Switches
    out(pins, 8)            .side(0b1011)      # Shift 8 bits from OSR to Row 3 LEDs
    in_(pins, 8)            .side(0b1011)      # Shift 8 bits to ISR from Row 3 Switches
    out(pins, 8)            .side(0b0111)      # Shift 8 bits from OSR to Row 4 LEDs
    in_(pins, 8)            .side(0b0111)      # Shift 8 bits to ISR from Row 4 Switches
    mov(osr, x)             .side(0b1111)      # OSR is empty now, temporarily store X there
    mov(x, isr)             .side(0b1111)      # Copy switch state from ISR to X
    jmp(x_not_y, "changed") .side(0b1111)      # If Switch state changed...
    jmp("continue")         .side(0b1111)      # Switch state didn't change, jump to end
    label("changed")
    mov(y, x)               .side(0b1111)      # Save new switch state
    push(noblock)           .side(0b1111)      # Push Switch state to main process
    irq(0)                  .side(0b1111)      # Tell the main process switches changed
    label("continue")
    mov(x, osr)             .side(0b1111)      # get X back from OSR
    out(null, 32)           .side(0b1111)      # empty the OSR
    wrap()

sm = rp2.StateMachine(0, ledswitches, freq=2000, out_base=Pin(0), in_base=Pin(8), sideset_base=Pin(18))
sm.irq(lambda p: sm.put(sm.get() ^ 0xffffffff))
sm.active(1)

while True:
  pass