;============================================================================
;  MSP430 Floating Point Package   Version 4.10  IAR
;
;  Trigonometric and other Functions
;  Texas Instruments Deutschland
;  Date:    March  9   2000    4.07.1           IAR Version
;  Version: 4.02  12.3.97 LB   Sin X, Sinh X
;  Version: 4.03  18.3.97 LB   Cos X, Cosh X, FLT_RNG added
;  Version: 4.04  27.3.97 LB   Tan X, Cot X
;  Version: 4.05   7.8.97 LB   Ln X,  Exp X, A^B       tested
;  Version: 4.06 12.10.97 LB   X^0.5		   tested
;  Version: 4.07 14.10.97 LB   X^1/3	    tested
;  Version: 4.07.1 7.3.00 LB   Fehler: FLT1 enabled
;  Version: 4.10  23.7.02 LB   Error: square root
;==============================================================================
;  BEGIN OF THE TRIGONOMETRIC FUNCTIONS
;
; Sin, Cosine, Hyperbolic Sine, Hyperbolic Cosine of X (radians)
;
; Call: MOV   #addressX,RPARG	; RPARG points to the address of X
;	CALL  #FLT_xxx		; Call the function
;	...			; RPARG, RPRES and SP point to result
;
; Range: -2xPi < X < +2xPi	for larger numbers FAST loss of accuracy
;
;   Initialization for the trigonometric functions
;  +--------------+-------+-------+--------+--------+
;  | INIT	  | sin X | cos X | sinh X | cosh X |
;  +--------------+-------+-------+--------+--------+
;  | Sign Mask	  | 080h  |  080h | 000h   |  000h  |
;  | n		  | 1.0   |  0.0  | 1.0    |  0.0   |
;  | Serial Term  |   X   |  0.0  |   X    |  0.0   |
;  | Result Area  |   X   |  0.0  |   X    |  0.0   |
;  +--------------+-------+-------+--------+--------+
;
FPL      EQU    (ML/8)+1        ; Length of FPP numbers (bytes)
;
; Floating Point Sine Function: Result on TOS = SIN(@RPARG)
; Prepare the stack with the initial constants
;
FLT_SIN PUSH	#80h		; Sign mask (toggle)
	JMP	SINc
;
; Hyperbolic Sine Function: Result on TOS = SINH(@RPARG)
;
FLT_SINH PUSH	#00h		; Sign mask (always pos.)
SINc	PUSH	#0		; n: 1
     IF DOUBLE=1
	PUSH	#0
     ENDIF
	PUSH	#08000h 	; .FLOAT 1.0
;
     IF DOUBLE=1
        PUSH    4(R5)        ; Series term: X
     ENDIF
        PUSH    2(R5)
        PUSH    @R5          ;
;
	JMP	TRIGCOM 	; To common part
;
; Floating Point Cosine Function: Result on TOS = COS(@RPARG)
; Prepare the stack with the initial constants
;
FLT_COS PUSH	#80h		; Sign mask (toggle)
	JMP	COSc
;
; Hyperbolic Cosine Function: Result on TOS = COSH(@RPARG)
;
FLT_COSH PUSH	#00h		; Sign mask (always pos.)
COSc	PUSH	#0		; n: 0
     IF DOUBLE=1
	PUSH	#0
     ENDIF
	PUSH	#00h		; .FLOAT 0.0
;
     IF DOUBLE=1
	PUSH	#0		; Series term: 1.0
     ENDIF
	PUSH	#0
	PUSH	#08000h 	; .FLOAT 1.0
;
; Common part for sin X, cos X, sinh X and cosh X
;
TRIGCOM  EQU    $
     IF DOUBLE=1
        PUSH    4(R5)        ; Push X onto stack (gets X^2)
     ENDIF
        PUSH    2(R5)        ;
        PUSH    @R5          ;
;
	MOV	RPARG,RPRES	; Both pointers to X
	CALL	#FLT_MUL	; X^2 to actual stack
;
	ADD	#FPL,RPARG	; Copy serial term to result space
        MOV     @R5+,3*FPL+4(SP)   ; is X or 1.0
        MOV     @R5+,3*FPL+6(SP)
     IF DOUBLE=1
        MOV     @R5+,3*FPL+8(SP)
     ENDIF
	SUB	#FPL,SP 	; Working area for calculations
	MOV	SP,RPRES
;
TRIGLOP MOV	#FLT2,RPARG	; Address of .FLOAT 2.0
	ADD	#3*FPL,RPRES	; Address n
	CALL	#FLT_ADD	; n + 2
        MOV     @R5+,3*FPL(SP)  ; (n+2) -> n
        MOV     @R5+,3*FPL+2(SP)
     IF DOUBLE=1
        MOV     @R5+,3*FPL+4(SP)
     ENDIF
;
; Build (n+1)x(n+2) for next term. (n+2)^2 - (n+2) = (n-1)x(n+2)
;
	MOV	RPRES,RPARG	; Both point to (n+2)
	CALL	#FLT_MUL	; (n+2)^2
	ADD	#3*FPL,RPARG	; Point to old n
	CALL	#FLT_SUB	; (n+2)^2 -(n+2) = (n+1)x(n+2)
;
; The serial term is divided by (n+1)x(n+2)
;
	ADD	#2*FPL,RPRES	; Point to serial term
	CALL	#FLT_DIV	; Serial term/(n+1)x(n+2)
	ADD	#FPL,RPARG	; Point to x^2
	CALL	#FLT_MUL	; ST x X^2/(n+1)x(n+2)
	JN	TRIGERR 	; Error, status in SR and HELP
	XOR	4*FPL(SP),0(SP) ; Modify sign of new serial term
        MOV     @R5+,2*FPL(SP)        ; save new serial term
        MOV     @R5+,2*FPL+2(SP)
      IF DOUBLE=1
        MOV     @R5+,2*FPL+4(SP)
      ENDIF
	ADD	#3*FPL+4,RPARG		; Point to result area
	CALL	#FLT_ADD		; Old sum + new serial term
        MOV     @R5+,4*FPL+4(SP)     ; New result to result area
        MOV     @R5+,4*FPL+6(SP)
     IF DOUBLE=1
        MOV     @R5+,4*FPL+8(SP)
     ENDIF
;
; Check if enough iterations are made
;
	CMP	Nmax,3*FPL(SP)	; Compare with nmax (2 x iterations)
	JLO	TRIGLOP
;
TRIGERR ADD	#4*FPL+2,SP	; Housekeeping: free stack
	BR	#FLT_END	; To completion part. Error in HELP
;
     IF DOUBLE=1
FLT2    DW      08100h,00000h,00000h ; .double 2.0
     ELSE
FLT2    DW      08100h,00000h   ; .float 2.0
     ENDIF
Nmax    DW      08470h,00000h   ; .FLOAT 30.0 (Iterations x 2)
;
;-----------------------------------------------------------------------
; Subroutine FLT_RNG moves angle X into the range -Pi < X < +Pi
;
; Call: MOV   #addressX,RPARG	; RPARG points to the address of X
;	CALL  #FLT_RNG		; Call the function
;	...			; RPARG, RPRES and SP point to result on TOS
;
; Range: -100xPI < X < +100xPI	loss of accuracy increases with X
;
FLT_RNG PUSH    @R5          ; Save sign of X on stack
	AND	#080h,0(SP)	; Only sign remains
	SUB	#FPL,SP 	; Reserve space for 2^n x Pi
     IF DOUBLE=1
        PUSH    4(R5)        ; X on stack
     ENDIF
        PUSH    2(R5)
        PUSH    @R5
	BIC	#080h,0(SP)	; |X| remains
FR1	MOV	FLT2PI,FPL(SP)	; 2xPi to stack
	MOV	FLT2PI+2,FPL+2(SP)
     IF DOUBLE=1
	MOV	FLT2PI+4,FPL+4(SP)
     ENDIF
	CMP	@SP,FLTPI	; Pi - |X|
	JHS	FR2		; Pi > |X|: range process done
;
; Successive approximation by subtracting 2^n x2Pi
;
FR3	INC.B	FPL+1(SP)	; 2Pi x 2
	CMP	@SP,FPL(SP)	; 2^n x 2Pi - |X|
	JLO	FR3		; 2^n x 2Pi < |X|
	DEC.B	FPL+1(SP)	; 2^n x 2Pi > |X| divide by 2
	MOV	SP,RPRES	; Address |X|
	MOV	SP,RPARG
	ADD	#FPL,RPARG	; Address 2^n x 2Pi
	CALL	#FLT_SUB	; |X| - 2^n x 2Pi
	JMP	FR1		; Check if in range now
;
; Move X (now between -Pi and +Pi) to old working area
;
FR2	XOR	2*FPL(SP),0(SP) ; Correct sign of X
	MOV	@SP+,2*FPL+2(SP) ; Result to old WA
	MOV	@SP+,2*FPL+2(SP)
     IF DOUBLE=1
	MOV	@SP+,2*FPL+2(SP)
     ENDIF
	ADD	#FPL+2,SP	; To return address of FLT_RNG
	BR	#FLT_END	; To completion part
;
     IF DOUBLE=1
FLTPI   DW      08149h,0FDAh,0A221h ; .DOUBLE 3.141592653589793 Pi
FLT2PI  DW      08249h,0FDAh,0A221h ; .DOUBLE 3.141592653589793*2 2xPi
     ELSE
FLTPI   DW      08149h,0FDBh    ; .FLOAT  3.141592653589793 Pi
FLT2PI  DW      08249h,0FDBh    ; .FLOAT  3.141592653589793*2 2xPi
     ENDIF
;--------------------------------------------------------------------------
; Tangens of X (radians)
;
; Call: MOV   #addressX,RPARG	; RPARG points to the address of X
;	CALL  #FLT_TAN		; Call the tangens function
;	...			; RPARG, RPRES and SP point to result
;
; Range: -2xPi < X < +2xPi	for larger numbers FAST loss of accuracy
;--------------------------------------------------------------------------
FLT_TAN CLR	R4		; Offset for tan X
	JMP	TRI_COM1	; Go to common handler
;
; Cotangens of X (radians)
;
; Call: MOV   #addressX,RPARG	; RPARG points to the address of X
;	CALL  #FLT_COT		; Call the cotangens function
;	...			; RPARG, RPRES and SP point to result
;
; Range: -2xPi < X < +2xPi	for larger numbers FAST loss of accuracy
;--------------------------------------------------------------------------
FLT_COT MOV	#2,R4		; Offset for cot X
	JMP	TRI_COM1	; Go to common handler
;
; Hyperbolic Tangens of X (radians)
;
; Call: MOV   #addressX,RPARG	; RPARG points to the address of X
;	CALL  #FLT_TANH 	; Call the hyperbolic tangens function
;	...			; RPARG, RPRES and SP point to result
;
; Range: -2xPi < X < +2xPi	for larger numbers FAST loss of accuracy
;--------------------------------------------------------------------------
FLT_TANH MOV	 #4,R4		 ; Offset for tanh X
	JMP	TRI_COM1	; Go to common handler
;
; Hyperbolic Cotangens of X (radians)
;
; Call: MOV   #addressX,RPARG	; RPARG points to the address of X
;	CALL  #FLT_COTH 	; Call the hyperbolic cotangens function
;	...			; RPARG, RPRES and SP point to result
;
; Range: -2xPi < X < +2xPi	for larger numbers FAST loss of accuracy
;--------------------------------------------------------------------------
FLT_COTH MOV	 #6,R4		 ; Offset forcoth X
;
; Common Handler for tan, cot, tanh, coth and exponent function
;
TRI_COM1  EQU $                 ;
        MOV     @R5+,2(SP)   ; Copy X to result space
        MOV     @R5+,4(SP)
         IF     DOUBLE=1
        MOV     @R5,6(SP)
         ENDIF
	SUB	#FPL,SP 	; Allocate new result space
	SUB	#4,RPARG	; Point to X again
	CALL	FT1(R4) 	; Calculate 1st function
	JN	TERR2		; Error: error code in HELP
	SUB	#FPL,SP 	; Allocate cosine working area
	ADD	#FPL+2,RPARG	; Point to X
	CALL	FT2(R4) 	; Calculate 2nd function
	ADD	#FPL,RPRES	; Point to result of 1st function
	CALL	#FLT_DIV	; 1st result/2nd result
	MOV	@SP+,2*FPL(SP)	; Final result to working area
	MOV	@SP+,2*FPL(SP)
         IF     DOUBLE=1
	MOV	@SP+,2*FPL(SP)
         ENDIF
TERR2	ADD	#FPL,SP 	; Skip 1st result
	BR	#FLT_END
;
FT1     DW      FLT_SIN         ; tan = sin/cos    1st function
        DW      FLT_COS         ; cot = cos/sin
        DW      FLT_SINH        ; tanh = sinh/cosh
        DW      FLT_COSH        ; coth = cosh/sinh
;
FT2     DW      FLT_COS         ; tan = sin/cos    2nd function
        DW      FLT_SIN         ; cot = cos/sin
        DW      FLT_COSH        ; tanh = sinh/cosh
        DW      FLT_SINH        ; coth = cosh/sinh
;
;---------------------------------------------------------------------------
;  BEGIN OF OTHER FUNCTIONS
; Natural Logaritm Function:	Result on TOS = LN(@RPARG)
;
; Call: MOV   #addressX,RPARG	; RPARG points to the address of X
;	CALL  #FLT_LN		; Call the function
;	...			; RPARG, RPRES and SP point to result lnX
;
; Range: +2.9x10^-38 < X < +3.4x10^38
;
; Errors: X = 0:  N = 1, C = 1, Z = 0	Result: -3.4E38
;	  X < 0:  N = 1, C = 1, Z = 0	Result: -3.4E38
;
; Stack: 3 x FPL + 6
;
FLT_LN   EQU   $
	PUSH	#0		; N binary (divisor, power)
     IF DOUBLE=1
        PUSH    4(R5)        ; Push X onto stack
     ENDIF
        PUSH    2(R5)        ;
        PUSH    @R5          ;
;
; Check for the legal range of X: 0 < X
;
	MOV	#FLT0,RPRES	 ; Check valid range: 0 < X
	CALL	#FLT_CMP
	JHS	LNNEG		; X is negative
;
; If X = 1.0 the result is 0.0
;
 	MOV	#FLT1,RPRES	 ; Check if X= 1
	CALL	#FLT_CMP
	JEQ	LN1P0		; X is 1
;
; The exponent of X is multiplied with ln2. Then ln1.5 is added to correct
; the division by 1.5. Result is base for final result
;
	SUB	#FPL,SP 	; Reserve working space
        MOV.B   1(R5),HELP   ; Copy exponent of X
	XOR	#80h,HELP	; Correct sign of exponent
	SXT	HELP		;
	MOV	HELP,0(SP)
	MOV	SP,RPARG
	CALL	#CNV_BIN16	; Exponent to FP format
	MOV	#FLN2,RPARG	; To ln2
	CALL	#FLT_MUL	; exp x ln2
	MOV	#FLN1P5,RPARG	 ; To ln1.5
	CALL	#FLT_ADD	; exp x ln2 + ln1.5
        MOV     @R5+,2*FPL+4(SP)     ; To result area
        MOV     @R5+,2*FPL+6(SP)
     IF DOUBLE=1
        MOV     @R5+,2*FPL+8(SP)
     ENDIF
;
; The mantissa of X is converted into the range -0.33 to +0.33 to get
; fast convergion
;
	ADD	#FPL,SP 	; Back to X
	MOV	SP,RPRES	; RPRES points to X
	MOV.B	#80h,1(SP)	; 1.0 =< X < 2.0
	MOV	#FLT1P5,RPARG	; To .FLOAT 1.5
	CALL	#FLT_DIV	; 2/3 =< X < 4/3
	MOV	#FLT1,RPARG	; To .FLOAT 1.0
	CALL	#FLT_SUB	; -1/3 =< X < +1/3
;
     IF DOUBLE=1                ;
	PUSH	#0		; 1.0 to X^N area
     ENDIF
	PUSH	#0
	PUSH	FLT1
;
     IF DOUBLE=1                ; N (FLT1.0) on stack
	PUSH	#0
     ENDIF
	PUSH	#0
	PUSH	FLT1
	SUB	#FPL,SP 	; Working area
;
LNLOP    EQU $
	MOV	SP,RPRES
	ADD	#2*FPL,RPRES	; To X^N
	MOV	SP,RPARG
	ADD	#3*FPL,RPARG	; To X
	CALL	#FLT_MUL	; X^(N+1)
        MOV     @R5+,2*FPL(SP)     ; New X^(N+1) -> X^N
        MOV     @R5+,2*FPL+2(SP)
     IF DOUBLE=1
        MOV     @R5+,2*FPL+4(SP)     ; RPARG points to N
     ENDIF
	CALL	#FLT_DIV	; X^N/N
    INC	4*FPL(SP)	; Incr. binary N
	BIT	#1,4*FPL(SP)	; N even?
	JNZ	LN1
	XOR	#80h,0(SP)	; Yes, change sign of X^N/N
;
LN1	ADD	#4*FPL+4,RPARG		; Point to result area
	CALL	#FLT_ADD		; Old result + new one
        MOV     @R5+,4*FPL+4(SP)     ; New result to result area
        MOV     @R5+,4*FPL+6(SP)
     IF DOUBLE=1
        MOV     @R5+,4*FPL+8(SP)
     ENDIF
;
; Float N is incremented
;
	MOV	#FLT1,RPARG	; To .FLOAT 1.0
	ADD	#FPL,RPRES	; To N
	CALL	#FLT_ADD
        MOV     @R5+,FPL(SP)     ; N+1 to N area
        MOV     @R5+,FPL+2(SP)
     IF DOUBLE=1
        MOV     @R5+,FPL+4(SP)
     ENDIF
;