;
; constrain.s
;
; Mouse movement constraining
;
;  1994-1998 Straylight
;

;----- Licensing note -------------------------------------------------------
;
; Constrain is free software; you can redistribute it and/or modify
; it under the terms of the GNU General Public License as published by
; the Free Software Foundation; either version 2, or (at your option)
; any later version.
;
; Constrain is distributed in the hope that it will be useful,
; but WITHOUT ANY WARRANTY; without even the implied warranty of
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
; GNU General Public License for more details.
;
; You should have received a copy of the GNU General Public License
; along with Constrain.  If not, write to the Free Software Foundation,
; 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

;----- Standard header ------------------------------------------------------

		GET	libs:header
		GET	libs:swis

		IMPORT	version

;----- Module header --------------------------------------------------------

		AREA	|!!!Module$$Header|,CODE,READONLY

		DCD	0			;Start code
		DCD	initialise		;Initialisation routine
		DCD	finalise		;Finalisation code
		DCD	0			;Service call handler
		DCD	title			;Title pointer
		DCD	version			;The help string
		DCD	0			;No keyword table
		DCD	&4A340			;SWI chunk number
		DCD	swi_handler		;SWI handler code
		DCD	swi_decode		;SWI decode table
		DCD	0			;No decoding code needed

title		DCB	"Constrain",0

;----- Initialisation and finalisation --------------------------------------

initialise	ROUT

		STMFD	R13!,{R14}		;Stack link register nicely

		; --- Get some workspace ---

		MOV	R0,#6			;Allocate workspace
		MOV	R3,#ws__wSize		;Make it *this* big
		SWI	XOS_Module		;Get me memory
		LDMVSFD	R13!,{PC}		;Return if it barfed
		STR	R2,[R12]		;Stash the workspace pointer
		MOV	R12,R2			;Move the pointer across

		; --- Do some initialising ---

		MOV	R14,#0			;No current constraint
		STR	R14,ws__routine		;Store that away

		; --- Done ---

		LDMFD	R13!,{PC}^		;Return to caller happy

		LTORG

finalise	ROUT

		STMFD	R13!,{R11,R14}
		MOV	R11,R12			;Keep the private word ptr
		LDR	R12,[R12]		;Find my workspace

		BL	constrain_finish	;Close any constraint

		; --- Free my workspace ---

		MOV	R0,#7			;Free RMA space
		MOV	R2,R12			;Point to workspace
		SWI	XOS_Module		;Try to free the memory
		MOV	R0,#0			;Gonna zero the private word
		STR	R0,[R11]		;Then zero it
		LDMFD	R13!,{R11,PC}^		;A happy bunny

		LTORG

;----- SWI dispatching ------------------------------------------------------

swi_decode	DCB	"Constrain",0
		DCB	"Finish",0
		DCB	"MousePos",0
		DCB	"Circle",0
		DCB	"Disc",0
		DCB	0

swi_handler	LDR	R12,[R12]		;Get my workspace
		CMP	R11,#(%01-%00)/4	;Within range?
		ADDLO	PC,PC,R11,LSL#2		;Yes -- dispatch
		B	%01			;No -- complain

00		B	constrain_finish
		B	constrain_mousePos
		B	constrain_circ
		B	constrain_disc

01		ADR	R0,noSuchSWI		;Point to the error
		ORRS	PC,R14,#V_flag		;And return with error

noSuchSWI	DCD	&1E6
		DCB	"Unknown Constrain operation",0

;----- Main code ------------------------------------------------------------

; --- divide ---
;
; On entry:	R0 == dividend
; 		R1 == divisor
;
; On exit:	R0 == quotient
;		R1 == remainder
;
; Use:		A standard divide routine.  Fairly speedy, hopefully.
;		The results are always such that
;
;			|quotient| <= |(divisor/dividend)|,
;
;			|remainder| < |divisor|
;
;		and
;
;			quotient * divisor + remainder == dividend

divide		ROUT

		STMFD	R13!,{R2,R3,R14}	;Save some registers

		; --- A note about the method ---
		;
		; We use traditional long division, but unroll the loop a
		; lot to keep the thing ticking over at a good rate.

		; --- First, mess about with the signs ---

		ANDS	R14,R0,#&80000000	;Get the dividend's sign bit
		ORR	R14,R14,R14,LSR #1	;Copy -- this is sign of mod
		RSBNE	R0,R0,#0		;Take absolute value of R0
		ANDS	R3,R1,#&80000000	;Get the divisor's sign too
		RSBNE	R1,R1,#0		;Take absolute value of R1
		EOR	R14,R14,R3		;Calculate sign of quotient
		MOV	R3,R1			;Look after the divisor

		; --- Shift divisor up for long division ---
		;
		; We keep shifting the divisor up until it's greater than
		; the dividend, and then we skip ahead to the divide section

		MOV	R2,#0			;Quotient starts off at 0
00divide	CMP	R0,R3,LSL #0
		BLS	%10divide
		CMP	R0,R3,LSL #1
		BLS	%11divide
		CMP	R0,R3,LSL #2
		BLS	%12divide
		CMP	R0,R3,LSL #3
		BLS	%13divide
		CMP	R0,R3,LSL #4
		BLS	%14divide
		CMP	R0,R3,LSL #5
		BLS	%15divide
		CMP	R0,R3,LSL #6
		BLS	%16divide
		CMP	R0,R3,LSL #7
		MOVHI	R3,R3,LSL #8
		BHI	%00divide

		; --- Now we have the shift-down loop ---
		;
		; This is where the actual job of dividing is performed.
		; We shift the divisor back down until it's back where we
		; started again.

17divide	CMP	R0,R3,LSL #7
		ADC	R2,R2,R2
		SUBCS	R0,R0,R3,LSL #7
16divide	CMP	R0,R3,LSL #6
		ADC	R2,R2,R2
		SUBCS	R0,R0,R3,LSL #6
15divide	CMP	R0,R3,LSL #5
		ADC	R2,R2,R2
		SUBCS	R0,R0,R3,LSL #5
14divide	CMP	R0,R3,LSL #4
		ADC	R2,R2,R2
		SUBCS	R0,R0,R3,LSL #4
13divide	CMP	R0,R3,LSL #3
		ADC	R2,R2,R2
		SUBCS	R0,R0,R3,LSL #3
12divide	CMP	R0,R3,LSL #2
		ADC	R2,R2,R2
		SUBCS	R0,R0,R3,LSL #2
11divide	CMP	R0,R3,LSL #1
		ADC	R2,R2,R2
		SUBCS	R0,R0,R3,LSL #1
10divide	CMP	R0,R3,LSL #0
		ADC	R2,R2,R2
		SUBCS	R0,R0,R3,LSL #0

		CMP	R3,R1			;Have we finished dividing?
		MOVHI	R3,R3,LSR #8		;No -- shift down a byte
		BHI	%17divide		;And loop round again

		; --- Now tidy everything up ---

		TST	R14,#&40000000		;Is remainder to be negative?
		RSBNE	R1,R0,#0		;Yes -- negate it
		MOVEQ	R1,R0			;No -- just copy it nicely
		TST	R14,#&80000000		;Is quotient to be negative?
		RSBNE	R0,R2,#0		;Yes -- negate it
		MOVEQ	R0,R2			;No -- just copy it nicely

		LDMFD	R13!,{R2,R3,PC}^	;Return to caller

		LTORG

; --- divRound ---
;
; On entry:	R0 == dividend
;		R1 == divisor
;
; On exit:	R0 == quotient, rounded to nearest integer
;		R1 == remainder
;
; Use:		Calculates a rounded-to-nearest quotient, rather than one
;		rounded towards zero, which is what divide returns you.
;
;		The remainder is fiddled during this process, so that the
;		properties
;
;			quotient * divisor + remainder == dividend
;
;		and
;
;			|remainder| < |divisor|
;
;		still hold (so the remainder's sign may well change).

divRound	ROUT

		STMFD	R13!,{R2,R14}		;Save some registers
		MOV	R2,R0			;Keep a copy of the dividend
		CMP	R1,#0			;Is the divisor positive?
		MOVGE	R14,R1			;Yes -- just copy it
		RSBLT	R14,R1,#0		;No -- negate it on the way
		CMP	R0,#0			;Is the dividend positive?
		ADDGE	R0,R0,R14,ASR #1	;Yes -- add half the divisor
		SUBLT	R0,R0,R14,ASR #1	;No -- subtract it
		SUB	R2,R2,R0		;Remember this difference
		BL	divide			;Do the division
		ADD	R1,R1,R2		;Modify remainder suitably
		LDMFD	R13!,{R2,PC}^		;Return to caller

		LTORG

; --- sqrt ---
;
; On entry:	R0 == value to square-root
;
; On exit:	R0 == the result
;
; Use:		Evaluates the square root of the number given.  This routine
;		is constructed from the information supplied by David Seal,
;		and is *extremely* fast.

		EXPORT	sqrt
sqrt		ROUT

		STMFD	R13!,{R1-R4,R14}	;Stack registers
		MOV	R1,#0			;Result so far
		MOV	R2,#0			;Current remainder
		MOV	R3,#1			;A '01' pair
		MOV	R4,#3			;A nice mask

		GBLA	count
count		SETA	0			;Start the count at 30

		WHILE	count<=28		;Set up the loop condition

		AND	R14,R4,R0,LSR #30-count
		ORR	R2,R14,R2,LSL #2
		ORR	R14,R3,R1,LSL #2
		CMP	R2,R14
		ADC	R1,R1,R1
		SUBCS	R2,R2,R14

count		SETA	count+2

		WEND

		AND	R14,R4,R0
		ORR	R2,R14,R2,LSL #2
		ORR	R14,R3,R1,LSL #2
		CMP	R2,R14
		ADC	R1,R1,R1
		SUBCS	R2,R2,R14

c		MOV	R0,R1			;Put the result in R0
		LDMFD	R13!,{R1-R4,PC}^	;Return to caller

		LTORG

; --- constrain_circ ---
;
; On entry:	R0 == x position of circle centre
; 		R1 == y position of circle centre
;		R2 == radius of circle
;		R3 == maximum distance below centre allowed
;
; On exit:	--
;
; Use:		Constrains the mouse to the given circle outline

constrain_circ	ROUT

		STMFD	R13!,{R0-R2,R14}	;Stack some registers
		BL	constrain_finish	;Clear a current constraint
		MUL	R14,R2,R2		;Get the radius squared
		MOV	R2,R14			;Put in back in R2
		STMIA	R12,{R0-R3}		;Store parameter in RMA ws
		MOV	R0,#1			;Call this often
		ADR	R1,constrain__circ	;Routine to call
		STR	R1,ws__routine		;Remember this pointer
		MOV	R2,R12			;Pass this R12 value
		SWI	XOS_CallEvery		;Call every 50cs
		MOV	R0,#106			;Define mouse parameters
		MOV	R1,#&81			;Pointer 1 -- de-linked
		SWI	XOS_Byte		;Do the OS_Byte call
		LDMFD	R13!,{R0-R2,PC}^	;Return to caller

; --- constrain_disc ---
;
; On entry:	R0 == x position of circle centre
; 		R1 == y position of circle centre
;		R2 == radius of circle
;
; On exit:	--
;
; Use:		Constrains the mouse to the given circle outline

constrain_disc	ROUT

		STMFD	R13!,{R0-R2,R14}	;Stack some registers
		BL	constrain_finish	;Clear a current constraint
		MUL	R14,R2,R2		;Get the radius squared
		MOV	R2,R14			;Put in back in R2
		STMIA	R12,{R0-R2}		;Store parameter in RMA ws
		MOV	R0,#1			;Call this often
		ADR	R1,constrain__disc	;Routine to call
		STR	R1,ws__routine		;Remember this pointer
		MOV	R2,R12			;Pass this R12 value
		SWI	XOS_CallEvery		;Call every 50cs
		MOV	R0,#106			;Define mouse parameters
		MOV	R1,#&81			;Pointer 1 -- de-linked
		SWI	XOS_Byte		;Do the OS_Byte call
		LDMFD	R13!,{R0-R2,PC}^	;Return to caller

; --- constrain__circ ---
;
; On entry:	--
;
; On exit:	--
;
; Use:		Called on interrupts to constrin the mouse

constrain__circ	ROUT
		STMFD	R13!,{R0-R11,R14}	;Stack some registers

		SWI	XOS_Mouse		;Get the current mouse pos
10		LDMIA	R12,{R4-R6,R9}		;Load interesting values
		MOV	R10,#0			;Temporary flags word

		; --- Set R7 == horizontal distance from centre squared ---

		SUB	R14,R0,R4		;Get distance in R14
		MUL	R7,R14,R14		;Square it and copy
		CMP	R7,#&10000		;Is it too big?
		MOVGT	R7,#&10000		;Yes -- reduce (stop oflow)
		LDR	R8,ws__oldX		;Get the old x position
		SUB	R8,R8,R4		;Get the relative position
		EOR	R8,R8,R14		;See if top bit is changed
		TST	R8,#(1<<31)		;Test the top bit
		ORRNE	R10,R10,#(1<<0)		;Set the 'side changed' bit

		; --- Set R8 == vertical distance from centre squared ---

		SUB	R11,R5,R9		;Get actual maximum height
		MUL	R8,R9,R9		;Get the y distance squared
		CMP	R1,R11			;Is it high enough?
 		MOVLE	R1,R11			;No -- then raise it
		SUBLE	R7,R6,R8		;Calculate the corner x pos
   		ORRLE	R10,R10,#2		;Remember we made this bodge
		SUBS	R14,R1,R5		;Get distance in R14
		BGE	%00constrain__circ	;If in upper quadrants, skip
		CMP	R8,R6			;Is there a sensible distance
		BGT	%00constrain__circ	;No -- skip this magic bit
		TST	R10,#1			;Have we changed sides?
		BEQ	%00constrain__circ	;Nope -- skip ahead
		LDR	R0,ws__oldX		;Make sure were on correct sd
		SUB	R7,R6,R8		;Calculate the corner x pos
		ORR	R10,R10,#2		;Remember we made this bodge
		B	%01constrain__circ	;Skip next instruction

00		MUL	R8,R14,R14		;Square it and copy
		CMP	R8,#&10000		;Is it too big?
		MOVGT	R8,#&10000		;Yes -- reduce (stop oflow)

01		ADDS	R9,R7,R8		;Dist from centre squared
		BEQ	%90constrain__circ	;If 0, return

		; --- Don't allow a diagonal skip ---

		TST	R10,#1			;Has x changed sides?
		BEQ	%02constrain__circ	;No -- forget this check
		LDR	R14,ws__oldY		;Get the old Y value
		SUB	R3,R14,R5		;Get distance from centre
		MOV	R2,R1			;Get the new Y value
		SUB	R2,R2,R5		;Get the new distance
		EOR	R3,R3,R2		;Has sign bit changed?
		TST	R3,#(1<<31)		;Test it
		BEQ	%02constrain__circ	;No -- skip ahead
		MOV	R1,R14			;Use this Y value
		LDR	R0,ws__oldX		;And this X value
		SUB	R14,R0,R4		;Get the hz distance
		MUL	R7,R14,R14		;And put the square in R7
		SUB	R14,R1,R5		;Get the vt distance
		MUL	R8,R14,R14		;And put the square in R8
		ORR	R10,R10,#2		;Force a *mouse* update
		ADDS	R9,R7,R8		;Dist from centre squared
		BEQ	%90constrain__circ	;If 0, return

		; --- Calculate corrected relative positions ---

02		MOV	R2,R0			;Look after the original...
		MOV	R3,R1			;... mouse position

		MOV	R1,R9			;We'll need to divide by this
		MUL	R0,R7,R6		;Multiply up to give thing
		BL	divRound		;Do the main division
		BL	sqrt			;And take the square root
		CMP	R2,R4			;Which side of the centre?
		ADDGE	R2,R4,R0		;This is the x position
		SUBLT	R2,R4,R0		;This is the x position

		MOV	R1,R9			;We'll need to divide by this
		MUL	R0,R8,R6		;Multiply up to give thing
		BL	divRound		;Do the main division
		BL	sqrt			;And take the square root
		CMP	R3,R5			;Which side of the centre?
		ADDGE	R3,R5,R0		;This is the y position
		SUBLT	R3,R5,R0		;This is the y position

		; --- Make sure y isn't too low now ---

		CMP	R3,R11			;Is the y value too low?
		MOVLT	R0,R2			;Yes -- get the new x and...
		MOVLT	R1,R3			;... y coordinates
		BLT	%10constrain__circ	;And skip back round (yeuch)

		; --- Store the newly modified mouse positions ---

		STR	R2,ws__oldX		;Store the new pointer...
		STR	R3,ws__oldY		;...positions away

		; --- Now move the mouse pointer position ---

		MOV	R0,#5			;OS_Word subreason code
		ORR	R0,R0,R2,LSL #8		;Move in the x position
		ORR	R0,R0,R3,LSL #24	;And the bottom of the y
		MOV	R1,R3,LSR #8		;Get the top of the y
		STMFD	R13!,{R0,R1}		;Save the block on the stack
		MOV	R0,#21			;OS_Word main reason code
		MOV	R1,R13			;Point to the new block thing
		SWI	XOS_Word		;Position the mouse pointer

		; --- Constrain the *mouse* to the circular band ---

		SUBS	R14,R9,R6		;f(distance from circumf.)
		RSBLT	R14,R14,#0		;Make sure it's positive
		CMP	R14,#256		;Is this within the band?
		CMPLE	R10,#1			;Make sure we didn't bodge
		MOVGT	R14,#3			;No -- move mouse position
		STRGTB	R14,[R13,#0]		;Store new subreason code
		SWIGT	XOS_Word		;And move the position

		ADD	R13,R13,#8		;Reclaim the OS_Word block
90		LDMFD	R13!,{R0-R11,PC}^	;Return to caller

		LTORG

; --- constrain__disc ---
;
; On entry:	--
;
; On exit:	--
;
; Use:		Called on interrupts to constrin the mouse

constrain__disc	ROUT
		STMFD	R13!,{R0-R11,R14}	;Stack some registers
		SWI	XOS_Mouse		;Get the current mouse pos
10		LDMIA	R12,{R4-R6}		;Load interesting values
		MOV	R10,#0			;Temporary flags word

		; --- Set R7 == horizontal distance from centre squared ---

		SUB	R14,R0,R4		;Get distance in R14
		MUL	R7,R14,R14		;Square it and copy
		CMP	R7,#&10000		;Is it too big?
		MOVGT	R7,#&10000		;Yes -- reduce (stop oflow)

		; --- Set R8 == vertical distance from centre squared ---

		SUBS	R14,R1,R5		;Get distance in R14
00		MUL	R8,R14,R14		;Square it and copy
		CMP	R8,#&10000		;Is it too big?
		MOVGT	R8,#&10000		;Yes -- reduce (stop oflow)

01		ADDS	R9,R7,R8		;Dist from centre squared
		BEQ	%90constrain__disc	;If 0, return

		; --- Calculate corrected relative positions ---


02		MOV	R2,R0			;Look after the original...
		MOV	R3,R1			;... mouse position

		CMP	R6,R9			;Are we in the circle?
		BGE	%03constrain__disc	;Yes -- just move it then

		MOV	R1,R9			;We'll need to divide by this
		MUL	R0,R7,R6		;Multiply up to give thing
		BL	divRound		;Do the main division
		BL	sqrt			;And take the square root
		CMP	R2,R4			;Which side of the centre?
		ADDGE	R2,R4,R0		;This is the x position
		SUBLT	R2,R4,R0		;This is the x position

		MOV	R1,R9			;We'll need to divide by this
		MUL	R0,R8,R6		;Multiply up to give thing
		BL	divRound		;Do the main division
		BL	sqrt			;And take the square root
		CMP	R3,R5			;Which side of the centre?
		ADDGE	R3,R5,R0		;This is the y position
		SUBLT	R3,R5,R0		;This is the y position

		; --- Store the newly modified mouse positions ---

03		STR	R2,ws__oldX		;Store the new pointer...
		STR	R3,ws__oldY		;...positions away

		; --- Now move the mouse pointer position ---

		MOV	R0,#5			;OS_Word subreason code
		ORR	R0,R0,R2,LSL #8		;Move in the x position
		ORR	R0,R0,R3,LSL #24	;And the bottom of the y
		MOV	R1,R3,LSR #8		;Get the top of the y
		STMFD	R13!,{R0,R1}		;Save the block on the stack
		MOV	R0,#21			;OS_Word main reason code
		MOV	R1,R13			;Point to the new block thing
		SWI	XOS_Word		;Position the mouse pointer

		; --- Constrain the *mouse* to the circular band ---

		CMP	R6,R9
		MOVLT	R14,#3			;No -- move mouse position
		STRLTB	R14,[R13,#0]		;Store new subreason code
		SWILT	XOS_Word		;And move the position

		ADD	R13,R13,#8		;Reclaim the OS_Word block
90		LDMFD	R13!,{R0-R11,PC}^	;Return to caller

		LTORG

; --- constrain_finish ---
;
; On entry:	--
;
; On exit:	--
;
; Use:		Finishes a constraining operation

constrain_finish ROUT

		STMFD	R13!,{R0-R2,R14}	;Stack some registers

		; --- Check whether there's a constrain on ---

		LDR	R0,ws__routine		;Check the routine
		CMP	R0,#0			;Is there one going?
		LDMEQFD	R13!,{R0-R2,PC}^	;No -- do nothing then

		; --- Remove the routine ---

		MOV	R1,R12			;Pass this R12 value
		SWI	XOS_RemoveTickerEvent	;Call every 50cs
		MOV	R0,#106			;Change mouse parameters
		MOV	R1,#&1			;Pointer 1, linked
		SWI	XOS_Byte		;Do the thing

		; --- Clear the routine ---

		MOV	R14,#0			;Clear the pointer
		STR	R14,ws__routine		;Store that away
		LDMFD	R13!,{R0-R2,PC}^	;Return to caller

; --- constrain_mousePos ---
;
; On entry:	--
;
; On exit:	R0 == x coordinate of pointer
;		R1 == y coordinate of pointer
;
; Use:		Returns the current mouse position during a constrain
;		operation.

constrain_mousePos ROUT

		ADR	R0,ws__oldX		;Point to the coordinates
		LDMIA	R0,{R0,R1}		;And read them out
		MOVS	PC,R14			;Return to caller

		LTORG

;----- Workspace layout -----------------------------------------------------

		^	0,R12

ws__wStart	#	0			;The start of the workspace
ws__parameters	#	16			;Parameters for the constrain
ws__routine	#	4			;The routine to call
ws__oldX	#	4			;The old X coordinate
ws__oldY	#	4			;The old Y coordinate

ws__wSize	EQU	{VAR}-ws__wStart	;The workspace size

;----- That's all, folks ----------------------------------------------------

		END
