# SPDX-FileCopyrightText: 2022 Jamon Terrell <[email protected]>
# SPDX-License-Identifier: MIT

from rp2 import PIO, StateMachine, asm_pio
from machine import Pin
import utime
#@rp2.asm_pio(set_init=rp2.PIO.OUT_LOW)
@asm_pio(autopush=True, push_thresh=32)#,set_init=rp2.PIO.OUT_HIGH,jmp_init=rp2.PIO.IN_HIGH)
def encoder():
    label("start")
    wait(0, pin, 0)         # Wait for CLK to go low
    jmp(pin, "WAIT_HIGH")   # if Data is low
    mov(x, invert(x))           # Increment X
    jmp(x_dec, "nop1")
    label("nop1")
    mov(x, invert(x))
    label("WAIT_HIGH")      # else
    jmp(x_dec, "nop2")          # Decrement X
    label("nop2")
   
    wait(1, pin, 0)         # Wait for CLK to go high
    jmp(pin, "WAIT_LOW")    # if Data is low
    jmp(x_dec, "nop3")          # Decrement X
    label("nop3")
    
    label("WAIT_LOW")       # else
    mov(x, invert(x))           # Increment X
    jmp(x_dec, "nop4")
    label("nop4")
    mov(x, invert(x))
    
    wrap()

@asm_pio(autopush=True, push_thresh=32)#,set_init=rp2.PIO.OUT_HIGH,jmp_init=rp2.PIO.IN_HIGH)
def encoder2():
    label("start")
    wait(0, pin, 0)         # Wait for CLK to go low
    jmp(pin, "WAIT_HIGH")   # if Data is low
    mov(x, invert(x))           # Increment X
    jmp(x_dec, "nop1")
    label("nop1")
    mov(x, invert(x))
    label("WAIT_HIGH")      # else
    jmp(x_dec, "nop2")          # Decrement X
    label("nop2")
   
    wait(1, pin, 0)         # Wait for CLK to go high
    jmp(pin, "WAIT_LOW")    # if Data is low
    jmp(x_dec, "nop3")          # Decrement X
    label("nop3")
    
    label("WAIT_LOW")       # else
    mov(x, invert(x))           # Increment X
    jmp(x_dec, "nop4")
    label("nop4")
    mov(x, invert(x))
    
    wrap()


pin0 = Pin(0, Pin.IN, Pin.PULL_UP)
pin1 = Pin(1, Pin.IN, Pin.PULL_UP)

pin2 = Pin(0, Pin.IN, Pin.PULL_UP)
pin3 = Pin(1, Pin.IN, Pin.PULL_UP)

    #                           freq=125_000_000
sm1 = StateMachine(1, encoder, freq=50000, in_base=pin0, jmp_pin=pin1)
sm1.active(1)
sm2 = StateMachine(2, encoder2, freq=50000, in_base=pin2, jmp_pin=pin3)
sm2.active(1)

while(True):
    utime.sleep(1)
    sm1.exec("in_(x, 32)")
    sm2.exec("in_(y, 32)")
    y = sm1.get()
    x = sm1.get()
    print(x,':',y)