;******************************
;*
;* dxyToDirection
;* 
;* r0 = dx (-1023..1024)
;* r1 = dy (-1023..1024)
;*
;* return 
;*
;* r2 = direction	0 = "N", 1 = "NO", 2 = "O", 3 = "SO", 4 = "S", 5 = "SW", 6 = "W", 7 = "NW"
;*	

dxyToDirection

	;prolog

	push	r3
	push	r4
	push	r5
	push	r6
	push	r7
	
	;
		
	movei	r7,0
	movei	r2,0		;r2 = quadrant = 0
	
	sub	r3,r7,r0	;r3 = -dx
	sub	r4,r7,r1	;r4 = -dy
	
	movei	r6,2
	cmplosi	r0,0		;	     dx < 0 ?
	movets	r3,r3,r0	;udx (r3) =		 -dx : dx
	movets	r2,r2,r6	;q (r2)   =		   0 : 2
	
	
	cmplosi	r1,0		;	     dy < 0 ?
	movets	r4,r4,r1	;udy (r4) =		 -dy : dy
	addtqi	r2,r2,0		;q (r2)  +=                1 : 0
	
	muli	r3,64		;udx = udx * 64
	muli	r2,4		;q = q << 2 

	udivinit	15	;16 bit division
	udivsdd		r3	;udx *64
	udivsdv		r4	;	 / udy
	
	udivstep	;0
	udivstep	;1
	udivstep	;2
        udivstep 	;3
	udivstep	;4
	udivstep	;5
	udivstep	;6
        udivstep	;7
	udivstep	;8
	udivstep	;10
	udivstep	;11
        udivstep 	;12
	udivstep	;13
	udivstep	;14
	udivstep	;15
        udivstep	;16
	
	udivgq	r3	;r3 = qa (increase)
	movei	r5,2	;sq = 2 (subqadrant)
	
	movei	r7,0
	cmploi	r3,27		;qa < (min1 = 0.41421 * 64)
	brts	dxToDSubQuadrantOk
	movets	r5,r7,r5	;sq = 0 if true	delay slot
	nop			;		delay slot
	nop			;		delay slot
	movei	r7,1		;		delay slot
	
	cmploi	r3,155		;qa < (min2 = 2.41421 * 64)
	movets	r5,r7,r5	;sq = 1 if true	


dxToDSubQuadrantOk

	;r2 = quadrant * 4
	;r5 = subquadrant
	
	dexti	>quswToDirection
	movei	r3,quswToDirection	;get lut
	
	or	r2,r2,r5	;qsw = (q * 4) | sq
	nop
	rqld	r3,r2		;		  quswToDirection[qsw (r2)]
	nop
	nop
	ld	r2		;direction (r2) =
		
	;epilog
	
	rqpop
	rqpop
	rqpop
	pop	r7
	pop	r6
	pop	r5
	
	rqpop
	rqpop
	jmpi	r7,0
	pop	r4	;delay slot
	pop	r3	;delay slot
	nop		;delay slot
	nop		;delay slot	
	
quswToDirection

                word	0 ;0
                word	1 ;1
                word	2 ;2
                word	0 ;3 	illegal
                word	4 ;4
                word	3 ;5
                word	2 ;6
                word	0 ;7  	illegal
                word	0 ;8
                word	7 ;9
                word	6 ;10
                word	0 ;11  	illegal
                word	4 ;12
                word	5 ;13
                word	6 ;14
		
;******************************
;*
;* dxyToLength
;* 
;* r0 = dx (-1023..1024)
;* r1 = dy (-1023..1024)
;*
;* return 
;*
;* r2 = length (approx)	( ~ sqrt(dx^2+dy^2) ) 
;*	

dxyToLength	

	;prolog

	push	r3
	push	r4
	push	r5
	push	r6
	push	r7
	
	;
	
	movei	r7,0
	movei	r6,2
	
	sub	r3,r7,r0	;r3 = -dx
	sub	r4,r7,r1	;r4 = -dy
	
	cmplosi	r0,0		;	     dx < 0 ?
	movets	r3,r3,r0	;udx (r3) =		 -dx : dx	
	
	cmplosi	r1,0		;	     dy < 0 ?
	movets	r4,r4,r1	;udy (r4) =		 -dy : dy

	;
	
	movei	r7,3
	cmplo	r3,r4
	movets	r1,r3,r4	;lo (r1) = Min(udx,udy)
	movets	r0,r4,r3	;hi (r0) = Max(udx,udy)
	
	mul	r3,r1,r7	;lo * 3
	movei	r7,0
	sub	r3,r3,r0	;lo * 3 - hi
	nop
	cmplosi	r3,0
	movets	r3,r7,r3	;max(0,lo * 3 - hi)
	nop
	add	r3,r3,r1	;max(0,lo * 3 - hi) + lo
	nop
	lsri	r3,3		;(max(0,lo * 3 - hi) + lo) / 8	
	nop
	add	r2,r3,r0	;r2 = (max(0,lo * 3 - hi) + lo) / 8 + hi
	

	;epilog
	
	rqpop
	rqpop
	rqpop
	pop	r7
	pop	r6
	pop	r5
	
	rqpop
	rqpop
	jmpi	r7,0
	pop	r4	;delay slot
	pop	r3	;delay slot
	nop		;delay slot
	nop		;delay slot		    
	
;******************************
;*
;* waitVPos
;*
;* r3 pos
;* 

waitVPos	
	
	push	r1
	push	r2
	push	r7
	
	;debug output
	
	dexti	>debugOutput
	movei	r1,debugOutput
	gpci	r7,2
	jmpi	r1,0	
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	
	;epilog
	
	dexti	>UuLedR
	movei	r1,UuLedR		;is emulator
	nop
	rqldi	r1,0
	nop
	nop
	ld	r2
	nop
	extri	r2,0
	brts	dontWaitVPos		;emulator dont wait for vblank
	dexti	>UcVideoHITVCOUNT	;delay slot
	movei	r1,UcVideoHITVCOUNT	;delay slot
	nop				;delay slot
	nop				;delay slot
	
	st	r1,r3		;program vcount position to hit
	
	dexti	>UcVideoRPVcountHit
	movei	r1,UcVideoRPVcountHit
	nop
	
waitV	
	rqldi	r1,0	;get hit
	nop
	nop
	ld	r2	;get hit
        nop
        extri	r2,0	;bit 0 = 1 then hit programmed vcount
        brtc	waitV
        nop			;delay slot
	nop			;delay slot
	nop			;delay slot
	nop			;delay slot
	
	;prolog
	
dontWaitVPos
	rqpop
	rqpop
	rqpop
	pop	r7
	pop	r2	
	pop	r1     	
	
	jmpi	r7,0
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	
	
;******************************
;*
;* switchFrames
;*
;*

switchFrames

	;prolog

	push	r0
	push	r1
	push	r2
	push	r3
	push	r4
	push	r5
	
	;

	movei	r0,displayFrame
	movei	r1,drawFrame
	moveih	r0,>displayFrame	
	moveih	r1,>drawFrame
		
	rqldi	r0,0
	rqldi	r0,1
	nop
	ld	r2	;display frame high
	ld	r3	;display frame low
	
	rqldi	r1,0
	rqldi	r1,1
	nop
	ld	r4	;draw frame high
	ld	r5      ;draw frame low

	stinc	r0,r4
	stinc	r1,r2
	st	r0,r5
	st	r1,r3
	
	;r4:r5 display frame now
	;r2:r3 draw frame now
	
	;store to video processing
	
	movei	r0,UcVideoMEMStartAdrLow
	movei	r1,UcVideoMEMLatch
	moveih	r0,>UcVideoMEMStartAdrLow	;low memstart
	moveih	r1,>UcVideoMEMLatch		;latch out
	
	addi	r5,16		;x offset
	addtqi	r4,r4,0
	
	stinc	r0,r5		;display frame low
	nop
	st	r0,r4		;display frame high
	st	r1,r1		;latch out
			
	;epilog
	
	rqpop
	rqpop
	rqpop
	poparqp	r5
	poparqp	r4
	poparqp	r3
	pop	r2
	pop	r1
	pop	r0
	
	jmpi	r7,0
	nop		;delay slot
	nop	     	;delay slot
	nop		;delay slot
	nop		;delay slot		
	
;******************************
;*	
;* debugOutput
;*

debugOutput
	
	push	r0
	push	r1
	push	r2
	push	r3
	push	r7

	dexti	>UcRSwichtWLedG
	movei	r0,UcRSwichtWLedG
	nop
	rqldi	r0,0
	nop
	nop
	ld	r0	;sw 2
	nop
	extri	r0,1
	brtc	debugModeDisabled
	movei	r0,UcVideoRVCountWHSYNC		;		delay slot
	nop					;		delay slot
	moveih	r0,>UcVideoRVCountWHSYNC	;vblank address	delay slot
	nop					;		delay slot
	
	rqldi	r0,0	;vcount
	nop
	nop
	ld	r0
	nop
	subi	r0,30
	nop
	lsri	r0,2	;/4
	
	dexti	1
	movei	r1,$88	;width
	
	movei	r3,drawFrame
	mul	r0,r1,r0	;r0 = low
	moveih	r3,>drawFrame
	gmulhi	r1		;r1 = high 
	
	rqldi	r3,0	;high
	rqldi	r3,1	;low
	nop
	ld	r3	;high
	ld	r2	;low
	
	nop
	add	r2,r2,r0	;low
	addt	r3,r3,r1	;high
	movei	r0,$e0
	addi	r2,16		;first 16 pixels are invisible
	addtqi	r3,r3,0
	moveih	r0,$ff
	esadr	r3,r2
	est	r0,0
	est	r0,1
	est	r0,2
	est	r0,3
	est	r0,4
	est	r0,5
	est	r0,6
	est	r0,7
	
debugModeDisabled	
	
	rqpop
	rqpop
	rqpop
	poparqp	r7
	poparqp	r3
	pop	r2
	pop	r1
	pop	r0
	
	jmpi	r7,0
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot	