TMS570LC4357: How to properly set up the SWAG?

Part Number: TMS570LC4357

Hello, I've been fighting with the Software Angle Generator for quite some time. I made this topic previously: https://e2e.ti.com/support/microcontrollers/hercules/f/312/t/826325

In that topic I had thought I had figured it out, only to later find out I was making an error.

I still can not get the SWAG to function properly. Here is my current code:

TOOTH_PIN	.equ 2
OUT_PIN	        .equ 18

ACTUAL_TEETH .equ 58
MISSING_TEETH .equ 2
TEETH_SKIP .equ ACTUAL_TEETH - 1 
STEP_FACTOR .equ 8
GAP_START .equ ((STEP_FACTOR * (ACTUAL_TEETH - 1)) + 1)
GAP_END .equ (((STEP_FACTOR * (ACTUAL_TEETH + MISSING_TEETH )) - 1))

START   BR { cond_addr=START_00,event=NOCOND};0
START_00   APCNT { type=FALL2FALL,control=OFF,prv=OFF,period=0};1
START_0   BR { cond_addr=WAIT_FOR_LOW,event=NOCOND};2

WAIT_FOR_LOW   BR { next=START,cond_addr=WFL1,pin=2,event=HIGH};3
WFL1           MOV64 { next=START,remote=START_0,cond_addr=CALC_PERIOD,comp_mode=ECMP,reg=NONE};4

; FIFO holding last three measurements  (S01 is oldest, S03 is newest)
CALC_PERIOD   BR { next=START,cond_addr=S01,pin=2,event=FALL};5
S01   ADD  {src1=REM, src2=ZERO, dest=IMM, data=999999, hr_data=0, remote=S02}  ;N2 = S02 + 0
S02   ADD  {src1=REM, src2=ZERO, dest=IMM, data=999999, hr_data=0, remote=S03}  ;N1 = S03 + 0
S03   ADD  {src1=T,   src2=ZERO, dest=IMM, data=999999, hr_data=0}              ;N  = T + 0
S04   MOV64 { next=START,remote=START_0,cond_addr=COMPARE_PERIOD,comp_mode=ECMP,reg=NONE};

COMPARE_PERIOD   ADD { src1=REM,src2=ZERO,dest=R,remote=S01,data=0}; R = N2
S05   ADD { src1=REM,src2=R,dest=R,remote=S02,data=0}; R = N2 + R
S06   ADD { src1=R,src2=ZERO,dest=NONE,rdest=REM,remote=S07,data=0};12
S07   MCMP { en_pin_action=ON,cond_addr=BRANCH_0,pin=4,order=REG_GE_DATA,action=SET,reg=T,data=99999,hr_data=0};13
S08   MOV64 { next=START,remote=START_0,cond_addr=CALC_PERIOD,comp_mode=ECMP,reg=NONE};14
BRANCH_0   MOV64 { next=START,remote=START_0,cond_addr=SWAG_PREP1,comp_mode=ECMP,reg=NONE};15

SWAG_PREP1 BR { next=START,cond_addr=SP1,pin=TOOTH_PIN,event=HIGH};16
SP1        MOV64 { next=START,remote=START_0,cond_addr=SWAG_PREP2,comp_mode=ECMP,reg=NONE};

SWAG_PREP2 BR { next=START,cond_addr=SP20,pin=TOOTH_PIN,event=FALL};18
SP20   ADD { src1=ZERO,src2=ZERO,dest=R,data=0};
SP200      MOV64 { remote=START,cond_addr=START_0,comp_mode=ECMP,reg=NONE};
SP21        MOV64 { next=START,remote=START_0,cond_addr=WAIT_ONE_REV,comp_mode=ECMP,reg=NONE};

WAIT_ONE_REV   ECNT { pin=TOOTH_PIN,event=FALL,reg=R,data=0};22
WOR0   MCMP { next=START,cond_addr=WOR00,pin=0,order=REG_GE_DATA,reg=R,data=TEETH_SKIP};
WOR00   MOV64 { remote=START,cond_addr=START_00,comp_mode=ECMP,reg=NONE};
WOR1   MOV64 { next=START,remote=START_0,cond_addr=SWAG,comp_mode=ECMP,reg=NONE};

; Angle Counter 
SWAG   SCNT { step=STEP_FACTOR,gapstart=GAP_START,data=0}; 26
SG_01   ACNT { edge=FALLING,irq = ON, gapend=GAP_END,data=0};27
OUT20   ACMP { next=START,en_pin_action=ON,cond_addr=OUT20_PULSE_LOW,pin=OUT_PIN,action=SET,reg=B,data=0};22
; Switch OUT20
OUT20_PULSE_LOW    MOV64 { next=START,remote=OUT20,en_pin_action=ON,cond_addr=OUT20_PULSE_HIGH,pin=OUT_PIN,comp_mode=ECMP,action=CLEAR,reg=B,data=0};
OUT20_PULSE_HIGH   MOV64 { next=START,remote=OUT20,en_pin_action=ON,cond_addr=OUT20_PULSE_LOW, pin=OUT_PIN,comp_mode=ECMP,action=SET,  reg=B,data=0};

Here is the code with some description of what it's doing:

And this is what my results look like:

As you can see, the results are incorrect. 

I'm hoping someone can point me in the right direction and help me to figure out how to configure the SWAG properly. Has anyone been able to get it to work?