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

class PIO_QENC:
    def __init__(self, sm_id, pins, freq=10_000_000):
        if not isinstance(pins, (tuple, list)) or len(pins) != 2:
            raise ValueError('2 successive pins required')
        pinA = int(str(pins[0]).split(')')[0].split('(')[1].split(',')[0])
        pinB = int(str(pins[1]).split(')')[0].split('(')[1].split(',')[0])
        if abs(pinA-pinB) != 1:
            raise ValueError('2 successive pins required')
        in_base = pins[0] if pinA < pinB else pins[1]
        self.sm_qenc = StateMachine(sm_id, self.sm_qenc, freq=freq, in_base=in_base, out_base=in_base)
        self.sm_qenc.exec("set(x, 1)")  # we once decrement at the start
        self.sm_qenc.exec("in_(pins, 2)")
        self.sm_qenc.active(1)
    
    @staticmethod
    @rp2.asm_pio(in_shiftdir=PIO.SHIFT_LEFT, out_shiftdir=PIO.SHIFT_RIGHT)
    def sm_qenc():
        # Kvadraturális encoder feldolgozás - ugró tábla
        jmp("read")        # 0000 : from 00 to 00 = no change
        jmp("decr")        # 0001 : from 00 to 01 = backward
        jmp("incr")        # 0010 : from 00 to 10 = forward
        jmp("read")        # 0011 : from 00 to 11 = error
        jmp("incr")        # 0100 : from 01 to 00 = forward
        jmp("read")        # 0101 : from 01 to 01 = no change
        jmp("read")        # 0110 : from 01 to 10 = error
        jmp("decr")        # 0111 : from 01 to 11 = backward
        jmp("decr")        # 1000 : from 10 to 00 = backward
        jmp("read")        # 1001 : from 10 to 01 = error
        jmp("read")        # 1010 : from 10 to 10 = no change
        jmp("incr")        # 1011 : from 10 to 11 = forward
        jmp("read")        # 1100 : from 11 to 00 = error
        jmp("incr")        # 1101 : from 11 to 01 = forward

        label("decr")
        jmp(x_dec, "read") # 1110 : from 11 to 10 = backward

        label("read")      # 1111 : from 11 to 11 = no change
        mov(osr, isr)      # save last pin input in OSR
        mov(isr, x)
        push(noblock)
        out(isr, 2)        # 2 right bits of OSR into ISR, all other 0
        in_(pins, 2)       # combined with current reading of input pins
        mov(pc, isr)       # jump into jump-table at addr 0

        label("incr")      # increment x by inverting, decrementing and inverting
        mov(x, invert(x))
        jmp(x_dec, "here")
        label("here")
        mov(x, invert(x))
        jmp("read")
        
        nop()
        nop()
        nop()
        nop()
        nop()
        nop()
        nop()

    def read(self):
        for _ in range(self.sm_qenc.rx_fifo()):
            self.sm_qenc.get()
        n = self.sm_qenc.get()
        return n if n < (1<<31) else n - (1<<32)

# Pinok beállítása az encoder jelekhez
pinA = Pin(15, Pin.IN, Pin.PULL_UP)  # Encoder A pin
pinB = Pin(16, Pin.IN, Pin.PULL_UP)  # Encoder B pin

# Létrehozzuk az encoder objektumot
qenc = PIO_QENC(0, (pinA, pinB))

# Kezdés
print('starting....')

# Számláló érték kiírása 120 cikluson keresztül
for i in range(120):
    print('x:', qenc.read())  # Kiírjuk a számláló értékét
    sleep_ms(500)  # Várakozás fél másodpercet

# Állapotgép leállítása
qenc.sm_qenc.active(0)
print('stop')