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 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')

        # A StateMachine inicializálása
        self.sm_qenc = StateMachine(sm_id, self.sm_qenc, freq=freq)
        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 kódja itt
        pass

    def read(self):
        return 0  # Csak példaként

# Két egymás melletti pin beállítása, pin számok használata
pinA = 15  # Pin szám
pinB = 16  # Pin szám

qenc = PIO_QENC(0, (pinA, pinB))

# Tesztelés
for i in range(10):
    print('x:', qenc.read())
    sleep_ms(500)