'Laser Lock - Main Controller.SXB
'-------------------------------------------------------------------------------------
'Overview:
'-------------------------------------------------------------------------------------

'The main controller manages the ADC, the DAC, and the peripheral controllers via a 
'modified SPI bus, reads in the output adjust/lock level adjust knob, lock set, lock 
'release buttons, and the external override signal.  It also performs the math 
'required to undergo the active feedback.


'-------------------------------------------------------------------------------------
'Notes
'-------------------------------------------------------------------------------------

'MAX525 programming notes:
'------------------------

'the MAX525 receives 16-bit instructions, with the following structure

'----------------------------------------------Address register select 1
'|  -------------------------------------------Address register select 0
'|  |  ----------------------------------------command register 1
'|  |  |  -------------------------------------command register 0
'|  |  |  |  ---------------------------------/12-bit data to be written to address
'|  |  |  |  |  |  |  |  |  |  |  |  |  |  |  | 
'A1 A0 C1 C0 DB DA D9 D8 D7 D6 D5 D4 D3 D2 D1 D0

'useful commands include:
'C = %01			-Load input register A, don't update DAC register 
'C = %11			-Load input register A, change DAC register
'C = %00, A = %01, D = X	-Update all DAC registers from their input registers
'C = %00, A = %10		'Load all DAC registers
'C = %00, A = %11, D = X	'shutdown (provided /PDL = 1)
'C = %10, A = %00, D = X	'set user-programmable output low
'C = %10, A = %01, D = X	'set user-programmable output high
'C = %00, A = %00, D = X	'NOP (no operation)
'C = %10, A = %11, D = X	'set SPI DOUT to mode 1: DOUT clocked out on SCLK /
'C = %10, A = %10, D = X	'set SPI DOUT to mode 0: DOUT clocked out on SCLK \ 


'Optical encoder encoder waveform notes:
'--------------------------------------

'the optical encoder is a quadrature encoder, meaning the knob has four measurable 
'position states given by two digital output values:

'position	0	1	2	3
'OA		low	high	high	low
'OB		low	low	high	high

'change and direction can be sensed by observing how these values change.


'TLC2543 ADC notes:
'-----------------
'-the TLC2543 uses a SPI connection to transfer data through a synchronous serial 
'peripheral interface connection
'-according to the timing diagram in the spec sheets:
'	-the MSB is sent first (MSB)
'	-the first bit to be sent is present before the first clock pulse (PRE)
'-due to this timing, the correct method for SHIFTIN/SHIFTOUT
'to use is MSBPRE, which corresponds to mode 0
'-control byte format
'   
'                         /Address select:
'                         |     - 0 to A: channels 0 to A
'  +--+--+--+-------------|     - B: (Vref+ - Vref-)/2
'  |  |  |  |             |     - C: Vref-
'  |  |  |  |             |     - D: Vref+
'  |  |  |  |             \     - E: Power down mode
'  |  |  |  |
'  |  |  |  |             /Output data length:
'  |  |  |  |  +--+-------|	- 0 1: 8 bits
'  |  |  |  |  |  |	  |	- X 0: 12 bits
'  |  |  |  |  |  |	  \	- 1 1: 16 bits
'  |  |  |  |  |  |
'  |  |  |  |  |  |  +----MSB first(0) or LSB first(1)
'  |  |  |  |  |  |  |  +-Unipoloar(0) or Bipolar(1) mode
' D7 D6 D5 D4 D3 D2 D1 D0

'-------------------------------------------------------------------------------------
'Speed selection and device initialization
'-------------------------------------------------------------------------------------
'uncomment these two lines for debug speed: 4 MHz internal clock
'DEVICE	SX28, OSC4MHz, TURBO, STACKX, OPTIONX
'FREQ	4_000_000

'uncomment these two lines for operating speed: 50 MHz external clock (need to use the 
'run/program switch on board)
DEVICE	SX28, OSCHS3, TURBO, STACKX, OPTIONX
FREQ	50_000_000
'this is used for timing-specific parts of the code, like PAUSE
'this is also used by the SX-KEY, as it generates a clock for debugging
'purposes

'-using an SX28 device, with external clock of 50 MHz
'-TURBO refers to the operation mode, where one clock cycle per
'instruction.
'-STACKX, OPTIONX required for SX28 only - when programming SX48, TURBO, STACKX, 
'and OPTIONX are assumed

'The device directives OSCLP1, OSCLP2, OSCXT1, OSCXT2, OSCHS1, OSCHS2 and OSCHS3 
'correspond to the following settings shown in the SX datasheet (taken from the SX28 
'here, but SX48 is the same):

'FOSC2: FOSC0 External oscillator configuration (valid when IRC = 1):
'	000b = LP1 - low power crystal (32KHz)
'	001b = LP2 - low power crystal/resonator (32 KHz to 1 MHz)
'	010b = XT1 - normal crystal/resonator (32 KHz to 10 MHz)
'	011b = XT2 - normal crystal/resonator (1MHz to 24 MHz)
'	100b = HS1 - high speed crystal/resonator (1 MHz to 50 MHz)
'	101b = HS2 - high speed crystal/resonator (1 MHz to 50 MHz)
'	110b = HS3 - high speed crystal/resonator (1 MHz to 50 MHz)


'-------------------------------------------------------------------------------------
'Pin Assignment
'-------------------------------------------------------------------------------------

'data/clock lines on the serial bus:
'----------------------------------

Miso			pin	RB.0	input	'master in, slave out serial line
Mosi			pin	RB.1	output	'master out, slave in serial line
SPIClk			pin	RB.2	output	'SPI clock, generated by master
AttnAck			pin	RB.3	input	'Slave attention acknowledge to ensure 
						' proper data transfer
'NOTE: pullup resistor required on AttnAck pin

'chip select lines on the serial bus
'NOTE: these use inverted logic, so they must be held high when not in use
CS			pin	RA	output	'0: memory controller
						'1: display controller	
						'2: ADC
						'3: DAC

'external inputs: 
'---------------

'optical encoder voltage lines
'NOTE: due to capacity issues, had to directly code this where it was needed
'OA			pin	RB.5	input
'OB			pin	RB.6	input

LockSet			pin	RB.7	input	'lock set button press input
LockRelease		pin	RC.0	input	'lock release button press input
						'external override input
ExtOverride		pin	RB.4	input	pullup
'NOTE: for some reason, this behaves like a pulldown...?

'debug pin, useful for tracing thru the code and observing buggy behaviour
Debug 			pin 	RC.2	output	

'RS232 communication I/O:
'-----------------------
RxD			Pin	RC.4	input	'serial receive line (input)
TxD			Pin	RC.5	output	'serial transmit line (output)
CTS			Pin	RC.7	output	'CTS response to computer (output)
RTS			Pin	RC.6	input	'RTS request from computer (input)


'-------------------------------------------------------------------------------------
'Variable Declaration
'-------------------------------------------------------------------------------------

'this status byte is used to store information about the locking behaviour - internal,
' not to be displayed
LockingStatus		var	byte

'flags when the display needs to be updated
UpdateDisplay		var	LockingStatus.1	
'stores the current tune direction requested by the active feedback
CurrentTuneDirection	var	LockingStatus.2
'stores the past tune direction requested by the active feedback
PastTuneDirection	var	LockingStatus.3
'stores the sign of the LHS of the active feedback comparison calculation
NegLHS			var	LockingStatus.4	
'stores the sign of the RHS of the active feedback comparison calculation
NegRHS			var	LockingStatus.5
'set high when the optical encoder knob has been increased
EncoderIncrease		var	LockingStatus.6
'set high when the optical encoder knob has been decreased
EncoderDecrease		var	LockingStatus.7

'current system status flags stored in this byte.  Thse flags are displayed on
'the display controller's Status LED bank, and are used to perform operations
SystemStatus		var	byte

'set high when the lock levels have been set and locking is enabled
LockEnabled		var	SystemStatus.0
'set high every time the system increases the voltage
TuningUp		var	SystemStatus.2
'set high every time the system decreases the voltage
TuningDown		var	SystemStatus.1
'flagged when the tuning voltage reaches its limit.
TuneOverload		var	SystemStatus.3
'flagged when the external override voltage is set high during active lock
ExtOverrideEnabled	var	SystemStatus.4
'flagged when the input intensity drops too low to use during active lock
LostSumSignal		var	SystemStatus.5

'scratchpad variables - used for calculations along with __Param storage registers
Scratch0		var	byte

'NOTE: everything after this is on paged memory - this means that it can only be 
'accessed if proper care is taken to set the page to the register.  inter-page data 
'operations will need scratchpad or W use in order to function correctly
Scratch1		var	byte
Scratch2		var	byte
Scratch3		var	byte
Scratch4		var	byte
Scratch5		var	byte
Scratch6		var	byte
Scratch7		var	byte

'this is used to hold the upper 16 bits of a multiplication operation for two-byte 
'values
TwoByteOver		var	byte(2)

'parameters that govern laser lock interface and feedback behaviour.  These are set 
'and controlled by the display controller

'the change in voltage to change in knob
OutputAdjGain		var	byte	
'the change in lock level to change in knob
LockLevelAdjGain	var	byte	
'the change in voltage to change in active tuning
LockAdjGain		var	byte	
'the "inertia", which is used to assess whether to adjust the output voltage when 
' actively locking
TuneInertia		var	byte	
'the running counter that runs from 0 to TuneInertia before an output voltage 
' adjustment is performed when actively locking
CurrentTuneInertia	var	byte	


'two-byte parameter storing the number of "samples" to take during running average
Samples			var	byte
SamplesMSB		var	byte		'unused MSB of samples word, for proper
						' 16-bit arithmetic

'this stores the state of the optical encoder knob, and is used to detect change
OldState		var	byte		
'TODO: this only uses two bits, so maybe move this into upper two bits of SystemStatus
' to save memory?

'running indices for loops and such
'i			var	byte
'j			var 	byte

'DAC feedback output value: this is a 12-bit value, ranging from $000 to $FFF.  It 
' controls a 12-bit DAC in a bipolar configuration, where the midrange value ($7FF) 
' equals zero output voltage wrt. common
VOut			var	byte(2)

'Locking parameter "two-byte" numbers.  They are essentially word data types, 
' with manual code for manipulation.  These numbers are stored LSB first, MSB last 
' (little endian format)
Top			var	byte(2)		'top half intensity
Bottom			var	byte(2)		'bottom half intensity
Sum			var	byte(2)		'running average of SampSum
Diff			var	byte(2)		'running average of SampDiff
SampSum			var	byte(2)		'Top + Bottom
SampDiff		var	byte(2)		'Top - Bottom (bipolar)
LockSum			var	byte(2)		'locked sum - stored from sum when
						'lock level is set
LockDiff		var	byte(2)		'locked difference - stored from 
						'difference when lock level is set

'these are used to compare the current sampled status to the lock level status, to 
' determine feedback action
LHS			var	byte(2)
RHS			var	byte(2)

'poll count: used to delay the time between regular polling of the display controller, 
' for updating the status and user-defined parameters
pollcount		var	byte(3)

'button debounce delay counter
'debounce 		var	byte
'NOTE: this has been removed because it is not needed.  If needed for future project,
' reinstate.

'storage for data packets passed between devices on the serial bus, the RS232 
' connection, and for big number crunching
packet			var	byte(10)


'-------------------------------------------------------------------------------------
'Constants Declaration
'-------------------------------------------------------------------------------------

'synchronous serial clock speed multiplier
spdmlt			con	100

'this determines how many bytes are used in the poll delay counter.  Range: 1 to 3
PollDelayCoarse		con	$02	
'this determines the value of the MSB of the poll delay counter.  Range $0 to $FF
PollDelayFine		con	$02	


'-------------------------------------------------------------------------------------
'Subroutine Definitions
'-------------------------------------------------------------------------------------

'performs a packet write operation to the specified device using asynchronous serial
' SEROUT commands.  A boolean flag specifies if handshaking is required on the AttnAck
' line
ASyncPacketWrite	sub	1	

'performs a packet read operation from the specified device using asynchronous serial 
' SERIN commands.
ASyncPacketRead		sub	1	

'writes packet to the RS232 port
'RS232Write		sub 	0	
'NOTE: removed for space considerations

'reads a packet from the RS232 port (assumes RTS handshaking was already detected)
'RS232Read		sub	0	
'NOTE: removed for space considerations

'this sets DAC channel #1 to the value in VOut
SetDac			sub	0	

'this reads the specified ADC channel and stores the value in the packet array
ReadADC			sub	0	

'reads from memory controller at specified address
ReadFromMemory		sub	2
	
'writes a byte to specified address
WriteToMemory		sub	3	

'performs initialization communication of the display controller SX chip
InitDisplayController	sub	0	

'updates the display controller with the current system status
UpdateDisplayController	sub	0	

'reads in properties from display controller and updates memory controller
GetDisplayProperty	sub	2	

'reads in the number of properties that the displaycontroller is managing
GetDisplayPropCount	sub	0	

'adds two referenced "two-byte" numbers and stores result in referenced "two-byte" 
' number
Sum2b			sub	2,3	

'adds referenced two-byte value to literal byte value
Sum2b1b			sub	2	

'subtracts two referenced "two-byte" numbers and stores result in referenced 
' "two-byte" number
Diff2b			sub	2,3	

'subtracts literal byte value from referenced two-byte value 
Diff2b1b		sub	2	

'multiplies referenced "two-byte" number by specified value, and stores in specified 
' destination.  Handles overflow into TwoByeOver()
Mult2b			sub	3	

'divides referenced "two-byte" number by specified value
Divide2b		sub	3	

'flips the sign of a two-byte number
FlipSign2b		sub	1	

'sets value in 1st argument to value in 2nd argument
Equate2b		sub	2	

'reads if there has been a change in the optical encoder position
GetEncoderChange	sub	0	

'sets value in __Param1 equal to literals passed in __Param2 (MSB), __Param3 (LSB)
Set2b			sub	3

'clears the TwoByteOver value
ClearTBO		sub	0

'assembly language serial in function, to conserve space
AsmSerIn		sub	1

'assembly language synch serial in function, to conserve space
AsmShiftIn		sub	2

'-------------------------------------------------------------------------------------
'Main Program
'-------------------------------------------------------------------------------------

'The PROGRAM directive specifies the label where the program begins.  In this case, 
' "Start" is the label specified.  This also specifies where to put the internally 
'  generated SX initialization code.
PROGRAM Start 'NOSTARTUP
'NOTE: The NOSTARTUP option removes startup code like memory clearing.  
' Good for assembly-level debugging

'the Start label where code execution begins
Start:

'Initialization:
'--------------

'set all chip select outputs high
CS = %1111
'set the DAC to the midrange value

'set the RS232 clear to send handshaking line to the inactive state
'CTS = 1

Set2b	@Vout(1),	$7,	$FF
SetDac

'1 second pause to let other controllers fully initialize themselves
pause 1000

'test and read properties from memory controller
'this performs an initialization on the memory controller.  It tests the 
' memory by writing a value to a specified address, and checking to see if it comes 
' back the same.  This will fail if there is a communication problem, or if the NVRAM 
' is damaged or not present

'perform memory read/write test
WriteToMemory $FF, $03, $AA
ReadFromMemory $FF, $03		'returned value is passed in __Param1 by 
					' subroutine
'if read/write is success, then proceed
if __Param1 = $AA then
	OutputAdjGain = ReadFromMemory $01, $00
	LockLevelAdjGain = ReadFromMemory $02, $00
	LockAdjGain = ReadFromMemory $03, $00
	TuneInertia = ReadFromMemory $04, $00
	Samples = ReadFromMemory $05, $00
else
	'send error message to display controller
	
	'this causes the display controller to flash all display LED's.
	'packet(0) = 3
	'packet(1) = $FF				'command byte $FF: error
	'packet(2) = $FF				'error code #FF: memory error
	asm
		MOV FSR, #Packet+0
		MOV IND, #3
		INC FSR
		MOV IND, #$FF
		INC FSR
		MOV IND, #$FF
		Bank $00
	endasm

	'write to the memory controller
	ASyncPacketWrite	1		
	'the main controller will shut down here.
	sleep					
endif

'store the current position of the optical encoder
GetEncoderChange

InitDisplayController

'Main Execution Loop:
'-------------------
MainLoop:

'sample the ADC values into the packet() array
ReadADC

'determine the top and bottom intensities for this sample
'NOTE: pass the MSB of the "two-byte" number for this to work properly
Sum2b	@packet(2),	@packet(6),	@Top(1)
Sum2b	@packet(4),	@packet(8),	@Bottom(1)

'combine the top and bottom to give the sum and difference for this sample
Sum2b	@Top(1),	@Bottom(1),	@SampSum(1)
Diff2b	@Top(1),	@Bottom(1),	@SampDiff(1)

'averaging code to produce a smoothed value: the pseudo-average system does the 
' following math:

'new algorithm to reduce truncation errors when dividing:
'Sum = ( (N-1)*Sum + SampSum )/N

'This weighted sum acts as if there is an average over N values, where N-1 of them are
' stored in Sum, and the last value comes from the latest sample.  

'offset the difference signal by $20_00 for unsigned math (easier in this step)
Scratch0 = 0
Scratch1 = $10
Sum2b		@SampDiff(1),	@Scratch1,	@SampDiff(1)
Sum2b		@Diff(1),	@Scratch1,	@Diff(1)

ClearTBO
'determine correction function for sum
Divide2b	@SampSum(1),	@SamplesMSB,	@Scratch7

if Scratch6 >= 10 then		
	's + .5*N = s + 5 * N / 10
	asm
		MOV FSR, #Packet+0
		MOV IND, #5
		INC FSR
		CLR IND
		BANK $00
	endasm
	Mult2b	@Packet(1),	@SamplesMSB, 	@Packet(3)
	Packet(0) = 10
	Divide2b @Packet(3), 	@Packet(1),	@Packet(3)

	'finally, add correction into sample sum
	Sum2b	@SampSum(1),	@Packet(3),	@SampSum(1)
else
	if Scratch6 >= 4 then
		'.95*s + N = 95 * s / 100 + N
		asm
			MOV FSR, #Packet+0
			MOV IND, #95
			INC FSR
			CLR IND
			BANK $00
		endasm
		Mult2b @Packet(1), 	@SampSum(1),	@SampSum(1)
		Packet(0) = 100
		Divide2b @SampSum(1),	@Packet(1),	@SampSum(1)
		Sum2b	@SampSum(1), 	@SamplesMSB,	@SampSum(1)
	endif
endif

'decrement Samples value by 1 to perform the (N-1)/N product
Samples = Samples - 1

Mult2b 		@Sum(1),	@SamplesMSB,	@Sum(1)
Sum2B		@Sum(1),	@SampSum(1),	@Sum(1)


'bring the value of Samples back up to N so you can divide by N
Samples = Samples + 1

Divide2b	@Sum(1),	@SamplesMSB,	@Sum(1)

'now do the same for difference signals

ClearTBO
'determine correction function for difference
Divide2b	@SampDiff(1),	@SamplesMSB,	@Scratch7

'lower part of correction function and store in Packet(2), Packet(3)
if Scratch6 >= 10 then		
	's + .5*N = s + 5 * N / 10
	asm
		MOV FSR, #Packet+0
		MOV IND, #5
		INC FSR
		CLR IND
		BANK $00
	endasm
	Mult2b	@Packet(1),	@SamplesMSB, 	@Packet(3)
	Packet(0) = 10
	Divide2b @Packet(3), 	@Packet(1),	@Packet(3)

	'finally, add correction into sample sum
	Sum2b	@SampDiff(1),	@Packet(3),	@SampDiff(1)
else
	if Scratch6 >= 4 then
		'.95*s + N = 95 * s / 100 + N
		asm
			MOV FSR, #Packet+0
			MOV IND, #95
			INC FSR
			CLR IND
			BANK $00
		endasm
		Mult2b @Packet(1), 	@SampDiff(1),	@SampDiff(1)
		Packet(0) = 100
		Divide2b @SampDiff(1),	@Packet(1),	@SampDiff(1)
		Sum2b	@SampDiff(1), 	@SamplesMSB,	@SampDiff(1)
	endif
endif

'decrement Samples value by 1 to perform the (N-1)/N product
Samples = Samples - 1

Mult2b 		@Diff(1),	@SamplesMSB,	@Diff(1)
Sum2B		@Diff(1),	@SampDiff(1),	@Diff(1)

'bring the value of Samples back up to N so you can divide by N
Samples = Samples + 1

Divide2b	@Diff(1),	@SamplesMSB,	@Diff(1)

ClearTBO

'return the Diff value to its original position so it can be treated as a signed 
' number once again
Scratch0 = 0
Scratch1 = $10
Diff2b		@Diff(1),	@Scratch1,	@Diff(1)

'check for a change on the optical encoder.  Status flags will be set here
GetEncoderChange

'if locking is enabled, perform active lock calculations, and update DAC accordingly
if LockEnabled = 1 then

	'Status exceptions code:
	'----------------------

	'check for button use 
	'if debounce > 0 then		'dead time for right after button press
		'debounce = debounce - 1

	'if Lock Release button is pressed
	if LockRelease = 1 then
		'reset the debounce timer
		'debounce = $FF

		'reset the status flags and update the display controller
		SystemStatus = 0
		LockingStatus = 0
		UpdateDisplayController

		'In the event that the DAC has exceeded the valid range, impose a 
		' correction to bring it back
		if VOut(1) > $F then
			'check if it's a negative number
			if VOut(1) > $F0 then
				Set2b 	@VOut(1),	$0,	$0
			else
				Set2B	@Vout(1),	$F,	$FF
			endif
			SetDAC
		endif

		goto MainLoop

	endif

	'ClearTBO

	'check the ratio of lock intensity with the current intensity
	'note: scratch3 passed here because it is one byte higher than Scratch2
	Divide2B	@LockSum(1),	@Sum(1), 	@Scratch3
	
	'if the intensity has dropped by more than a factor of two, then flag the
	' signal has been lost, and stop trying to lock
	if Scratch2 > 1 then
	
		'if it hasn't already been flagged
		if LostSumSignal = 0 then
			'flag it and update status on display controller
			LostSumSignal = 1
	
			UpdateDisplayController
		endif	
		

		'if there is a lost sum signal, then don't perform the calculation
		goto MainLoop

	else
		'if a lost sum signal has been previously flagged 
		if LostSumSignal = 1 then
			'unflag it and resume display
			LostSumSignal = 0
			
			UpdateDisplayController
		endif
	endif

	'if the external override has been enabled
	if ExtOverride = 1 then
	
		'if it hasn't already been flagged
		if ExtOverrideEnabled = 0 then
			'flag it and update status on display controller
			ExtOverrideEnabled = 1
	
			UpdateDisplayController
		endif	
		

		'external override keeps the locking mechanism from performing
		goto MainLoop

	else
		if ExtOverrideEnabled = 1 then
			'unflag it and resume display
			ExtOverrideEnabled = 0
			
			UpdateDisplayController
		endif
	endif

	'Lock Level Adjust code: this adjusts the lock difference value when the 
	'user adjusts the optical encoder
	if EncoderIncrease = 1 then
		EncoderIncrease = 0
		Sum2b1b		@LockDiff(0),	LockLevelAdjGain
	elseif EncoderDecrease = 1 then
		EncoderDecrease = 0

		Diff2b1b	@LockDiff(0), 	LockLevelAdjGain
	endif

	'Active feedback code:
	'--------------------

	'first, store the signs of the difference and lock difference values,
	'and set them to be unsigned numbers.  This makes the comparison math
	'considerably easier.
	
	if Diff(1) > $7F then
		NegLHS = 1			'flag that the LHS is negative
		FlipSign2b	@Diff(1)	'flip the sign
	endif	

	if LockDiff(1) > $7F then
		NegRHS = 1			'flag that the RHS is negative
		FlipSign2b	@LockDiff(1)	'flip the sign
	endif	

	'perform the following comparison:
	'LHS = (Lock sum) * (difference)  
	'RHS = (sum) * (Lock difference)
	'compare LHS to RHS, whichever one is bigger will determine which way to go

	'LHS calculation, store result in packet(1) through packet(4)
	Mult2b		@LockSum(1),	@Diff(1),	@Packet(2)
	Equate2b	@Packet(4),	@TwoByteOver(1)

	'RHS calculation, store result in packet(5) through packet(8)
	Mult2b		@Sum(1),	@LockDiff(1),	@Packet(6)
	Equate2b	@Packet(8),	@TwoByteOver(1)

	'this compares the LHS to the RHS using efficient pointer use
	asm
		CLRB CurrentTuneDirection

		MOV __Param3,#packet+5              	'load the address of LHS, +1
        	MOV __Param4,#packet+9			'load the address of RHS, +1
		MOV __Param5, #5			'load counter with 5, so we 
							' can check over 4 bytes

		'start here, or jump back here if the last compare bytes were equal
		MOV W, #1				'decrement all pointers by 1
		Sub __Param3, W
		Sub __Param4, W
		Sub __Param5, W				'decrement the counter
		JNZ @$+7				'if the counter hits 0, we've 
							' checked all bytes and they 
							' all are equal

		'if all bytes are equal
		BANK $00
		CLR CurrentTuneInertia			'reset the tune intertia 
							' counter to 0
		JMP  @$+19				'jump ahead to final BANK

		'otherwise, jump to here, load and check next byte
		MOV FSR, __Param3			'point to LHS byte
		MOV __Param1, IND			'load LHS value into __Param1
		MOV FSR, __Param4			'point to RHS byte
		MOV __param2, IND			'load RHS value into __Param2
		
		CSNE __Param1,__Param2			'check if the bytes are equal, 
							' skip next line if they are 
							' not equal
		JMP  @$-22				'if they're equal, jump back 
							' and check next byte

		'TODO: check polarity and reverse here if necessary
		'CSB __Param1, __Param2
		CSA __Param1, __Param2			'check if LHS > RHS.  If it
							' is, skip next line
	
		SETB CurrentTuneDirection		'set the tune direction
			
		'jump to here if all bytes equal and we're done
		BANK $00

	endasm

	'now, take into account the sign of the LHS, RHS

	Scratch5 = NegLHS + NegRHS
	'if this value equals 0, then no sign flip took place
	'if this value equals 1, then one sign was flipped, and so the LHS and RHS 
	' have opposite sign
        'if this value equals 2, then they both have flipped signs, and so the 
	' LHS = RHS comparison needs opposite action

	if Scratch5 = 1 then		'if one is negative, one is positive
		
		'TODO: check polarity here, and replace with opposite one if necessary
		CurrentTuneDirection = NegLHS
	endif

	'if they're both negative, then we need to invert the result of the comparison
	if Scratch5 = 2 then
		CurrentTuneDirection = ~CurrentTuneDirection
	endif

	'now, check whether it was tuning up or down on previous reading
	Scratch5 = CurrentTuneDirection + PastTuneDirection
	'NOTE: this is a crude XOR of two bit values stored into a byte value
	'TODO: define ScratchB0 through ScratchB7 to perform bitwise scratchpad work?

	'if Scratch5 equals 2 or 0, the current and past direction matches.  
	' Otherwise, the sign has changed
	if Scratch5 = 1 then
		'reset the counter, and point the past tune direction in the current 
		' direction
		CurrentTuneInertia = 0
		PastTuneDirection = CurrentTuneDirection
	else
		'increment the counter	
		CurrentTuneInertia = CurrentTuneInertia + 1

		'if there is a voltage overload error, then don't tune
		if TuneOverLoad = 1 then
			'no operation		

		'if the system has consistently determined the tune direction to be 
		' the same for the required number of intervals, then we are clear to 
		' adjust the output
		elseif CurrentTuneInertia >= TuneInertia then
			
			'reset the counter
			CurrentTuneInertia = 0
		
			'modify VOut accordingly
			if CurrentTuneDirection = 1 then
				TuningUp = 1			
				Sum2b1b		@VOut(0), LockAdjGain
			else
				TuningDown = 1
				Diff2b1b	@VOut(0), LockAdjGain
			endif

			'update the display to indicate tuning direction
			UpdateDisplayController

			'update the DAC
			SetDac
		endif
	endif

	'reset the difference values back to negative if necessary
	if NegLHS = 1 then		
		NegLHS = 0
		FlipSign2b	@Diff(1)
	endif	

	if NegRHS = 1 then
		NegRHS = 0
		FlipSign2b	@LockDiff(1)
	endif	

else	'if the system is not locking
	
	'watch the encoder for user manipulation of the output voltage
	if EncoderIncrease = 1 then
		EncoderIncrease = 0
		Sum2b1b		@VOut(0), OutputAdjGain

		'keep it from going past the 12-bit limit
		if VOut(1) > $F then
			Set2b	@VOut(1), 	$F,	$FF
		endif
		SetDAC
	elseif EncoderDecrease = 1 then
		EncoderDecrease = 0
		Diff2b1b	@VOut(0), OutputAdjGain

		'keep from going negative
		if VOut(1) > $F0 then
			Set2b	@VOut(1),	$0,	$0
		endif
		SetDAC
	endif

	'check for button use
	'if debounce > 0 then		'dead time for right after button press
		'debounce = debounce - 1
	'if Lock Set button is pressed
	if LockSet = 1 then
		'reset the debounce timer
		'debounce = $FF

		'store the current status into the lock status
		Equate2B	@LockSum(1), 	@Sum(1)
		Equate2B	@LockDiff(1),	@Diff(1)

		'flag that locking is now enabled
		LockEnabled = 1

		UpdateDisplayController
	endif
endif

'this asm routine is an efficient multi-byte counter.  It counts down to 0, and flags 
' UpdateDisplay when it hits it
asm
	mov FSR, #pollcount		'load the address of the LSB of pollcount into
					' FSR
	mov __Param1, #PollDelayCoarse	'use the __Param1 as a counter to know how 
					' many bytes have been updated
	SUB IND, #1			'decrement the value of the LSB of pollcount
	JC @$+14			'jump ahead if the carry flag is set (no 
					' underrun)

	INC FSR				'if the LSB underran, then seek FSR ahead by 
					' one to work on next most significant byte
	DEC __Param1			'decrease the bytes counter to pay attention
					' to how many bytes have been modified this 
					' time around
	JZ  @$+5			'if we're done with all bytes, then jump to 
					' the end
	JMP @$-10			'otherwise go back and do the next-most 
					' signigicant byte

	SETB UpdateDisplay		'toggle that the display needs to be updated
	DEC FSR				'go back to the address of interest
	MOV IND, #PollDelayFine		'reset the value for the counter
	
	Bank $00
endasm

'Update Display/Memory:
'---------------------

'perform communication between display controller and memory controller to update 
'all properties
if UpdateDisplay = 1 then

	'reset the display update flag, and tuning flags
	UpdateDisplay = 0

	asm
		AND SystemStatus, #%111001
	endasm
	'TuningUp = 0
	'TuningDown = 0

	'this checks if the display controller has the correct number of properties 
	' loaded.  If it doesn't, then the main controller will reprogram it and load 
	' it with the values to display.  This occurs during initial startup, and in 
	' the case when the display controller is reset prematurely (eg. during 
	' programming)
	GetDisplayPropCount	'result is passed back in __Param1

	'if the display is showing the correct number, then read from display 
	' controller
	if __Param1 = 5 then
		'update all properties, and commit to memory if necessary
		GetDisplayProperty 	1, 	@OutputAdjGain
		GetDisplayProperty	2,	@LockLevelAdjGain
		GetDisplayProperty 	3,	@LockAdjGain
		GetDisplayProperty	4, 	@TuneInertia
		GetDisplayProperty	5,	@Samples

		'this sends the status byte to the display controller
		UpdateDisplayController
	else
		'perform first time initialization of the display controller
		InitDisplayController
	endif		
endif

'Check the RS232 port for receive handshaking request
'if RTS = 0 then
'  RS232Read
  'TODO: react to computer packet
'endif
'NOTE: this has been removed since it is not used, and for space considerations

'end of the main code
GOTO MainLoop

'-------------------------------------------------------------------------------------
'Subroutines
'-------------------------------------------------------------------------------------

'this subroutine determines the state of the optical encoder knob, and flags if there 
' has been an increase or decrease
sub GetEncoderChange
	
	'read the encoder states directly - done due to space considerations
	__Param2 = 0
	__Param2.0 = RB.5
	__Param2.1 = RB.6


	'if there's no change, diff will be 0
	__Param3 = __Param2 XOR OldState

	'if there is a change, then determine what the change is
	if __Param3 > 0 then

		'shift the old state left by one bit
		OldState = OldState << 1

		'perform XOR again to determine direction of quadrature change		
		__Param3 = OldState XOR __Param2
	
		'NOTE: you can change the polarity of the encoder change here if the 
		' knob rotation direction needs to be redefined
		if __Param3.1 = 1 then
			EncoderIncrease = 1
		else
			EncoderDecrease = 1
		endif
	endif

	'store the current state now, to be compared in the next iteration
	OldState = __Param2
endsub

'this subroutine performs an initialization on the display controller.  It sends the 
' display controlller the properties, via async serial communication on the serial bus
sub InitDisplayController

	'efficient assignment via assembly language
	asm

		MOV __Param1, OutputAdjGain	'store values in global parameters
		MOV __PARAM2, LockLevelAdjGain
		MOV __PARAM3, LockAdjGain
		MOV __PARAM4, TuneInertia
		MOV __PARAM5, Samples

		MOV FSR,#packet+0		'seek to beginning of packet array
		MOV IND,#8			'header size of 7 bytes
			
		INC FSR				'step ahead to the second byte in the packet
		CLR IND				'packet(1) is the command byte: $00 means
						' program all values
		
		INC FSR				'step ahead to the next byte in the packet
		MOV IND,#$5			'packet(2): # of parameters to program

		INC FSR				'step ahead to the next byte in the packet
		MOV IND,__PARAM1		'store value in packet array
		
		INC FSR				'step ahead to the next byte in the packet
		MOV IND,__PARAM2		'store value in packet array
		
		INC FSR				'step ahead to the next byte in the packet
		MOV IND,__PARAM3		'store value in packet array
		
		INC FSR				'step ahead to the next byte in the packet
		MOV IND,__PARAM4		'store value in packet array

		INC FSR				'step ahead to next byte
		MOV IND,__Param5		'store value in packet array

		BANK $00			'reset the memory bank to the first bank
	endasm

	'call async packet write on display controller
	ASyncPacketWrite	1
endsub

'this subroutine sends the status byte to the display controller, to be displayed on 
' the status LED's
sub UpdateDisplayController
	'update display controller with locking active status

	'packet(0) = 3
	'packet(1) = $02				'command $02: set status message
	'packet(2) = SystemStatus
	asm
		MOV FSR, #Packet+0
		MOV IND, #3
		INC FSR
		MOV IND, #2
		INC FSR
		MOV IND, SystemStatus
		Bank $00
	endasm

	ASyncPacketWrite	1
endsub

'asynchronous packet write code - used to transfer data to other controllers
sub ASyncPacketWrite
	asm
		MOV __PARAM3,#1                '__Param4 = 1 << __Param1
		MOV __PARAM2,__PARAM1
		JZ @$+8                       
		CLC                           
		RL __PARAM3
		DJNZ __PARAM2,@$-2            

                'mask for the CS lines.  Lower the desired chip to enable sync data transfer
		MOV W, #$F
		XOR W, __Param3
		MOV CS, W
	endasm

	'read in which chip to select
	'__Param3 = __Param1
	'__Param4 = 1 << __Param3
	'mask for the CS lines.  Lower the desired chip to enable sync data transfer
	'__Param3 = $F - __Param4

	'lower the appropriate CS line
	'CS = __Param3


	'wait for attention acknowledge handshake line to signify response
	do while AttnAck = 1
	loop

	pauseus 5
	
	'send all bytes
	for Scratch7 = 0 to packet(0)
		if Scratch7 < packet(0) then
			serout Mosi, N57600, packet(Scratch7)
			pauseus 5
		endif
	next
	'reset cs to indicate byte transfer done
	CS = $F

	'wait for attention acknowledge handshake to signify done
	do while AttnAck = 0
	loop
endsub

'this performs asynchronous packet read operation.  Note that the roles of chip select
' and attention acknowledge are reversed in handshaking.
sub ASyncPacketRead

	'read in which chip to select
	'__Param3 = __Param1
	'__Param4 = 1 << __Param3
	'mask for the CS lines.  Lower the desired chip to enable sync data transfer
	'__Param3 = %1111 - __Param4

	asm

		MOV __PARAM3,#1                '__Param4 = 1 << __Param1
		MOV __PARAM2,__PARAM1
		JZ @$+8                       
		CLC                           
		RL __PARAM3
		DJNZ __PARAM2,@$-2            

                'mask for the CS lines.  Lower the desired chip to enable sync data transfer
		MOV W, #$F
		XOR W, __Param3
		MOV __Param3, W
	endasm

	'wait for the Attention Acknowlege to go low, signifying that the slave 
	' controller is ready to transmit
	do while AttnAck = 1
	loop

	'respond by lowering the appropriate CS line
	CS = __Param3

	'read packet size header byte
	'serin Miso, N57600, Packet(0)
	AsmSerIn 0

	'if packet(0) > 10 then
	'	packet(0) = 2
	'endif

	'read the rest of the packet
	for Scratch7 = 1 to packet(0)
		if Scratch7 < packet(0) then
			'serin Miso, N57600, Packet(Scratch7)
			AsmSerIn Scratch7
		endif
	next

	'wait for Attention acknowledge to go high again
	do while AttnAck = 0
	loop
	CS = %1111

endsub

'this subroutine reads a byte value from the memory controller
sub ReadFromMemory

	'request a read on address 00A0
	

	'packet(0) = 4				'packet byte size header
	'packet(1) = $01				'command $01: read in value
	'packet(2) = __Param1			'address LSB
	'packet(3) = __Param2 & %111		'address MSB, with bitmask limiting 
						' value to 11-bit address
	asm
		AND __PARAM2,#%111	'address MSB, with bitmask limiting 

		MOV FSR,#packet+0
		MOV IND,#4
		INC FSR
		MOV IND,#1                  
		INC FSR
		MOV IND,__PARAM1              
		INC FSR
		MOV IND,__PARAM2
		BANK $00                      
	endasm

	'high Debug
	ASyncPacketWrite 0			'write request packet
	'low Debug
	ASyncPacketRead 0			'read response
	'this pause is necessary to allow the memory address controller enough
	' time to recognize that the read operation is complete.  This was the
	' cause of those bad starts!
	pauseus 5

	return packet(1)
endsub

'this subroutine writes a byte value to the memory controller
sub WriteToMemory

	'request a read on address 00A0
	'packet(0) = 5				'packet byte size header
	'packet(1) = $02				'command $02: write value
	'packet(2) = __Param1			'address LSB
	'packet(3) = __Param2 & %111		'address MSB, with bitmask limiting
						' value to 11-bit address
	'packet(4) = __Param3			'data value to be written
	asm
		AND __PARAM2,#%111	'address MSB, with bitmask limiting 

		MOV FSR,#packet+0
		MOV IND,#5
		INC FSR
		MOV IND,#$02
		INC FSR
		MOV IND,__PARAM1              
		INC FSR
		MOV IND,__PARAM2              
		INC FSR
		MOV IND, __PARAM3
		BANK $00                      
	endasm

	ASyncPacketWrite 0			'write packet
endsub

'this subroutine reads in the requested property value from the display controller
sub GetDisplayProperty

	Scratch0 = __Param1			'store property number in a more 
						' permanent place
	Scratch1 = __Param2			'store address of parameter in 
						' Scratch1

	'packet(0) = 3				'packet byte size header
	'packet(1) = $01				'command $01: read property
	'packet(2) = Scratch0			'property #
	asm
		MOV FSR, #packet+0
		MOV IND, #3
		INC FSR
		MOV IND, #1
		INC FSR
		MOV IND, Scratch0
		BANK $00
	endasm

	ASyncPacketWrite 1			'send read request to display 
						' controller

	ASyncPacketRead 1			'read in result from display 
						' controller

	__Param3 = packet(1)			'retrieve the result from the read

	'use pointer reference.. is there a way to do this outside asm?
	asm
		MOV FSR, Scratch1		'load address of parameter into FSR
		MOV __Param1, IND		'read value of parameter into __Param1
	endasm

	'if the value in memory does not match the value from the display controller, 
	' then it needs updating
	if __Param1 <> __Param3 then
		asm
			MOV FSR, Scratch1	'load address of parameter into FSR
			MOV IND, __Param3	'write value of parameter from display 
						' controller
		endasm	
	
		'update the memory controller with the updated value
		WriteToMemory Scratch0, $00, __Param3
	endif
endsub

'this subroutine reads in the number of display properties currently configured on 
' the display controller
sub GetDisplayPropCount
	asm
		mov FSR, #Packet+0
		MOV IND, #2
		INC FSR
		MOV IND, #3
		BANK $00
	endasm

	ASyncPacketWrite 1			'send request
	ASyncPacketRead 1			'read response

	return packet(1)
endsub

'this subroutine reads in from the RS232 port and stores the result into the packet 
' array
'sub RS232Read

	'flag CTS to let the computer know we're ready to receive
'	low CTS

'	serin RxD, T30000, packet(0)

	'the max function compares the given value and crops it to a max specified 
'	' value
'	packet(1)  = packet(1) max 10

'	for i = 1 to packet(0)
'		if i < packet(0) then
'			serin RxD, T30000, packet(i)
'		endif
'	next

	'wait for RTS to flag back up
'	do while RTS = 0
'	loop

	'simple echo behavior to prove data has been received and do error checking
	'TODO: remove this?
'	RS232Write

	'flag CTS to let computer know receive complete
'	CTS = %1

'endsub

'this subroutine writes the data stored in the packet array to the RS232 port
'sub RS232Write
'	serout TxD, T30000, packet(0)

'	for Scratch7 = 1 to packet(0)
'		if Scratch7 < packet(0) then
'			serout TxD, T30000, packet(Scratch7)
'		endif
'	next

'endsub

'this sets DAC channel #1 to the value specified in VOUT
sub SetDac
	'make sure there has not been a tune overload error
	if TuneOverLoad = 0 then
		'check if the most siginificant byte has gone too high (or negative)
		if VOut(1) > $F then		
			TuneOverLoad = 1
		else
			'add in command %11: load input and update DAC, for address 01
			packet(0) = VOut(1) + %0111_0000
			packet(1) = VOut(0)	'value LSB	
	
			'send the packet	
			CS = %0111		'DAC is on CS3

			for Scratch7 = 0 to 1
				shiftout Mosi, SPIClk, 1, packet(Scratch7), spdmlt
			next

			'need to reset the Mosi line low so that upcoming async serial
			' outputs are not messed up	
			Mosi = 0

			CS = %1111
		endif
	endif
endsub

'this subroutine performs ADC operation and read from TLC2543, then stores the result 
' in packets
sub ReadADC
	for Scratch7 = 0 to 3
		'select channel, mode
		'12-bit mode, MSB first, unipolar
		'index holds channel number selection
		__WParam12 = 0, Scratch7

		'this determines the index of the adcdata array to write to
		Scratch0 = Scratch7 * 2
		Scratch0 = Scratch0 + 1

		'ADC is at CS2: RA.2
		CS = %1011

		pauseus 1

		'send control byte, MSB first:  note that even though this is mode 1, 
		' MSB is sent first
		shiftout Mosi, SPIClk, 1, __WParam12\12, spdmlt
		'need to reset the Mosi line low so that subsequent async serial
		' outputs are not messed up
		Mosi = 0

		'wait for conversion
		pauseus 10
		'read data in

		Scratch0 = Scratch0 + 1
		'read in MSB 4 bits first
		'shiftin Miso, SPIClk, 0, packet(Scratch0)\4, spdmlt
		AsmShiftIn Scratch0, 4
		Scratch0 = Scratch0 - 1
		'shiftin Miso, SPIClk, 0, packet(Scratch0), spdmlt
		AsmShiftIn Scratch0, 8
	
		CS = %1111
		pauseus 5
	next
endsub

'assembly language serial input function, to conserve space
'TODO: is this timing correct for other frequencies?
sub AsmSerIn
'__Param1: states which section of the packet array to input to
'serin Miso, N57600, packet(__Param1)
	asm
		MOV FSR,#__TRISB               
		MODE $0F                      
		SETB IND.0                    
		MOV !RB,IND                   
		BANK $00                      
		JB Miso,@$                    
		JNB Miso,@$                   
		MOV __PARAM2,#8               
		MOV __PARAM4,#86              
		DJNZ __PARAM4,@$              
		JMP @$+2                      
		MOV __PARAM4,#172             
		DJNZ __PARAM4,@$              
		JMP @$+2                      
		RR __PARAM3                   
		MOVB __PARAM3.7,/Miso         
		DJNZ __PARAM2,@$-12           
		MOV W,#packet
		ADD W,__PARAM1
		MOV FSR,W
		MOV IND,__PARAM3
		BANK $00
	endasm
endsub

'assembly language synch serial in function, to conserve space
sub AsmShiftIn
	'shiftin Miso, SPIClk, 0, packet(Scratch0)\4, spdmlt
	'__Param1: index of packet to write to
	'__Param2: bits to read
	'NOTE: speed multiplier is hardcoded this way
	'NOTE: pin configuration is hardcoded this way
	asm
		MOV FSR,#__TRISB
		MODE $0F                      
		SETB IND.0                    
		CLRB IND.2                    
		MOV !RB,IND                   
		BANK $00                      
		CLR __PARAM5
		CLR __PARAM3
		'MOV __PARAM2,#4
		MOV __PARAM4,#1               
		DJNZ __PARAM4,@$              
		RL __PARAM5                  
		RL __PARAM3
		MOVB __PARAM5.0,Miso          
		XOR RB,#%00000100             
		MOV __PARAM4,#1               
		DJNZ __PARAM4,@$              
		XOR RB,#%00000100             
		DJNZ __PARAM2,@$-20           
		MOV W,#packet                 
		ADD W,__Param1
		MOV FSR,W                     
		MOV IND,__PARAM5
		BANK $00     
	endasm
endsub

'-------------------------------------------------------------------------------------
'two-byte arithematic
'-------------------------------------------------------------------------------------

'sums two 16-bit numbers.  can sum 32-bit number if first parameter is the low word
'and the high word is stored in TwoByteOver
sub Sum2b
	asm
		'load
		CLR Scratch0
		MOV __Param5, __Param3		'store destination in __Param5

		MOV FSR, __Param2		'seek to second number MSB
		MOV __Param4, IND		'store MSB
		DEC FSR				'seek to second number LSB
		MOV __Param3, IND		'store LSB

		MOV FSR, __Param1		'seek to first number MSB
		MOV __Param2, IND		'store MSB
		DEC FSR				'seek to first number LSB
		MOV __Param1, IND		'store LSB

		'add
		ADD __Param1, __Param3		'add the LSB's
		JNC @$+6			'jump to MSB addition if no carry
		INC __Param2			'increment MSB by one
		SNZ 				'if the MSB has not wrapped, go on to MSB
		SETB Scratch0.0			'flag that a carry to high word is required

		ADD __Param2, __Param4		'add the MSB's

		'check for carry to upper word
		MOV W, Status			
		OR W, Scratch0			'force carry if there was a carry from LSB
		MOV Status, W

		JNC @$+11			'if no carry, then we're done.  Jump to 
						' last part
		MOV FSR, #TwoByteOver+0
		INC IND
		JNZ @$+5			'if there's no carry on this one, we're 
						' done.  Jump to last part
		INC FSR
		INC IND

		'last part - write output
		MOV FSR, __Param5
		MOV IND, __Param2
		DEC FSR
		MOV IND, __Param1
		
		BANK $00			'reset the current memory bank back to
						' 0, where the rest of the program 
						' expects it to be by default
	endasm
endsub

'adds a byte value to a two-byte value and stores the result in the two-byte value
sub Sum2b1b
	asm
		MOV FSR,__Param1              	'store address of LSB in file select 
						' register
		ADD IND, __Param2		'add in the byte value
		INC FSR				'increment file select register to MSB
		ADDB IND, status.0		'add in carry bit 
		BANK $00			'reset memory bank to default bank 0
	endasm
endsub

'subtracts "two-byte" numbers referenced in __Param1, __Param2, and puts the result in 
' the address referenced by __Param3
sub Diff2b
	asm
		'MSB
		MOV FSR,__Param1              	'store address of first MSB in file 
						' select register
		MOV __Param4,IND              	'store value of first MSB in __Param4 
		MOV FSR,__Param2		'store address of second MSB in file 
						' select register
		SUB __Param4,IND		'perform subtraction into __Param4

		'LSB
		SUB __Param1, #1		'subtract address values by 1 to 
						' target the LSB's
		SUB __Param2, #1
		MOV FSR,__Param1              	'store address of first LSB in file 
						' select register
		MOV __Param5,IND              	'store value of first LSB in __Param5
		MOV FSR,__Param2		'store address of second LSB in file 
						' select register
		SUB __Param5,IND		'perform subtraction into __Param5
		SUBB __Param4, /C		'handle the carry value to the MSB

		MOV FSR,__Param3		'store address of destination in file 
						' select register                
		MOV IND,__Param4 		'transfer MSB value into destination 
						' referenced by __Param3

		SUB __Param3, #1		'increment address of destination by 
						' 1 to select MSB
		MOV FSR,__Param3		'store address of destination in file
						' select register                
		MOV IND,__Param5 		'transfer LSB value into destination
						' referenced by __Param3

		BANK $00			'reset the current memory bank back to
						' 0, where the rest of the program 
						' expects it to be by default
	endasm
endsub

'subtracts byte value from a two-byte value and stores the result in the two-byte 
' value
sub Diff2b1b
	asm
		MOV FSR,__Param1              	'store address of LSB in file select 
						' register
		Sub IND, __Param2		'subtract the byte value
		INC FSR				'increment file select register to MSB
		SUBB IND, /C			'subtract carry bit 
		BANK $00			'reset memory bank to default bank 0
	endasm
endsub

'this subroutine multiplies aa "two-byte" value at address in __Param1 by the 
' "two-byte" value at address in __Param2 and stores it in the address in __Param3.
sub Mult2b
	asm
		'store result address for now
		MOV __Param5, __Param3		'store address of result in __Param5

		'load values
		MOV FSR, __Param2		'store address of second number in FSR
		MOV __Param4, IND		'store value of second number MSB in 
						' __Param4
		DEC FSR				'decrement address in FSR by 1 to 
						' target LSB
		MOV __Param3, IND		'store value of second number LSB in 
						' __Param3

		MOV FSR, __Param1		'store address of first number in FSR
		MOV __Param2, IND		'store value of first number MSB in 
						' __Param2
		DEC FSR				'increment address in FSR by 1 to 
						' target MSB
		MOV __Param1, IND		'store value of first number LSB in 
						' __Param1

		BANK $0				'switch to correct memory bank for 
						' scratch values

		'prepare scratchpad and result storage
		CLR Scratch0			'clear scratchpad bytes 0 and 1 to 
						' make room for number crunching	
		CLR Scratch1
		CLR Scratch2			'clear scratchpad byte 2 to make room 
						' for result
		MOV Scratch3, #$80		'put a flag on high bit of MSB, used 
						' to flag end of operation

		'here's where it all begins!

		'JNC @$-F destination
		RR __PARAM2			'shift first number right by one bit,
	        RR __PARAM1                   	' and push the low bit into the carry
		JNC @$+9			'if it's a 0, then no addition 
						' required.. jump down by 9 
						' instructions (RR Scratch3)

						'if it's a 1, then 
		ADD Scratch0, __Param3		' add in the LSB of the second number

		MOV W, __Param4            	'store the MSB of the second number 
						' for now
		SNC                           	'if there was no carry from the last 
						' add, then skip next line

		MOVSZ W,++__Param4		'increment by one, and skip next line 
						' if the result is 0
		ADD Scratch1,W                	'if W <> 0, then add it into Scratch1

		RR Scratch1			'shift Scratch1 right, into Scratch0
		RR Scratch0			'shift Scratch0 right, into Scratch3
		RR Scratch3			'shift Scratch3 right, into Scratch2 
		RR Scratch2			'shift Scratch2 right, where carry 
						' flags end of operation
		JNC @$-15                 	'if carry != 0 (flag put in at 
						' beginning), then we're done

		'transfer from scratch to __Param values so we don't have to worry 
		' about bank
		MOV __Param1, Scratch2
		MOV __Param2, Scratch3
		MOV __Param3, Scratch1

		MOV FSR, #TwoByteOver
		MOV IND, Scratch0
		INC FSR
		MOV IND, __Param3

		MOV FSR, __Param5		'load address of result destination 
						' into FSR
		MOV IND, __Param2		'write MSB
		DEC FSR				'seek address back one
		MOV IND, __Param1		'write LSB


		Bank $00			'reset memory bank to 0
	endasm
endsub

'this subroutine performs division of a 32-bit dividend by a 16-bit divisor, with
' a 16-bit quotient.  The upper word of the dividend needs to be stored in the 
' TwoByteOver number
sub Divide2b
	asm
		'store result address for now
		MOV __Param5, __Param3		'store address of result in __Param5
		
		'load values
		MOV FSR, __Param2		'store address of divisor in FSR
		MOV __Param4, IND		'store value of divisor MSB in 
						' __Param4
		DEC FSR				'decrement address in FSR by 1 to 
						' target LSB
		MOV __Param3, IND		'store value of divisor LSB in 
						' __Param3
		
		MOV W,__PARAM3         		'check for divide by zero
		OR W,__PARAM4                 
		JZ @$+116                	'if there's a divide by zero, then 
						' jump ahead to the end (BANK $0 
						' statement)

		MOV FSR, __Param1		'store address of dividend in FSR
		MOV __Param2, IND		'store value of dividend MSB in 
						' __Param2
		DEC FSR				'decrement address in FSR by 1 to 
						' target LSB
		MOV __Param1, IND		'store value of dividend LSB in 
						' __Param1
		MOV FSR, #TwoByteOver+1		'seek to the TwoByteOver word
		MOV W, IND			'store MSB in W register
		BANK $00			'flip to correct bank
		MOV Scratch7, W			'store MSB in Scratch7
		MOV FSR, #TwoByteOver+0		'seek to the TwoByteOver word
		MOV W, IND			'store LSB in W register
		BANK $00			'flip to correct bank
		MOV Scratch6, W			'store LSB in Scratch6
		
		CLR Scratch0         	  	'clear quotient LSB
		CLR Scratch1           		'clear quotient MSB

		CLR Scratch2			'clear dividend storage low word, LSB
		CLR Scratch3			'clear dividend storage low word, MSB
		CLR Scratch4			'clear dividend storage high word, LSB
		CLR Scratch5			'clear dividend storage high word, MSB

		AND FSR,#$E0            	'the lower 5 bits of FSR is being used
						' to count bit shifts.  this clears
						' the lower 5 bits, and leaves the 
						' bank address intact
		
		'prepare for division			
		JMP @$+11                	'jump into the INC FSR line to begin 
						' counting and shifting
		
		CLC                     	'clear any carry contents so it 
						' doesn't mess up the shifting

		RR Scratch7			'shift upper word dividend MSB right
		RR Scratch6			'shift upper word dividend LSB right
		RR __Param2			'shift dividend MSB right
		RR __Param1			'shift dividend LSB right
		RR Scratch5			'shift dividend storage high word MSB right
		RR Scratch4			'shift dividend storage high word LSB right
		RR Scratch3			'shift dividend storage low word MSB right
		RR Scratch2			'shift dividend storage low word LSB right

		INC FSR                 	'increment the FSR bit shift counter     

		MOV W, Scratch6
		OR W, Scratch7
		JNZ @$-12

			'test if shifted dividend is less than or equal to divisor yet
		CJA __Param2, __Param4,@$-15	'check if dividend MSB is greater than
						' divisor MSB
		CJNE __Param2, __Param4, @$+10	'if not equal, strictly less, and no need 
						' to check LSB
		CJA __Param1, __Param3, @$-25	'check if dividend LSB is greater than 
						' divisor LSB

		
		'division operation
	
			'check if shifted dividend is greater than or equal to the divisor
		CJB __Param2, __Param4, @$+22	'if dividend MSB is smaller than divisor, 
						' then no
		CJNE __Param2, __Param4, @$+10	'if they don't equal, dividend MSB is 
						' greater and don't need to check LSB
		CJB __Param1, __Param3, @$+12   'if dividend LSB is smaller than divisor, 
						' then no
			'if yes, add 1 to quotient and subtract divisor from shifted 
			' dividend
		SETB Scratch0.0
		SUB __Param1, __Param3		'subtract divisor LSB from dividend LSB
		SUBB __Param2, /C		'perform carry operation as necessary
		SUB __Param2, __Param4		'subtract divisor MSB from dividend MSB


			'if no, jump to here
		DEC FSR				'decrement shift counter
		JZ @$+15			'if this reaches zero, we're done
		CLC

			'shift dividend left
		RL Scratch2			'shift dividend storage low word LSB left
		RL Scratch3			'shift dividend storage low word MSB left
		RL Scratch4			'shift dividend storage high word LSB left
		RL Scratch5			'shift dividend storage high word MSB left
		RL __Param1			'shift dividend LSB left
		RL __Param2			'shift dividend MSB left
			'shift quotient left
		CLC				'just in case junk is floating in carry, 
						' clear it
		RL Scratch0			'shift quotient LSB left
		RL Scratch1			'shift quotient MSB left
			'jump to top of division operation 
		JMP @$-36

		'BANK $00			'clean up in the end

		MOV FSR, #Scratch1		
		MOV __Param3, IND
		MOV FSR, __Param5		'load address of result destination
						' into FSR
		MOV IND, __Param3		'write MSB
		MOV FSR, #Scratch0
		MOV __Param3, IND
		MOV FSR, __Param5		'seek address back one
		DEC FSR
		MOV IND, __Param3		'write LSB

		Bank $00
	endasm
endsub

'flips the sign of a two byte number
sub FlipSign2b
	asm
		MOV __Param5, __Param1		'store address of result in __Param5

		'load values
		MOV FSR, __Param1		'store address of MSB in fsr
		MOV __Param2, IND		'store value of MSB in __Param2
		DEC FSR				'decrement address in FSR by 1 to
						' target LSB
		MOV __Param1, IND		'store value of LSB in __Param1

		'perfrom the inversion
		XOR __Param1, #$FF		'XOR all bits in LSB
		XOR __Param2, #$FF		'XOR all bits in MSB

		INC __Param1			'increment by one
		SNZ				'skip next line if the LSB has not 
						' wrapped around to 0
		INC __Param2			'perform carry as necessary

		'transfer result out
		MOV FSR, __Param5		'load file select register with
						' destination address
		MOV IND, __Param2		'move MSB out
		DEC FSR				'decrement address in FSR to target
						' LSB
		MOV IND, __Param1		'move LSB out

		Bank $00
	endasm

endsub

'sets two-byte number passed in param1 equal to two-byte number passed in param2
sub Equate2b

	asm
		MOV FSR, __Param2			'point to MSB of source
		MOV __PARAM3,IND              		'read value into buffer
		MOV FSR,__Param1			'point to MSB of destination
		MOV IND,__PARAM3			'write value from buffer

		DEC __Param1				'decrement destination 
							' address to LSB
		DEC __Param2				'decrement source address
							' to LSB

		MOV FSR, __Param2			'point to LSB of source
		MOV __PARAM3,IND              		'read value into buffer
		MOV FSR,__Param1			'point to LSB of destination
		MOV IND,__PARAM3			'write value from buffer

		BANK $00                      
	endasm
endsub

'sets two-byte number passed in param1 to literal passed in param2, param3
sub Set2b
	asm
		MOV FSR, __Param1			'point to MSB
		MOV IND, __Param2			'write MSB
		INC FSR					'seek to LSB
		MOV IND, __Param3			'write LSB
		BANK $00
	endasm
endsub

'clears the TwoByteOver data
sub ClearTBO
	asm
		MOV FSR, #TwoByteOver+0			'seek to TwoByteOver LSB
		CLR IND					'erase data
		INC FSR					'seek to TwoByteOver MSB
		CLR IND					'erase data
		BANK $00
	endasm
endsub