;******************************************************************************
;*	COPYRIGHT (C), 1999 SEIKO EPSON CORP.
;*  ALL RIGHTS RESERVED
;*
;*	File: SIN.S
;*
;*	C Standard Library Function "sin"
;*	The "sin" function calculates the sine of x which is radian.
;*
;*	Revision History
;*	12/08/1999	Created by H.Ishida
;*	10/25/2016	Modified by Naoyuki Sawa
;*			- P/ECE開発環境付属のsin()は、『オーバーフローする演算が直前に在ると、sin()関数の結果が不正になる。』というバグが有ったので、修正しました。
;*			  「\cc33\UTILITY\lib_src\ansilib33\math\src\sin.s」をコピーして、一行変更しただけです。変更箇所は、「;//{{2016/10/25〜」で囲った部分です。
;*			- 尚、sin.sをコンパイルするにはmathf.defも必要なので、mathf.defも「\cc33\UTILITY\lib_src\ansilib33\math\src\mathf.def」からコピーしました。
;*			  mathf.defは元のままで、一切変更していません。
;******************************************************************************
;
; --- Format of floating point (double precision) ---
;
;  63|62         52|51               32|31                                0
;   -----------------------------------------------------------------------
;  | |   exponent  |                    fraction                           |
;   -----------------------------------------------------------------------
;  |    12 bits          20 bits       |             32 bits               |
;  |            lower word             |            higher word            |

#include	"mathf.def"

;==============================================================================
;	Function: double sin(double x)
;
;	Input:  %r12: Argument x in radian double (low).
;           %r13: Argument x in radian double (high).
;
;	Output: %r10: Return value, sine of x in double (low).
;			%r11: Return value, sine of x in double (high).
;
	.code
	.align	1
	.global	sin

sin:
	pushn	%r3					; Save registers %r3...%r0.
	xsub	%sp,%sp,20			; Presearve working area in five words.

	;[%sp+16] = Taylor series's sign parameter (high).
	;[%sp+12] = Taylor series's sign parameter (low).
	;[%sp+08] = Quadrant of trigonometric function (word).
	;[%sp+07] = 2nd argument of modf, pointer to its fraction part (high).
	;[%sp+00] = 2nd argument of modf, pointer to its fraction part (low).

	ld.w	%r2,%r12			; Low word of argument x.
	ld.w	%r3,%r13			; High word of argument x.

;##### REMOVED 12/08/1999 #############
;# ;
;# ;	Clear the sign bit of argument x in %r3:%r2 by shifting
;# ;	to make it a plus value.
;# ;
;# 	sll		%r3,1				; Shift the argument x to the left  one bit.
;#	srl		%r3,1				; Shift the argument x to the right one bit.
;##### REMOVE END #####################

	xld.w	%r4,0x0				; Initialize the quadrant to 0.
	xld.w	[%sp+8],%r4			; Initialize the quadrant to 0.

;//{{2016/10/25変更:P/ECE開発環境付属のsin()は、『オーバーフローする演算が直前に在ると、sin()関数の結果が不正になる。』というバグが有ったので、修正しました。
;//	sra		%r13,1				; Check if x is minus?
;//↓2016/10/25変更:P/ECE開発環境付属のsin()は、『オーバーフローする演算が直前に在ると、sin()関数の結果が不正になる。』というバグが有ったので、修正しました。
	cmp		%r13,0				; Check if x is minus?
;//}}2016/10/25変更:P/ECE開発環境付属のsin()は、『オーバーフローする演算が直前に在ると、sin()関数の結果が不正になる。』というバグが有ったので、修正しました。
	xjrge	_ArgIsNotMinus		; Branch if plus or zero.
;
;	--- if the argument x is minus ---
;	Clear the sign bit of argument x in %r3:%r2 by shifting
;	to make it a plus value.
;
	sll		%r3,1				; Shift the argument x to the left  one bit.
	srl		%r3,1				; Shift the argument x to the right one bit.

	xld.w	%r4,0x2				; Set the quadrant 2.
	xld.w	[%sp+8],%r4			; Set the quadrant 2.
;
;	Mutiply the argument x by the constant 2*Pai
;
_ArgIsNotMinus:
	ld.w	%r12,%r2			; Argument x (low).
	ld.w	%r13,%r3			; Argument x (high).
	xld.w	%r14,TWO_PAI_L		; Multiplier, constant 2*Pai (low).
	xld.w	%r15,TWO_PAI_H		; Multiplier, constant 2*Pai (high).
								; 
	xcall	__muldf3			; Calculate multiplication; argument x = x * (2 * Pai).
								; The result product is %r11:%r10.
	ld.w	%r2,%r10			; Put result x in %r2 (low).
	ld.w	%r3,%r11			; Put result x in %r3 (high).
;
;	Check if the argument x is large radian.
;	If large, do some argument reduction, else skip reduction procedure.
;
	xld.w	%r0,LARGE_RAD_L		; Set the large radian constant (low).
	xld.w	%r1,LARGE_RAD_H		; Set the large radian constant (high).
	ld.w	%r12,%r2			; Argument x (low).
	ld.w	%r13,%r3			; Argument x (high).
	ld.w	%r14,%r0			; Large radian constant (low).
	ld.w	%r15,%r1			; Large radian constant (high).
								; Compare argument x and the large radian.
	xcall	__fcmpd				; to check if the argument x is large.
	xjrle	_ArgIsNotLarge		; Skip if the argument x is not large.
;
;	--- Argument x is large ---
;	Reduce the argument to avoid overflow.
;
	ld.w	%r12,%r2			; Argument x (low  word), 1st parameter of modf().
	ld.w	%r13,%r3			; Argument x (high word), 1st parameter of modf().
	ld.w	%r14,%sp			; Pointer to the fraction part [%sp+00], the 2nd parameter of modf().
								; 
	xcall	modf				; Calculate the integer part and the fraction part.
;
;	Use the integer part as a Tayler series's sign parameter.
;
	xld.w	[%sp+12],%r10		; Tayler series's sign parameter (low).
	xld.w	[%sp+16],%r11		; Tayler series's sign parameter (high).
;
;	Use the fraction part as a new argument x.
;
	xld.w	%r2,[%sp]			; Copy fraction result to the argument x (low).
	xld.w	%r3,[%sp+4]			; Copy fraction result to the argument x (high).

;
;	Adjust the new argument 
;	by adding the quadrant.
;
	xld.w	%r12,[%sp+8]		; Load the quadrant integer.
								; 
	xcall	__floatsidf			; Convert the quadrant to a double value.
								;
	ld.w	%r12,%r2			; Argument x (low).
	ld.w	%r13,%r3			; Argument x (high).
	ld.w	%r14,%r10			; Quadrant in double (low).
	ld.w	%r15,%r11			; Quadrant in double (high).
								;
	xcall	__adddf3			; Add the argument x and the quadrant.
								; The result is %r11:%r10.
	ld.w	%r2,%r10			; Put the result back to the argument x (low).
	ld.w	%r3,%r11			; Put the result back to the argument x (high).

	.loc	45
;# 		modf( 0.25 * x, &f );

	ld.w	%r12,%r2			; Argument x (low).
	ld.w	%r13,%r3			; Argument x (high).
	xld.w	%r14,QUARTER_L		; Constant 0.25 in double (low).
	xld.w	%r15,QUARTER_H		; Constant 0.25 in double (high).
								;
	xcall	__muldf3			; Calculate multiplication x*0.25.
								;
	ld.w	%r12,%r10			; Set x*0.25 result as 1st param of modf (low).
	ld.w	%r13,%r11			; Set x*0.25 result as 1st param of modf (high).
	ld.w	%r14,%sp			; 2nd param of modf, pointer to the fraction.
								;
	xcall	modf				; call MODF, another library function.
;
;	Calculate the new quadrant
;	by subtracting 4.0 from the argument x
;	and then multiplying by the fraction part of the argument.
;	(argument x - 4.0 * fraction)
;
	xld.w	%r12,[%sp]			; Load the fraction part (low).
	xld.w	%r13,[%sp+4]		; Load the fraction part (high).
	xld.w	%r14,FOUR_L			; Set a constant 4.0 (low).
	xld.w	%r15,FOUR_H			; Set a constant 4.0 (high).
								; 
	xcall	__muldf3			; Caculate fraction*4.0.
								; 
	ld.w	%r12,%r2			; Set the argument x.
	ld.w	%r13,%r3			; Set the argument x.
	ld.w	%r14,%r10			; Set the result of fraction*4.0 (low).
	ld.w	%r15,%r11			; Set the result of fraction*4.0 (high).
								; 
	xcall	__subdf3			; Calculate fraction*4.0-x.
								; 
	ld.w	%r12,%r10			; Put the result back to the argument x (low).
	ld.w	%r13,%r11			; Put the reuslt back to the argument x (high).
								; 
	xcall	__fixdfsi			; Make it into an integer (resulting quadrant).
								; 
	xld.w	[%sp+8],%r10		; Store the new quadrant.
	xjp	_ChkIfQuadrant1or3		; Jump to the next process,
								; completing the adjustment, 
								; when the argument x is large.
;
;	--- Argument x is not large ---
;
;	Obtain the integer part of the argument x in double
;	to use it for adjusting the Taylor series's sign parameter
;	accoring to the quadrant.
;
_ArgIsNotLarge:
	ld.w	%r12,%r2			; Argument x (low  word).
	ld.w	%r13,%r3			; Argument x (high word).
								; 
	xcall	__fixdfsi			; Cast the argument x in double to an integer.
								; %r10 <- Result integer.
	ld.w	%r0,%r10			; Keep the integer part of the argument x in %r0.
;
;	Obtain the fraction part of the argument x 
;	by subtracting the integer part.
;
	ld.w	%r12,%r0			; Set the integer part value as input to "__floatsidf".
								; 
	xcall	__floatsidf			; Cast the integer part back to a double value.
								; 
	ld.w	%r12,%r2			; Set the resulting double as subtrahend of __subdf3 (low).
	ld.w	%r13,%r3			; Set the resulting double as subtrahend of __subdf3 (high).
	ld.w	%r14,%r10			; Set the result of former __floatsidf as the next subtracter (low).
	ld.w	%r15,%r11			; Set the result of former __floatsidf as the next subtracter (high).
								; 
	xcall	__subdf3			; Subtract %14:%r15 (argument x) from %r13:%r12 (integer argument).
								; 
	xld.w	[%sp+12],%r10		; Set the subtraction result back to the argument x (low).
	xld.w	[%sp+16],%r11		; Set the subtraction result back to the argument x (high).
;
;	Calculate the new quadtrant
;	by adding the integer part of the argument x
;	and then logically anding with a constant 3.
;
	xld.w	%r4,[%sp+8]			; Quadrant integer.
	add		%r4,%r0				; Adding integer part and the argument x.
	xand	%r4,%r4,0x3			; Anding it with a consant 3.
	xld.w	[%sp+8],%r4			; Put the result as a new quadrant.
;
;	--- Checkng according to the quadrant ---
;	Check if the quadrant is 1 or 3 (odd number).
;
_ChkIfQuadrant1or3:
	xld.w	%r10,[%sp+8]		; Load the quadrant
	xand	%r10,%r10,0x1		; Check if the quadrant is 1 or 3?
	xjreq	_ChkIfQuadMoreThan1	; Skip if the quadrant is neither 1 nor 3.
;
;	--- Quadrant is 1 or 3 ---
;	Adjust the Taylor series's sign parameter
;	by calculating "param = 1 - param".
;
	xld.w	%r12,ONE_L			; Set constant 1.0 (low  word).
	xld.w	%r13,ONE_H			; Set constant 1.0 (high word).
	xld.w	%r14,[%sp+12]		; Taylor series's sign parameter (low  word).
	xld.w	%r15,[%sp+16]		; Taylor series's sign parameter (high word).
								; 
	xcall	__subdf3			; Subtract %r13:%r12 - %r15:%r14, Result is %r11:r10
								; 
	xld.w	[%sp+12],%r10		; Store the result of calculation (low  word).
	xld.w	[%sp+16],%r11		; Store the result of calculation (high word).
;
;	--- Checking if the quadrant is larger than 1 ---
;
_ChkIfQuadMoreThan1:
	xld.w	%r4,[%sp+8]			; Loading the quadrant.
	xcmp	%r4,1				; Check if it is larger than 1?
	xjrle	_SquareTaylorParam	; If not larger than 1, skip to complete calculating Tayler's param.
;
;	--- Quadrant is larget than 1 ---
;	then, negate the Taylor series's sign parameter.
;
	xld.w	%r13,[%sp+16]			; Load the Tayler series's parameter.
	XXOR	%r13,%r13,SIGN_MASK		; Negate it.
	xld.w	[%sp+16],%r13			; Store it.
;	
;	--- Calculate the square of the Tayler series's sign parameter ---
;	Complete adjusting the Taylor series's sign parameter
;	by calculating its square.
;
_SquareTaylorParam:
	xld.w	%r12,[%sp+12]		; Multiplicand, the argument x (low).
	xld.w	%r13,[%sp+16]		; Multiplicand, the argument x (high).
	xld.w	%r14,[%sp+12]		; Multiplier, the argument x (low).
	xld.w	%r15,[%sp+16]		; Multiplier, the argument x (high).
								; 
	xcall	__muldf3			; Calculate square, the argument x * argument x
								; 
	ld.w	%r2,%r10			; Put the square result in the argument x (low).
	ld.w	%r3,%r11			; Put the square result in the argument x (hig).
;
;	
;
;	(((((P4 * x + P3) * x + P2) * x + P1) * x + P0) * y)
;	----------------------------------------------------
;	           (((x + Q2) * x + Q1) * x + Q0)
;
	ld.w	%r12,%r2			; Argument square x (low).
	ld.w	%r13,%r3			; Argument square x (high).
	xld.w	%r14,CP4_L			; double 2.80782741762206855540e0 (low).
	xld.w	%r15,CP4_H			; double 2.80782741762206855540e0 (high).
								; 
	xcall	__muldf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	xld.w	%r14,CP3_L			; double -2.37859324578121572813e2 (low).
	xld.w	%r15,CP3_H			; double -2.37859324578121572813e2 (high).
								; 
	xcall	__adddf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	ld.w	%r14,%r2			; 
	ld.w	%r15,%r3			; 
								; 
	xcall	__muldf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	xld.w	%r14,CP2_L			; double 7.06413608140068845387e3 (low).
	xld.w	%r15,CP2_H			; double 7.06413608140068845387e3 (high).
								; 
	xcall	__adddf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	ld.w	%r14,%r2			; 
	ld.w	%r15,%r3			; 
								; 
	xcall	__muldf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	xld.w	%r14,CP1_L			; double -7.65864156388469564263e4 (low).
	xld.w	%r15,CP1_H			; double -7.65864156388469564263e4 (high).
								; 
	xcall	__adddf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	ld.w	%r14,%r2			; 
	ld.w	%r15,%r3			; 
								; 
	xcall	__muldf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	xld.w	%r14,CP0_L			; double 2.07823684169610118261e5 (low).
	xld.w	%r15,CP0_H			; double 2.07823684169610118261e5 (high).
								; 
	xcall	__adddf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	xld.w	%r14,[%sp+12]		; 
	xld.w	%r15,[%sp+16]		; 
								; 
	xcall	__muldf3			; 
								; 
	ld.w	%r0,%r10			; 
	ld.w	%r1,%r11			; 
	ld.w	%r12,%r2			; 
	ld.w	%r13,%r3			; 
	xld.w	%r14,CQ2_L			; double 1.08999811037129049396e2 (low).
	xld.w	%r15,CQ2_H			; double 1.08999811037129049396e2 (high).
								; 
	xcall	__adddf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	ld.w	%r14,%r2			; 
	ld.w	%r15,%r3			; 
								; 
	xcall	__muldf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	xld.w	%r14,CQ1_L			; double 5.65168679531691759621e3 (low).
	xld.w	%r15,CQ1_H			; double 5.65168679531691759621e3 (high).
								; 
	xcall	__adddf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	ld.w	%r14,%r2			; 
	ld.w	%r15,%r3			; 
								; 
	xcall	__muldf3			; 
								; 
	ld.w	%r12,%r10			; 
	ld.w	%r13,%r11			; 
	xld.w	%r14,CQ0_L			; double 1.32304666508649301250e5 (low).
	xld.w	%r15,CQ0_H			; double 1.32304666508649301250e5 (high).
								; 
	xcall	__adddf3			; 
								; 
	ld.w	%r12,%r0			; 
	ld.w	%r13,%r1			; 
	ld.w	%r14,%r10			; 
	ld.w	%r15,%r11			; 
								; 
	xcall	__divdf3			; 

	xadd	%sp,%sp,20			; Roll up the working area.
	popn	%r3					; Restore registers %r3...%r0.
	ret							; 

