import time
import rp2
from machine import Pin
print("start")

# Define the blink program.  It has one GPIO to bind to on the set instruction, which is an output pin.
# Use lots of delays to make the blinking visible by eye.
@rp2.asm_pio(set_init=rp2.PIO.OUT_LOW,  out_init = [rp2.PIO.OUT_HIGH]*4)
def blink():
    wrap_target()
    pull()
    out(y, 24)
    mov(isr, osr) # save the input state
    label("loop")
    set(pindirs, 1)
    mov(osr, isr) # reload osr with pin states
    out(pins, 4)  # Set first pin pattern
    nop()          [31]
    set(pindirs, 0)
    out(pins, 4)  # Set 2nd pin pattern
    nop()          [31]
    nop()          [31]
    jmp(y_dec, "loop")
    in_(x, 8)
    push()
    wrap()

# Instantiate a state machine with the blink program, at 2000Hz, with set bound to Pin(25) (LED on the rp2 board)
sm = rp2.StateMachine(0, blink, freq=2000, set_base=Pin(0), out_base=Pin(1), sideset_base = Pin(0), in_base = Pin(0), jmp_pin=Pin(0))

def encode_params(count, a, b):
    a&=0xf
    b&=0xf
    return (count<<8)|(a<<4)|(b)

# Run the state machine for 3 seconds.  The LED should blink.
while(1):
    sm.put(encode_params(3, 1|2, 4|8))
    sm.put(encode_params(3, 4|8, 1|2))
    sm.put(encode_params(5, 1|4, 2|8))
    sm.put(encode_params(5, 2|8, 1|4))
    #sm.put(0x12a55679)
    sm.active(1)
    for n in range(4):
        a = sm.get()
        print(a)
    sm.active(0)