storage 00000H
	;
	; The following variables hold the number of the four sensors that have
	; the stronggest signals.
	;
	var   	topFirst 	       ; Stronggest signal
	var	topSecond	       ; Second stronggest signal
	var	topThird	       ; Third stronggest signal
	var	topFourth	       ; Fourth stronggest signal
	;
	;
	var	topFirstIndex	       ; Sensor with the stronggest signal
	var	topSecondIndex	       ; Sensor with the second stronggest signal
	var	topThirdIndex	       ; Sensor with the third stronggest signal
	var	topFourthIndex	       ; Sensor with the fourth stronggest signal
	;
	;
	var	s0		       ;
	var	x0		       ;
	var	y0		       ;
	var	s1		       ;
	var	x1		       ;
	var	y1		       ;
	;
	;
	var	theX		       ;
	var	theY		       ;
	var	theS		       ;
	;
	;
storage	0C000H				;
	var	angle1			; I/O port: controls propulsor angle #1
	var	angle2			; I/O port: controls propulsro angle #2
storage	0C000H
	dim	sensorArray, 25		; I/O vector: 5 x 5 array of thermal sensors
;
;
;
	org	00000h
	watch	theX
	watch	theY
	watch	theS
	;
	;
	;
main:	frame				; Prepare for calling sub-routine
	fcall	getDirection		; Find out which sensor has the stronggest signal
	chop				; (The sub-routine does not return any value)
	frame				; Prepare for calling the next sub-routine
	fcall	setDirection		; Set the propulsion angles
	chop				; (Again, sub doesn't return anything useful)
	const	0			; Force a "false" condition (for next istruction)
	branch	main			; IF FALSE (always; see above), goto main
;
;
;
getDirection:
storage
	var	i			; Loop counter
	const	0			; (Starts with 0)
	var	current			; Current sensor's signal value
	const	0			; (Initialize with zero)
;;;
	const	0			; Clears the four stronggest signals
	store	topFirst		;
	const	0			;
	store	topSecond		;
	const	0			;
	store	topThird		;
	const	0			;
	store	topFourth		;
	const	0			; Clear the numbers of their respective sensors
	store	topFirstIndex		;
	const	0			;
	store	topSecondIndex		;
	const	0			;
	store	topThirdIndex		;
	const	0			;
	store	topFourthIndex		;
@tryNext:				; Check next sensor
	address	sensorArray external	; Computes the address of the nth sensor...
	fetch	i local			; ... as being the first sensor's address,...
	binop	+			; ... added to the loop counter
	fetchtop			; Reads that sensor
	store	current local, =	; Store its signal value (will be used later)
	frame		       		; Prepare for checking if among the stronggest
	fetch	i local			; sortTopFour(i, current)
	fetch	current local		;
	fcall	sortTopFour		;
	chop				; (sortTopFour doesn't return any value)
	const	1			; Adds 1 to the loop counter...
	store	i local, +		; ... so that now it points the next sensor
	fetch	i local			; Reads the loop counter
	const	25			; Compares it against 25 (last sensor is 24)
	binop	==			; Checks to see if it is equal to 25
	branch	@tryNext		; If different, go check next sensor
	fetch	topFirst		; Computes the average position between 1 & 2
	store	s0			;
	fetch	topFirstIndex		;
	const	5			;
	binop	/			;
	const	100			;
	binop	*			;
	store	y0			;
	fetch	topFirstIndex		;
	const	5			;
	binop	%			;
	const	100			;
	binop	*			;
	store	x0			;
	fetch	topSecond		;
	store	s1			;
	fetch	topSecondIndex		;
	const	5			;
	binop	/			;
	const	100			;
	binop	*			;
	store	y1			;
	fetch	topSecondIndex		;
	const	5			;
	binop	%			;
	const	100			;
	binop	*			;
	store	x1			;
	frame				;
	fcall	averagePOS		;
	chop				;
	fetch	topThird		; Computes the average between avg(1,2) & 3
	store	s1			;
	fetch	topThirdIndex		;
	const	5			;
	binop	/			;
	const	100			;
	binop	*			;
	store	y1			;
	fetch	topThirdIndex		;
	const	5			;
	binop	%			;
	const	100			;
	binop	*			;
	store	x1			;
	frame				;
	fcall	averagePOS		;
	chop				;
	fetch	topFourth		; Computes the average between agv(avg(1,2),3) & 4
	store	s1			;
	fetch	topFourthIndex		;
	const	5			;
	binop	/			;
	const	100			;
	binop	*			;
	store	y1			;
	fetch	topFourthIndex		;
	const	5			;
	binop	%			;
	const	100			;
	binop	*			;
	store	x1			;
	frame				;
	fcall	averagePOS		;
	chop				; (x0,y0) has the average position
	fetch	x0
	store	theX
	fetch	y0
	store	theY
	fetch	s0
	store	theS
	retsub				; End of sub-routine
;
;
;
averagePOS:
	const	255
	fetch	s0
	binop	-
	store	s0
	const	255
	fetch	s1
	binop	-
	store	s1 			; s0 & s1 are now distances inst. of proximities
	;
	;
	fetch	y1
	fetch	y0
	binop	-
	fetch	s0
	binop	*
	fetch	s0
	fetch	s1
	binop	+
	binop	/
	fetch	y0
	binop	+
	store	y0			; yo <-- y0 + (s0*(y1-y0))/(s0+s1)
	;
	;
	fetch	x1
	fetch	x0
	binop	-
	fetch	s0
	binop	*
	fetch	s0
	fetch	s1
	binop	+
	binop	/
	fetch	x0
	binop	+
	store	x0			; x0 <-- x0 + (s0*(x1-x0))/(s0+s1)
	;
	;
	fetch	s1			; Check which one is smaller
	fetch	s0			; (smaller distance means strongger signal)
	binop	<
	branch	@s1IsSmaller
	fetch	s1	    		; If it is the second one,...
	store	s0			; ... then replace first one with it
@s1IsSmaller:
	const	255			; Convert back to proximity (inst. of distance)
	fetch	s0
	binop	-
	store	s0
	;
	;
	retsub
;
;
;
sortTopFour:
storage
       	var  	index
       	var	value
;;
  	fetch	value			; Checks if the passed value is bigger than...
  	fetch	topFirst		; ... the value that is currently the biggest one
  	binop	>			;
  	branch	@trySecond		;
  	fetch	topThird		; If it is, moves the top 3 values one pos down
  	store	topFourth		;
  	fetch	topSecond		;
  	store	topThird		;
  	fetch	topFirst		;
  	store	topSecond		;
  	fetch	value			; And stores the new value as first stronggest one
  	store	topFirst		;
  	fetch	topThirdIndex		; Does the same to the respective top 4 sensors
  	store	topFourthIndex		; id's list
  	fetch	topSecondIndex		;
  	store	topThirdIndex		;
  	fetch	topFirstIndex		;
  	store	topSecondIndex		;
  	fetch	index			;
  	store	topFirstIndex		;
  	retsub				; Returns
@trySecond:				; Checks if the new value is the new second...
	fetch 	value			; ... stronggest value.
	fetch	topSecond		;
	binop	>			;
	branch	@tryThird		;
  	fetch	topThird		;
  	store	topFourth		;
  	fetch	topSecond		;
  	store	topThird		;
  	fetch	value			;
  	store	topSecond		;
  	fetch	topThirdIndex		;
  	store	topFourthIndex		;
  	fetch	topSecondIndex		;
  	store	topThirdIndex		;
  	fetch	index			;
  	store	topSecondIndex		;
  	retsub				;
@tryThird:				;
	fetch 	value			;
	fetch	topThird		;
	binop	>			;
	branch	@tryFourth		;
  	fetch	topThird		;
  	store	topFourth		;
  	fetch	value			;
  	store	topThird		;
  	fetch	topThirdIndex		;
  	store	topFourthIndex		;
  	fetch	index			;
  	store	topThirdIndex		;
  	retsub				;
@tryFourth:				;
	fetch 	value			;
	fetch	topFourth		;
	binop	>			;
	branch	@done			;
	fetch	value			;
	store	topFourth		;
	fetch	index			;
	store	topFourthIndex		;
@done:	retsub				;
;
;
;
setDirection:
storage
	var	addr			; Holds the address of the element in the
	const	0			; propulser control angles table
;;;
	fetch	topFirstIndex external	; Reads the number of the "selected" sensor
	const	2			; Multiply it by 2, because each sensor translates
	binop	*			; to a PAIR of control angles
	address	angles			; Adds that to the address of the first entry
	binop	+			; in the control angles table
	store	addr local, =		; Store it into the "addr" variable for later use
	fetch	addr local		; Get the address of the desired entry in table
	code				; CODE, because the table is in program memory
	fetchtop			; Read the first angle
	store	angle1 external, =	; Send that angle to the propulser control
	const	1			; Advance the address pointer, so that it now
	store	addr local, +		; points to the second control angle
	fetch	addr local		; Read the pointer's value
	code				; CODE, because the table is in program memory
	fetchtop			; Read the second angle
	store	angle2 external, =	; Send that angle to the propulser control
	retsub				; End of sub-routine
;
; This table holds pairs of angles, to be sent to the propulser control angles
; output ports.
; Each two values (pair of values) represents the angles that have to be sent
; to the propulser controls, so that the missile heads towards the source of heat.
; The table is organized as 5 rows, with 5 columns each (just like the thermal
; sensor array that is displayed in the virtual jet-propulsion laboratory screen).
; Each column has two values, for both propulser angle control ports.
;
angles:	db	96			; Line 1; Column 1; Angle 1
	db	0			;                   Angle 2
	db	80			;         Column 2; Angle 1
	db	10			;                   Angle 2
	db	64			;         Column 3; Angle 1
	db	16			;                   Angle 2
	db	48			;         Column 4; Angle 1
	db	10			;                   Angle 2
	db	32			;         Column 5; Angle 1
	db	0			;                   Angle 2
	db	112			; Line 2; Column 1; Angle 1
	db	8			;                   Angle 2
	db	96			;         Column 2; Angle 1
	db	48			;                   Angle 2
	db	64			;         Column 3; Angle 1
	db	80			;                   Angle 2
	db	32			;         Column 4; Angle 1
	db	48			;                   Angle 2
	db	16			;         Column 5; Angle 1
	db	0			;                   Angle 2
	db	128			; Line 3; Column 1; Angle 1
	db	32			;                   Angle 2
	db	128			;         Column 2; Angle 1
	db	80			;                   Angle 2
	db	128			;         Column 3; Angle 1
	db	128			;                   Angle 2
	db	128			;         Column 4; Angle 1
	db	192			;                   Angle 2
	db	128			;         Column 5; Angle 1
	db	240			;                   Angle 2
	db	144			; Line 4; Column 1; Angle 1
	db	16			;                   Angle 2
	db	160			;         Column 2; Angle 1
	db	32			;                   Angle 2
	db	192			;         Column 3; Angle 1
	db	80			;                   Angle 2
	db	224			;         Column 4; Angle 1
	db	48			;                   Angle 2
	db	240			;         Column 5; Angle 1
	db	0			;                   Angle 2
	db	160			; Line 5; Column 1; Angle 1
	db	0			;                   Angle 2
	db	176			;         Column 2; Angle 1
	db	10			;                   Angle 2
	db	192			;         Column 3; Angle 1
	db	16			;                   Angle 2
	db	208			;         Column 4; Angle 1
	db	10			;                   Angle 2
	db	224			;         Column 5; Angle 1
	db	0			;                   Angle 2

	end