*
*	SinCos.asm (of PCQ Pascal)
*
*	These routines replace earlier attempts at the sine and
*	cosine functions.  They are based on the series
*	representations of the functions, so they're much more
*	accurate for most of the curve.
*
*	Martin Combs sent me a Pascal version of the implementation
*	used here.  I had been looking at various series for exp()
*	and ln(), but I didn't like the algorithms I was coming up
*	with for sine and cosine.  Thus I appreciate Martin's
*	contribution.
*
*	Martin's original Pascal functions duplicated by the assembly
*	below are:
*
* FUNCTION sine(x:REAL):REAL;
* 
*   FUNCTION GetVal : REAL;
*     CONST rfac2 = 0.5;        {rfac stands for reciprocal of factorial}
*           rfac3 = 0.1666667;
*           rfac4 = 0.0416667;
*           rfac5 = 0.0083333;
*           sincosfourthpi = 0.7071068; {sine or cosine of pi/4}
*     VAR   x2 : REAL;
* {This variation gives four significant figure accuracy}
*   BEGIN
*     IF x>fourthpi THEN BEGIN   { if x in second octant }
*       x:=x-fourthpi;           { reduce x to first octant }
*       x2:=sqr(x);
*       x2:=x2*(rfac4*x2-rfac2)+1.0; {calculates cosine in first octant}
*    { above is cos(x)=1-(x^2)/2!+(x^4)/4! }
*       GETVAL:=sincosfourthpi*(sine(x)+x2)  { note the recursion }
* { Note: sin(x)=sin((x-pi/4)+pi/4)=sin(x-pi/4)cos(pi/4)+cos(x-pi/4)sin(pi/4)}
* { =.7071068(sin(x-pi/4)+cos(x-pi/4)) }
*     END ELSE
*     x2:=sqr(x);
*     GetVal:=x*(x2*(rfac5*x2-rfac3)+1.0)   {calculates sine in first octant}
*  {above is sin(x)=x-(x^3)/3!+(x^5)/5! }
*   END;
* 
* {This variation gives two significant figure accuracy, but faster}
* {  BEGIN
*     IF x>fourthpi THEN BEGIN
*       x:=x-fourthpi;
*       x2:=sqr(x);
*       GetVal:=sincosfourthpi*(sine(x)+1.0-rfac2*x2)  }
* {cosine portion of above is 1-(x^2)/2! }
* {    END ELSE
*     x2:=sqr(x);
*     GetVal:=x*(1.0-rfac3*x2)
*   END;   }
* 
* 
* BEGIN
*   IF (x>twopi) OR (x<0.0) THEN  { faster than always doing it }
*     x:= x-Floor(x/twopi)*twopi;
*   IF x<halfpi THEN          { first quadrant }
*     sine:=GetVal
*   ELSE IF x<pi THEN BEGIN   { second quadrant }
*     x:=pi-x;
*     sine:=GetVal
*   END ELSE IF x<threehalvespi THEN BEGIN   { third quadrant }
*     x:=x-pi;
*     sine:=-GetVal
*   END ELSE BEGIN            { fourth quadrant }
*     x:=twopi-x;
*     sine:=-GetVal
*   END
* END;
* 
* FUNCTION cosine(x:REAL):REAL;
* BEGIN
*   cosine:=sine(halfpi-x)
* END;
*

	XREF	_p%MathBase

	XREF	_LVOSPAdd
	XREF	_LVOSPSub
	XREF	_LVOSPMul
	XREF	_LVOSPFloor
	XREF	_LVOSPCmp
	XREF	_LVOSPNeg
	XREF	_LVOSPDiv

*
*	GetVal returns the sine of theta, held in d0, which is
*	an angle between 0 and Pi/2
*

GetVal
	move.l	d0,-(sp)	; save theta
	move.l	#$C90FD940,d1	; d1 := pi/4
	jsr	_LVOSPCmp(a6)	; compare 'em
	ble	FirstOctant	; if theta <= pi/4, skip this

	move.l	(sp),d0		; d0 := theta
	move.l	#$C90FD940,d1	; d1 := pi/4
	jsr	_LVOSPSub(a6)	; d0 := theta - pi/4
	move.l	d0,(sp)		; save new theta
	move.l	d0,d1		; set up for square
	jsr	_LVOSPMul(a6)	; d0 := theta squared
	move.l	d0,-(sp)	; save theta2

	move.l	#$AAAAB33C,d1	; d1 := 1/4!
	jsr	_LVOSPMul(a6)	; d0 := theta^2 * 1/4!
	move.l	#$80000040,d1	; d1 := 1/2!
	jsr	_LVOSPSub(a6)	; d0 := theta^2 * 1/4! - 1/2!
	move.l	(sp)+,d1	; d1 := theta^2
	jsr	_LVOSPMul(a6)	; d0 := t^2 * (t^2/4! - 1/2!) = t^4/4! - t^2/2!
	move.l	#$80000041,d1	; d1 := 1.0
	jsr	_LVOSPAdd(a6)	; d0 := everything + 1.0

	move.l	d0,-(sp)	; save this result
	move.l	4(sp),-(sp)	; put theta back on stack
	bsr	_p%sin		; call sine recursively
	addq.l	#4,sp		; and pop it back off
	move.l	(sp)+,d1	; get theta^2
	jsr	_LVOSPAdd(a6)	; add them
	move.l	#$B504F440,d1	; d1 := sin(pi/4)
	jsr	_LVOSPMul(a6)	; multiply by sin(pi/4)
	addq.l	#4,sp		; pop previous theta
	rts			; and return

FirstOctant
	move.l	(sp),d0		; d0 := theta
	move.l	d0,d1		; so does d1
	jsr	_LVOSPMul(a6)	; d0 := theta^2
	move.l	d0,-(sp)	; save this

	move.l	#$8888653A,d1	; d1 := 1/5! = 1/96
	jsr	_LVOSPMul(a6)	; d0 := theta^2 * 1/5!
	move.l	#$AAAAAD3E,d1	; d1 := 1/3!
	jsr	_LVOSPSub(a6)	; d0 := theta^2/5! - 1/3!
	move.l	(sp)+,d1	; pop theta^2
	jsr	_LVOSPMul(a6)	; d0 := theta^4/5! - theta^2/3!
	move.l	#$80000041,d1	; d1 := 1.0
	jsr	_LVOSPAdd(a6)	; d0 := above + 1.0
	move.l	(sp)+,d1	; d1 := theta
	jmp	_LVOSPMul(a6)	; d0 := theta - theta^3/3! + theta^5/5!
				; return with that value


* d0 contains the angle, which is also on the stack

	XDEF	_p%sin
_p%sin
	move.l	_p%MathBase,a6	; we'll use this all over
	move.l	#$C90FD943,d1	; d1 := 2Pi
	jsr	_LVOSPCmp(a6)	; compare theta and 2Pi
	bgt.s	ShrinkIt	; normalize to 0 <= theta <= 2Pi
	move.l	4(sp),d0	; d0 := theta
	moveq.l	#0,d1		; d1 := 0.0
	jsr	_LVOSPCmp(a6)	; compare theta & 0.0
	bge.s	AfterShrink	; don't bother with normalization
ShrinkIt
	move.l	4(sp),d0	; d0 := theta
	move.l	#$C90FD943,d1	; d1 := 2Pi
	jsr	_LVOSPDiv(a6)	; d0 := theta / 2Pi
	jsr	_LVOSPFloor(a6)	; d0 := |_theta / 2Pi_| ( floor )
	move.l	#$C90FD943,d1	; d1 := 2Pi
	jsr	_LVOSPMul(a6)	; d0 := floor * 2Pi
	move.l	d0,d1		; shift for upcoming subtract
	move.l	4(sp),d0	; d0 := theta
	jsr	_LVOSPSub(a6)	; d0 := x - the rest
	move.l	d0,4(sp)	; save as new theta, and fall through

AfterShrink
	move.l	4(sp),d0	; for not normalized case
	move.l	#$C90FD941,d1	; d1 := Pi/2
	jsr	_LVOSPCmp(a6)	; compare theta and Pi/2
	bge.s	NotFirst	; it's not in first quadrant
	move.l	4(sp),d0	; get theta
	bra	GetVal		; get value and leave

NotFirst
	move.l	#$C90FD942,d1	; d1 := Pi
	move.l	4(sp),d0	; d0 := theta
	jsr	_LVOSPCmp(a6)	; compare these
	bge.s	NotSecond	; it's not in first or second

	move.l	#$C90FD942,d0	; d1 := Pi
	move.l	4(sp),d1	; d0 := theta
	jsr	_LVOSPSub(a6)	; d0 := Pi - theta
	bra	GetVal		; get value and leave

NotSecond
	move.l	#$96CBE343,d1	; d1 := 3Pi/2
	move.l	4(sp),d0	; d0 := theta
	jsr	_LVOSPCmp(a6)	; compare theta & 3Pi/2
	bge	FourthQuad	; definitely

	move.l	4(sp),d1	; d1 := theta
	move.l	#$C90FD942,d0	; d0 := Pi
	jsr	_LVOSPSub(a6)	; d0 := Pi - theta
	bra	GetVal		; get value, and return

FourthQuad
	move.l	#$C90FD943,d0	; d0 := 2Pi
	move.l	4(sp),d1	; d1 := theta
	jsr	_LVOSPSub(a6)	; d0 := 2Pi - theta
	bsr	GetVal		; get sine here
	jmp	_LVOSPNeg(a6)	; return negative

	XDEF	_p%cos
_p%cos
	move.l	_p%MathBase,a6
	move.l	4(sp),d1	; d1 := theta
	move.l	#$C90FD941,d0	; d0 := Pi/2
	jsr	_LVOSPSub(a6)	; d0 := Pi/2 - theta
	move.l	d0,4(sp)	; save as new theta
	bra	_p%sin		; call sin, leave from there

	END
