from machine import Pin
from rp2 import PIO, StateMachine, asm_pio
from time import sleep

## use out as DL1, use side as DL2
@asm_pio(sideset_init=PIO.OUT_LOW, out_init=PIO.OUT_HIGH, out_shiftdir=PIO.SHIFT_RIGHT)
def rsl_prog():
    wrap_target()
    pull(noblock)              # 1# wait for OSR, non-block mode
    jmp(not_osre, "notspace")  # 1# jump if OSR have new data
    
    set(y,24)           # 1# TRR: loop 177 cycles, this block delays 1600us
    label("space")
    set(x,6)            # 1#
    label("space0")
    nop()          [7]  # 8#
    jmp(x_dec, "space0") # 1#
    jmp(y_dec, "space") # 1#
    
    label("notspace")
    nop() .side(1) [1]  # 2# TRR: put SCK high
    irq(rel(0))         # 3# TRR: rise an interrupt do let user programe put new data in
    set(y,10)      [5]  # 6# TRR: loop 11 cycles
    label("loop1")
    nop()          [7]  # 8#
    jmp(y_dec, "loop1") # 1#
    
    nop() .side(0) [4]  # 5# TRR: put SCK low, get data from OSR
    set(y,10)      [5]  # 6# TRR: loop 11 cycles
    label("loop2")
    nop()          [7]  # 8#
    jmp(y_dec, "loop2") # 1#
    set(x,4)            # 1# Initialise bit counter, assert start bit for 8 cycles
    
    ## TRR: loop 5 cycles
    label("bitloop")
    out(pins, 1)   [7]  # 8#
    set(y,8)      [5]   # 6# TRR: loop 9 cycles
    label("loop3")
    nop()          [7]  # 8#
    jmp(y_dec, "loop3") # 1#
    in_(pins, 1)        # 1#
    nop()          [7]  # 8#
    nop()          [7]  # 8#
    jmp(x_dec, "bitloop") # 1#
    
    set(pins, 0)   [7]  # 8#
    set(y,10)      [5]  # 6# TRR: loop 11 cycles
    label("loop4")
    nop()          [7]  # 8#
    jmp(y_dec, "loop4") # 1#
    wrap()              # 1#


class PIORSL:
    def __init__(self, sm_id, pin_in, pin_da, pin_ck):
        count_freq = 10000
        self.counter = 0
        self._sm = StateMachine(sm_id, rsl_prog, freq=count_freq, in_base=Pin(pin_in), out_base=Pin(pin_da), sideset_base=Pin(pin_ck))
        ## Use exec() to load max count into ISR
        #self._sm.exec("pull()")
        #self._sm.exec("mov(isr, osr)")
        self.obuf = [0] * 64
        self.ibuf = [0] * 64
        for i in range(64):
            self.setio(i,0) #initial obuf
        self.obuf_pv = 0
        self._sm.irq(self._irq)
    def setio(self, addr, value):
        if addr < 64:
            value = value & 0x1f
            self.obuf[addr] = (value<<2)|0x01
    def getio(self, addr):
        if addr < 64:
            return self.ibuf[addr]
    def start(self):
        self.obuf_pv = 0
        self._sm.put(self.obuf[self.obuf_pv])
        self.obuf_pv += 1
        self._sm.active(1)
    def _irq(self,pin):
        tmp = self._sm.get()
        #tmp = 0
        if self.counter < 64:
            self._sm.put(self.obuf[self.counter]) # put data to OSR
        elif self.counter < 128:
            self._sm.put(0x01)  # put empty data to OSR
            self.ibuf[self.counter - 64] = tmp
        self.counter += 1
        if self.counter >=130:
            # not put any data to OSR
            self.counter = 0


# Pin 25 on Pico boards
rsl = PIORSL(0, 6, 5, 4)
#for i in range(64):
#    rsl.setio(i,0x00)
rsl.start()

print("RSL running")
for i in range(10):
    sleep(1)
    a = rsl.getio(17)
    print(a)