\ \\\\\\\\\\\\\\\\\\ RANGER CONTROL FUNCTIONS \\\\\\\\\\\\\\\\\\\\\\\\
\ Requires real time clock functions (rtc.fs) and
\ turret control functions (turret.fs) to be loaded first
HEX                          \ All numerical values are in hexidecimal format

\ Define an array of cells
: ARRAY VARIABLE 2* ALLOT GOES> SWAP 2* + ;

\ Turret ranger initialization function
: RANGER-INIT ( -- IMR CR IBC )
    IMR@ DUP                 \ Save the interrupt mask register
    EI5 OR IMR!              \ Mask the SUART interrupt (EI5)
    CR@                      \ Save the configuration register
    IBC@ DUP                 \ Save the interrupt base/control register
    0300 NOT AND             \ Clear the timer/counter bits (IBC(9:8))
    0100 OR IBC!             \ Set up TC0 to use EI3 as clock source (IBC(9:8))
;

\ Turret ranger halt function
: RANGER-HALT ( IMR CR IBC -- )
    IBC!                     \ Restore the IBC register
    CR!                      \ Restore the configuration register
    IMR!                     \ Restore the SUART interrupt (EI5)
;

7FFF CONSTANT CINIT          \ Starting value for range measurement counter
46 ARRAY RANGES              \ 70 (base 10) cell array to hold results

: TURRET-RANGER ( -- )
    WAIT-FOR-NEXT-TICK       \ Wait until a timer tick occurs
    CR@ 0008 NOT AND CR!     \ Clear the BOOT pin
    46 0 DO                  \ Step 70 (base 10) times rotates turret 360 deg.
	14 S>D WAIT-N-TICKS  \ Wait for 20 ticks (100 ms) 
	CINIT TC0!           \ Re-init Counter 0 start value
	14 S>D WAIT-N-TICKS  \ Wait for 20 ticks (100 ms) to transpire
	TC0@ I RANGES !      \ Get the new count value
	1 FULL-STEP
    LOOP
    CR@ 0008 OR CR!          \ Set the BOOT pin
    6 S>D WAIT-N-TICKS       \ Wait for 6 ticks to pass (ranger shutdown)
;

: TURRET-RANGER-TEST ( -- code )
    RTC-INIT                 \ Initialize the real time clock
    CHECK-TURRET-INDEX       \ Determine if turret is already aligned
    DUP 0<> IF               \ If the turret is not already aligned...
	DROP                 \ Discard the flag value
	ALIGN-TURRET         \ Perform turret alignment
    THEN
    DUP 0= IF                \ If the turret alignment was successful...
	DROP                 \ Discard the flag value
	RANGER-INIT          \ Perform turret ranger initialization
	TURRET-MOTOR-ON      \ Turn on the turret motor
	TURRET-RANGER        \ Perform the turret ranger function
	TURRET-MOTOR-OFF     \ Turn off the turret motor
	RANGER-HALT          \ Stop turret ranger configuration
	CHECK-TURRET-INDEX   \ Determine if turret is still aligned
	NOT                  \ Flip flag state, as 0 is success
    THEN
    RTC-HALT                 \ Halt the real time clock
;

: NORMALIZE-RANGES ( -- )
    0 RANGES @ DUP           \ Initial min and max values are first range value
    46 1 DO                  \ Do-loop for the other 69 range values
	I RANGES @ DUP       \ Read in a range value and duplicate it
	ROT MAX              \ Bring current max to TOS and find new max
	-ROT                 \ Push new max behind current min and range value
	MIN SWAP             \ Find new min and bring new max to TOS
    LOOP                     \ Loop on finding max and min
    OVER -                   \ Copy min and find max and min difference (diff)
    46 0 DO                  \ Do-loop to normalize all 70 range values
	OVER                 \ Copy the min value to TOS
	I RANGES @           \ Read in a range value
	SWAP -               \ Subtract the min from the range value (rdiff)
	OVER                 \ Copy the diff value to TOS
	100 -ROT             \ Load normalization max value and rotate back
	*/                   \ Normalize range value (nmax * rdiff / diff)
	I RANGES !           \ Save the normalized range value
    LOOP                     \ Loop on normalizing the range values
    DROP DROP                \ Discard diff and min
;

DECIMAL
: RANGE-DUMP ( -- )
    DECIMAL
    70 0 DO
	I RANGES @ . CR
    LOOP
    HEX
;
HEX

\ \\\\\\\\\\\\\\\\ END OF RANGER CONTROL FUNCTIONS \\\\\\\\\\\\\\\\\\\\\