'''
Project: 2 Rotary Encoders 
Project by: Jim F, Calgary AB
Date Started: 10 Nov 2024
Date Updated: 10 Nov 2024
'''

import rp2
from rp2 import PIO
from machine import Pin
from time import sleep

'''
State Machine 0
PIO funciton to read 2 Rotary Encoders
'''
@rp2.asm_pio(set_init=[PIO.IN_HIGH]*4)
def rotInputs():
    wrap_target()
    set(y, 0)
    label("1")
    mov(isr, null)
    in_(pins, 4) [2]
    mov(x, isr)                           
    jmp(x_not_y, "2")                    
    jmp("1")                              
    label("2")
    push()                           
    irq(0)
    mov(y, x)
    jmp("1")                              
    wrap()

'''
INPUT Pins
'''
for i in range(10, 17):
  Pin(i, Pin.IN, Pin.PULL_DOWN)

pin14=Pin(14, Pin.IN, Pin.PULL_UP)
pin15=Pin(15, Pin.IN, Pin.PULL_UP)


'''
OUTPUT Pins
'''
for i in range(0,6):
  Pin(i, Pin.OUT)

'''
GLOBALS
'''
seq=list()
rotPos1=0
rotPos2=0
rotInput=0

'''
Interrupt 0
'''
def oninput(machine):
  global seq,rotPos1,rotPos2,rotInput
  vals = machine.get()
  while machine.rx_fifo():
      vals = machine.get()
  sum=0
  for i in range(17):
    if (vals & (1 << i)):
      sum+=(2**i)
  if(len(seq)<4) :
    seq.append(sum)
  
  if(sum==15):
    rotPos1+=int(seq[0]==14 and seq[1]==12 and seq[2]==13)
    rotPos1-=int(seq[0]==13 and seq[1]==12 and seq[2]==14)
    rotPos2-=int(seq[0]==7 and seq[1]==3 and seq[2]==11)
    rotPos2+=int(seq[0]==11 and seq[1]==3 and seq[2]==7)
    seq=list()
    rotInput=1
  
''' 
State Machine 0
'''
sm = rp2.StateMachine(0, rotInputs, freq=20_000,in_base=Pin(10, Pin.IN, Pin.PULL_DOWN), set_base=Pin(6))
sm.active(1)
sm.irq(oninput)


'''
MAIN LOOP
'''
while True:
  p14=int(1-pin14.value())
  p15=int(1-pin15.value())
  if(p14>0 or p15>0):
    if(p14==1) :
      rotPos1=0
    if(p15==1) :
      rotPos2=0
    rotInput=1
  if(rotInput==1):
    print(rotPos1,rotPos2,p14,p15)
    rotInput=0
    sleep(0.2)
  sleep(0.01)