################################################################################
"""
Early Gen Roomba chassis Robot - OnePhase Decoder
Count transitions using RP2040 PIO based state machine, no CPU cycles and then
use timer based interrupt to read accumulated counts, aka velocity. - 5ms
Motor quadrature optical encoder: 32 CPT, ???:1, ~1000 counts per revolution
Derived from:
- RaspberryPi Foundation PICO Examples / PIO
   https://github.com/raspberrypi/pico-examples/blob/master/pio/quadrature_encoder
     /quadrature_encoder.pio
- MarkMakies Roverling Mk II quad encoder Motor control code
   https://github.com/MarkMakies/Roverling-Mk-II/blob/main/Code/MPUs/QuadDec.py

Requires that the "JMP" pin be defined as Input Pin+1 in the SM Init.

The code works by running a loop that continuously shifts the one encoder pin into
ISR and looks at the lower 2 bits to determine if a 0->1 transition has occurred
in the encoder input pin.  If so, then the motor direction "DIR" pin input is used
to determine whether to Inc or Dec the pulse counter in the X register.

ISR holds the last state of the encoder pin during most of the code. The X register
keeps the current encoder count and is incremented / decremented according to
the steps sampled.

the program keeps trying to write the current count to the RX FIFO without
blocking. To read the current count, the user code must drain the FIFO first
and wait for a fresh sample (takes ~4 SM cycles on average). The worst case
sampling loop takes 10 cycles, so this program is able to read step rates up
to sysclk / 10  (e.g., sysclk 125MHz, max step rate = 12.5 Msteps/sec)

"""
from machine import Pin, Timer
from rp2 import asm_pio, StateMachine
from array import array
import time

REncApin = Pin(20, Pin.IN, Pin.PULL_UP)   # Encoder Pulse pin
REncBpin = Pin(21, Pin.IN, Pin.PULL_UP)   # Dir pin
LEncApin = Pin(14, Pin.IN, Pin.PULL_UP)   # Encoder Pulse pin
LEncBpin = Pin(15, Pin.IN, Pin.PULL_UP)   # Dir pin

# Parameters  
Counts2Vel = 400

@asm_pio()
def encoder():       

    label('init')   	# Program initially starts here to init A'.
    in_(pins, 1) 		# get Phase A bit -> ISR (1st pin)
# read new pulse, combine with last pulse, and push current count into fifo
    label('read')
    mov(osr, isr)		# save A' in osr
    out(isr, 1)			# shift A' from osr into isr; rest of isr =0
    mov(isr, x)         # current count value -> Rx fifo
    push(noblock)		# do not wait if fifo full
    in_(pins, 1)		# read A into isr, shifting A' to 2nd bit posn
# decide, using Y value, if there is a pulse to count
    mov(y, isr) 		# save isr into Y
    jmp(not_y, 'read')	# test if =00; -> no action
    jmp(y_dec, 'count')	# test if =01; -> count 1 pulse (0->1 transition)
    jmp('read')			# value is 10 or 11; -> no action
# there is a pulse.  Decide if Fwd or Rev, and Inc or Dec the counter
    label('count')		# do we count up or down?
    jmp(pin,'cntup')	# test 2nd (direction) pin (defined in sm init)
    label('cntdn')
    jmp(x_dec, 'read')	# do count decrement here, then done
    label('cntup')
    mov(x, invert(x))   # Increment x by calculating x=~(~x - 1)
    jmp(x_dec,'cntup1')	#    (=invert, decrement, invert)
    label('cntup1')
    mov(x, invert(x))
# at this point isr bit 0 contains A', previous value of encoder input
    jmp('read')			# end of loop.  Repeat

RightMotorEncoder = StateMachine(0, encoder, freq=1000000, in_base=REncApin, jmp_pin=REncBpin)
LeftMotorEncoder = StateMachine(1, encoder, freq=1000000, in_base=LEncApin, jmp_pin=LEncBpin)

def twos_comp(val, bits):
    if (val & (1 << (bits - 1))) != 0:          # if sign bit is set e.g., 8bit: 128-255
        val = val - (1 << bits)                 # compute negative value
    return val                                  # return positive value as is

def QueryEncoders(): 
    global PrevLEnc, PrevREnc	# encoder counts, last time
    global LeftVel, RightVel	# wheel velocities, units = ???
    global LEE, REE				# encoder values, in counts

    k = 0
    while (LeftMotorEncoder.rx_fifo() > 0) and (k < 4):    # empty FIFO - last value used
        k += 1
        LEE = LeftMotorEncoder.get()
    if k > 0:
        LEE = twos_comp(LEE, 32)
        LE = LEE - PrevLEnc
        PrevLEnc = LEE
    else:
        LE = 0

    k = 0
    while (RightMotorEncoder.rx_fifo() > 0) and (k < 4):  
        k += 1
        REE = RightMotorEncoder.get()
    if k > 0:
        REE = twos_comp(REE, 32)
        RE = REE - PrevREnc
        PrevREnc = REE
    else:
        RE = 0
    
    dataV[0], dataV[1] = LE/ Counts2Vel, RE/Counts2Vel


def cbEncSampleTimer(t):
    QueryEncoders()
    
RightMotorEncoder.active(1)
LeftMotorEncoder.active(1)

EncSampleTimer = Timer(period=EncoderSamplePeriod , mode=Timer.PERIODIC, callback=cbEncSampleTimer)