# SPDX-FileCopyrightText: 2022 Jamon Terrell <github@jamonterrell.com>
# SPDX-License-Identifier: MIT

from rp2 import PIO, StateMachine, asm_pio
from machine import Pin
import utime
@asm_pio(autopush=True, push_thresh=32)
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()

   
sm1 = StateMachine(1, encoder, freq=125_000_000, in_base=Pin(3), jmp_pin=Pin(2))
sm1.active(1)

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

.program quadrature
start:
   wait 0 pin 0            ; wait for B == 0
   jmp PIN, wait_high      ; if A == 0
   mov y, !x                   ; x++ {
   jmp y--, nop1               ;
nop1:                           ;
   mov x, !y                   ; }
   jmp nop2
wait_high:
   jmp x--, nop2           ; x-- {
nop2:                       ; }

   wait 1 PIN 0            ; wait for B == 1
   jmp PIN, wait_low       ; if A == 0
   jmp x--, start               ; x-- {
                              ; }
wait_low:                   ; else
   mov y, !x                   ; x++ {
   jmp y--, nop4               ;
nop4:                           ;
   mov x, !y                   ;
   jmp start                   ; }

; Must start at 0 so that the following jump table can be jumped into with a
; 'mov pc, isr' instruction.
.origin 0
   ; 16 element jump table based on 4-bit encoder last state and current state.
   jmp delta0      ; 00-00
   jmp minus1      ; 00-01
   jmp plus1       ; 00-10
   jmp delta0      ; 00-11
   jmp plus1       ; 01-00
   jmp delta0      ; 01-01
   jmp delta0      ; 01-10
   jmp minus1      ; 01-11
   jmp minus1      ; 10-00
   jmp delta0      ; 10-01
   jmp delta0      ; 10-10
   jmp plus1       ; 10-11
   jmp delta0      ; 11-00
   jmp plus1       ; 11-01
   jmp minus1      ; 11-10
   jmp delta0      ; 11-11

; Program actually starts here.
.wrap_target
delta0:
public start:
   mov isr, null       ; Make sure that the input shift register is cleared when table jumps to delta0.
   in y, 2             ; Upper 2-bits of address are formed from previous encoder pin readings
   mov y, pins         ; Lower 2-bits of address are formed from current encoder pin readings. Save in Y as well.
   in y, 2
   mov pc, isr         ; Jump into jump table which will then jump to delta0, minus1, or plus1 labels.
minus1:
   jmp x-- output      ; Decrement x
   jmp output
plus1:
   mov x, ~x           ; Increment x by calculating x=~(~x - 1)
   jmp x-- next2
next2:
   mov x, ~x
output:
   mov isr, x          ; Push out updated counter.
   push noblock
.wrap