from machine import Pin
from rp2 import PIO, StateMachine, asm_pio
from time import sleep_ms
# https://forum.micropython.org/viewtopic.php?t=12277

class PIO_QENC:
    def __init__(self, sm_id, pins, freq=20_000_000):
        if len(pins) != 2:
            raise ValueError('2 pins required')

        pinA, pinB = pins

        # Ellenőrizzük, hogy a pinA és pinB egymás mellett van-e
        if abs(pinA - pinB) != 1:
            raise ValueError('Pins must be consecutive')

        # Pin-ek inicializálása
        self.pinA = Pin(pinA, Pin.IN, Pin.PULL_UP)
        self.pinB = Pin(pinB, Pin.IN, Pin.PULL_UP)

        # Állapotgép inicializálása
        self.sm_qenc = StateMachine(sm_id, self.sm_qenc, freq=freq, in_base=self.pinA, out_base=self.pinB)
        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ó kódja
        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)

# A két encoder pin beállítása (egymás melletti pin)
pinA = 15  # Pin szám
pinB = 16  # Pin szám

pinC = 17  # Pin szám
pinD = 18  # Pin szám

pinE = 19  # Pin szám
pinF = 20  # Pin szám

# Két encoder, 4x módon működik
qenc1 = PIO_QENC(0, (pinA, pinB))
qenc2 = PIO_QENC(1, (pinC, pinD))
qenc3 = PIO_QENC(3, (pinE, pinF))


# Tesztelés: 1 másodpercenként kiírjuk az encoderek számláló értékeit
# for i in range(120):
c=0
while True:
    print(c,'.Encoder 1:', qenc1.read())
    print(c,'.Encoder 2:', qenc2.read())    
    print(c,'.Encoder 3:', qenc3.read())
    sleep_ms(100)
    c+=1
qenc1.sm_qenc.active(0)
qenc2.sm_qenc.active(0)
qenc3.sm_qenc.active(0)
print('Stopped')