;;; Math subroutines used by BkLisp
;;; This code is derived from Vilnus BASIC ROM source code
;;; https://github.com/sergev/vak-opensource/blob/master/bk/basic-vvu-1985/float.mac

;.INCLUDE /config.inc/

;.IIF EQ C$AMAL .INCLUDE /pdp11.inc/

.SBTTL	Floating point module


;; PUSH R0, R1, R2, R3
;; Call it with	JSR	R0, PU0123
PU0123:	PUSH	R1, R2, R3, 6(SP)
	RTS	R0

;; PUSH R3, R2, R1, R0
;; Call it with	JSR	R3, PU3210
PU3210:	PUSH	R2, R1, R0, 6(SP)
	RTS	R3


; Call it with	JSR	R3, PU3210
;;!!!R PU3210:	PUSH	R2, R1, R0, 6(SP)
;;!!!R 	RTS	R3

PO0123:	POP	R3		; return address
	POP	R0, R1, R2
	RTS	R3

; Call it with	JSR	R4, SAVER4
SAVER4:	PUSH	R4		; return address
	MOV	2(SP), R4	; restore R4
	TST	(PC)+
$SAVJMP::
	.WORD	0		; temporary var for saving return address
	BNE	1$
	MOV	R4, $SAVJMP
	CLR	2(SP)
1$:	RETURN

; Call it with	JSR	R5, SAV5.0
SAV5.0:	PUSH	R4
	JSR	R3, PU3210	; PUSH	R3, R2, R1, R0
	PUSH	R5		; return address
	TST	$SAVJMP
	BNE	1$
	MOV	R4, $SAVJMP
	CLR	12(SP)		; R4 in stack
1$:	RETURN





; Call it by	JSR	R4, PUSH4Z
PUSH4Z:	CLR	-(SP)
	CLR	-(SP)
	CLR	-(SP)
	BR	RPFLT4

; Call it by	JSR	R4, PFLT4
PFLT4:	CLR	-(SP)
	CLR	-(SP)
	PUSH	(R4)+		; first word of FLT4
RPFLT4:	PUSH	6(SP)		; original R4
	CLR	10(SP)		; last word of FLT4
	RTS	R4

;; Pushes WORDS: 0, 0, 0, x
.MACRO	.PFLT4	x
	JSR	R4, PFLT4
	.WORD	x
.ENDM


AD3210:	ADD	2(SP), R3
	ADC	R2
	ADC	R1
	ADC	R0
	MOV	(SP)+, (SP)
	RETURN

;; MULTIPLE WORD SUBTRACTION
SBC4.1: SUB	(R5), R4	; SUBTRACT LSB'S
	SBC	R3		; RIPPLE BORROW
	SBC	R2
	SBC	R1
	SUB	-(R5), R3	; THIRD WORD
	SBC	R2
	SBC	R1
	SUB	-(R5), R2	; SECOND WORD
	SBC	R1
	SUB	-(R5), R1	; MSB (DON'T CARE ABOUT BORROW)
	RETURN


ASL1.4:	ASL	R4		; ZERO INTO LSB OF REMAINDER
	ROL	R3		; THIRD REMAINDER WORD
	ROL	R2		; SECOND REMAINDER WORD
	ROL	R1		; SIGN BIT INTO CARRY
	RETURN


QDAUG2:	CALL	(PC)
PDAUG2:	ASL	R3
	ROL	R2
	ROL	R1
	ROL	R0
	RETURN

;; RIGHT SHIFT number in R5::R0:R1:R2:R3 by 8 bit
;; R5 does not change
SR8DBL: CLRB	R3
	BISB	R2, R3
	SWAB	R3
	CLRB	R2
	BISB	R1, R2
	SWAB	R2
	CLRB	R1
	BISB	R0, R1
	SWAB	R1
	CLRB	R0
	BISB	R5, R0
	SWAB	R0
	RETURN



	;-----------------------------------------------------
	;    Conversion from string to DOUBLE
	;-----------------------------------------------------
	;
$PACK::	POP	R3		; R3: return address
	JSR	R4, PUSH4Z
	MOV	R3, (SP)
	PUSH	R0, R1, R2
	PUSH	R3, R4, R5
	PUSH	#-18., #101
	MOV	32(SP), R5
	ADD	R5, 30(SP)
	CALL	CL0123
1$:	CMP	R5, 30(SP)	; End of string?
	BGE	PACK31
	MOVB	(R5)+, R4
;	CMPB	R4, #C.SP
;	BEQ	1$
;
	CMPB	R4, #'+
	BEQ	PACK7
	CMPB	R4, #'-
	BNE	PACK4
	INC	26(SP)
	BR	PACK7

PACK5:	CMPB	R4, #9.
	BGT	PACK18
	INC	2(SP)
	BGT	PACK7
	CALL	PDAUG5
	CALL	PDAUG2
	ADD	R4, R3
	ADC	R2
	ADC	R2
	ADC	R0
PACK7:	CMP	R5, 30(SP)	; End of string?
	BGE	PACK6
	MOVB	(R5)+, R4
;	CMPB	R4, #C.SP
;	BEQ	7$
PACK4:	SUB	#'0, R4
	BLT	PACK15
	BNE	PACK5
	CMP	2(SP), #-18.
	BNE	PACK5
	TSTB	23(SP)
	BEQ	PACK7
	DEC	24(SP)
	BR	PACK7
;
PACK32:	TSTB	27(SP)
	BEQ	24$
	NEG	R3
24$:	ADD	R3, 24(SP)
	MOV	32(SP), R3
;
PACK8:	DEC	R5
PACK6:	MOV	R5, 10(SP)	; R3 on Return
	TST	R0
	BNE	PACK9
	TST	R1
	BNE	PACK9
	TST	R2
	BNE	PACK9
	TST	R3
	BNE	PACK9
PACK31:	MOV	R0, 24(SP)
	MOV	R1, 26(SP)
	MOV	R2, 30(SP)
	MOV	R3, 32(SP)
	CMP	(SP)+, (SP)+
	POP	R5, R4
	POP	R3, R2, R1, R0
	RETURN

PACK9:	TSTB	23(SP)
	BEQ	2$
	NEG	2(SP)
2$:	TST	2(SP)
	BGE	11$
	CLR	2(SP)
11$:	ADD	24(SP), 2(SP)
	BGT	12$
	BLT	PACK25
	BR	PACK30
;
12$:	CMP	R0, #31462
	BHI	14$
	CALL	PDAUG5
	INC	(SP)
13$:	DEC	2(SP)
	BGT	12$
	BR	PACK30
;
14$:	CALL	PD5DL4
	ADD	#3, (SP)
	BR	13$

PACK15:	CMPB	R4, #-2
	BNE	PACK8
	TSTB	23(SP)
	BNE	PACK8
	COMB	23(SP)
	MOV	2(SP), 24(SP)
	BR	PACK7

PACK18:	BICB	#40, R4
	SUB	#24, R4
	BEQ	19$
	DEC	R4
	BNE	PACK8
19$:	CMPB	(R5), #100	;ELSE OR NAME?
	BGT	PACK8
	BICB	#40, -(R5)
	MOVB	(R5)+, 22(SP)
10$:	MOVB	(R5)+, R4
	CMP	R5, 30(SP)
	BHI	PACK8
;	CMPB	R4, #40
;	BEQ	10$
	MOV	R3, 32(SP)
	CLR	R3
	CMPB	R4, #'+
	BEQ	20$
	CMPB	R4, #'-
	BNE	22$
	INCB	27(SP)
20$:	MOVB	(R5)+, R4
	CMP	R5, 30(SP)
	BHI	PACK32
22$:;	CMPB	R4, #40
;	BEQ	20$
	SUB	#60, R4
	BLT	PACK32
	CMPB	R4, #11
	BGT	PACK32
	CMP	R3, #500
	BHIS	20$
	ASL	R3
	ADD	R3, R4
	ASL	R3
	ASL	R3
	ADD	R4, R3
	BR	20$

PACK16:	CMP	(SP), #377
	BGT	PACK34
	MOV	(SP), R5
	CALL	SR8DBL
	ROR	26(SP)
	CALL	PDAL2
	CALL	ADC321
	ADC	R0
	BVS	PACK34
	BCC	PACK31
PACK34:	ER.MATH


PACK25:	TST	R0
	BLT	27$
26$:	DEC	(SP)
	CALL	PDAUG2
	BPL	26$
27$:	CALL	DG4DL5
	SUB	#3, (SP)
	INC	2(SP)
	BLT	PACK25
;
PACK30:	DEC	(SP)
	CALL	PDAUG2
	BCC	PACK30
	ADD	#200, (SP)
	BGT	PACK16
	PUSH	#PACK31
CL0123:	CLR	R0
CL123:	CLR	R1
	CLR	R2
	CLR	R3
	RETURN

	;---------------------------------------------------------
	;	Unpacking from SINGLE or DOUBLE to string formats: G,F,E,D
	;---------------------------------------------------------
EGO::	POP	R5, R0
	MOV	(SP), R1
	CLR	R2
	CLR	R3
	MOV	#6, (SP)
	PUSH	#42403
	BR	DGO1
;
$DGO::	POP	R5
	CALL	PO0123
	PUSH	#20, #42003
DGO1:	PUSH	#1, #$BUFOUT, #30, R5
;
UNPCK::	PUSH	R4
	MOV	R4, $SAVJMP
	JSR	R4, PUSH4Z
	ADD	16(SP), 14(SP)	; SP+14: buffer end
;
	ROL	R0
	ROL	6(SP)
	SWAB	R0
	MOVB	R0, 2(SP)
	BNE	2$
	CLR	R4
3$:	MOV	24(SP), R5
	ADD	20(SP), R5
	TSTB	22(SP)
	BEQ	4$
	MOV	R5, 2(SP)
	CALL	ROUND1
	RORB	22(SP)
	BCC	UNPC5
	CMP	(SP), #-1
	BLT	UNPC5
	MOV	2(SP), R5
	CMP	(SP), R5
	BGT	UNPC5
	SUB	(SP), R5
	CLRB	22(SP)
	MOV	R5, 24(SP)
	CLR	20(SP)
	BR	41$
2$:	SEC
	ROR	R0
	CLRB	R0
	SWAB	R1
	BISB	R1, R0
	CLRB	R1
	SWAB	R2
	BISB	R2, R1
	CLRB	R2
	SWAB	R3
	BISB	R3, R2
	CLRB	R3
;
	SUB	#200, 2(SP)
6$:	BLT	20$
	BEQ	30$
;
10$:	TST	R0
	BLT	11$
	CALL	PDAUG2
	DEC	2(SP)
	BGT	10$
	BR	30$
11$:	CALL	DG4DL5
	INC	(SP)
	SUB	#3, 2(SP)
	BR	6$
;
20$:	CMP	R0, #146314
	BHIS	21$
	CMP	2(SP), #-3
	BGT	21$
	CALL	DG5DL4
	DEC	(SP)
	ADD	#2, 2(SP)
	BR	22$
21$:	CALL	PDAL2
22$:	INC	2(SP)
	BNE	20$
;
30$:	CLR	R4
31$:	CALL	DG5DL4
	CALL	UDAUG8
	TST	R4
	BNE	3$
	DEC	(SP)
	BR	31$
;
4$:	ADD	(SP), R5
41$:	DEC	14(SP)
UNPC44:	MOV	14(SP), R5
	SUB	24(SP), R5
	DEC	R5
	MOV	R5, 4(SP)
	DEC	R5
	TSTB	22(SP)
	BNE	UNPC40
	ADD	(SP), 20(SP)
	BLE	UNPC40
	SUB	20(SP), R5
	PUSH	#UNPC42
SIGN:	MOV	R5, 26(SP)
	CMP	R5, 20(SP)
	BLO	2$
	ROR	10(SP)
	BCC	1$
	MOVB	#'-, (R5)+
	RETURN
1$:	MOVB	#C.SP, (R5)+
3$:	RETURN
;
2$:	ROR	10(SP)
	BCS	ASTER
	INC	R5
	CMP	R5, 20(SP)
	BHIS	3$
ASTER:	TST	(SP)+
	MOV	16(SP), R5
	MOV	R5, 24(SP)
	MOVB	#'%, (R5)+
	COM	22(SP)
	BR	UNPEND
UNPC5:	SUB	#5, 14(SP)
	TST	20(SP)
	BLE	UNPC44
	MOV	14(SP), R5
	SUB	24(SP), R5
	DEC	R5
	MOV	R5, 4(SP)
	SUB	20(SP), R5
	DEC	R5
	CALL	SIGN
	PUSH	#UNPC50
NUMB:	CMP	#12, R4
	BGT	1$
	MOVB	#'1, (R5)+
	SUB	#12, R4
1$:	CMP	6(SP), R5
	BNE	2$
	MOVB	#'., (R5)+
2$:	CMP	16(SP), R5
	BLOS	NUMB3
	ADD	#60, R4
	MOVB	R4, (R5)+
	CLR	R4
	CALL	DG5DL4
	PUSH	#1$
UDAUG8:	PUSH	R5
	MOV	#3, R5
1$:	CALL	PDAUG2
	ROL	R4
	SOB	R5, 1$
	POP	R5
	RETURN

NUMB3:	MOV	#'0, R4
	CMPB	-(R5), R4
	BEQ	NUMB3
	CMPB	(R5), #'.
	BNE	4$
	CMPB	-1(R5), R4
	BHIS	5$
	MOVB	R4, (R5)
4$:	INC	R5
5$:	RETURN


UNPC40:	CALL	SIGN
	MOVB	#'., (R5)+
	MOVB	#'0, (R5)+
UNPC42:	MOV	4(SP), R5
	TST	20(SP)
	BGT	43$
	INC	R5
43$:	SUB	20(SP), R5
	CALL	NUMB
	TSTB	22(SP)
	BEQ	UNPEND

UNPC50:	SUB	20(SP), (SP)
	MOVB	23(SP), (R5)+
	MOV	(SP), R4
	BGE	52$
	NEG	R4
	MOVB	#'-, (R5)+
	BR	UNPC53
;
52$:	MOVB	#'+, (R5)+
UNPC53:	MOVB	#'0-1, (R5)
60$:	INCB	(R5)
	SUB	#12, R4
	BGE	60$
55$:	ADD	#72, R4
	INC	R5
	MOVB	R4, (R5)+
UNPEND:	MOVB	#C.SP, (R5)+
	SUB	24(SP), R5
	ADD	#10, SP
	POP	R4
	CLR	$SAVJMP
	MOV	(SP)+, 4(SP)
	CMP	(SP)+, (SP)+
	ROL	2(SP)
	MOV	R5, 2(SP)
	RETURN

DG5DL4:	JSR	R0, PU0123
	CALL	DUDAL2
	CALL	ADC321
	ADC	R0
	CALL	AD3210
	ADC	R4
	ADD	(SP)+, R2
	ADC	R1
	ADC	R0
	ADC	R4
	ADD	(SP)+, R1
	ADC	R0
	ADC	R4
	ADD	(SP)+, R0
	ADC	R4
	RETURN

;
DG4DL5:	MOV	#20, R4
	CALL	UDAL2
	JSR	R3, PU3210	; PUSH	R3, R2, R1, R0
1$:	CALL	DUDAL2
2$:	CALL	UDAL2
	ADD	6(SP), R3
	ADC	R2
	ADC	R1
	ADC	R0
	ADD	4(SP), R2
	ADC	R1
	ADC	R0
	ADD	2(SP), R1
	ADC	R0
	ADD	(SP), R0
	NEG	R4
	BLT	2$
	SOB	R4, 1$
	ADD	#10, SP
	RETURN


ROUND1:	CMP	R5, #24
	BGT	4$
	TST	R5
	BEQ	5$
	BLT	4$
	PUSH	R4
	JSR	R0, PU0123
	MOV	#100000, R0
	CALL	CL123
1$:	DEC	R5
	BEQ	2$
	CALL	DG4DL5
	CALL	UDAL2
	CALL	DUDAL2
	BR	1$
2$:	CLR	R4
	CALL	AE3210
	ADC	R4
	ADD	(SP)+, R4
3$:	CMP	#12, R4
	BGT	4$
	INC	2(SP)
4$:	RETURN
;
5$:	ADD	#5, R4
	BR	3$

CRO123:	CLC
	BR	ROR123

PD5DL4:	CMP	R0, #146314
	BCS	1$
	CALL	UDAL2
	INC	2(SP)
1$:	JSR	R0, PU0123
	PUSH	#PLIUS
DUDAL2:	CALL	(PC)
UDAL2:	CLC
PDAL2:	ROR	R0
ROR123:	ROR	R1
	ROR	R2
	ROR	R3
	RETURN


;
PDAUG5:	JSR	R0, PU0123
	CALL	QDAUG2
;
PLIUS:	CALL	AE3210		; Changes stack
	RETURN




$DSGN::	CLR	4(SP)
	CLR	6(SP)
;
$SSGN::	TST	(SP)
	BEQ	1$
	ASL	(SP)
	MOV	#100400, (SP)
	ROR	(SP)
	CLR	2(SP)
1$:	JMP	@(R4)+

$ID::	MOV	(SP), R0
	CLR	(SP)
	CLR	-(SP)
	PUSH	R0
;
IS::	CLR	-(SP)
	MOV	2(SP), R1
	BGT	IR1
	BEQ	IR4
	NEG	R1
IR1:	ROL	-(SP)
	MOV	#221, R2
	CLRB	4(SP)
IR2:	DEC	R2
	ROL	R1
	BCC	IR2
	MOVB	R1,5(SP)
	CLRB	R1
	BISB	R2, R1
	SWAB	R1
	ROR	(SP)+
	ROR	R1
	RORB	3(SP)
	MOV	R1, (SP)
IR4:	JMP	@(R4)+
;
$DI::	MOV	(SP)+, 2(SP)
	MOV	(SP)+, 2(SP)
SI::	CLR	R2
	INC	R2
	POP	R1
	ROL	(SP)
	ROL	R1
	ROL	-(SP)
	MOVB	R1, R0
	CLRB	R1
	SWAB	R1
	SUB	#201, R1
	BLT	RI1
	BEQ	RI2
	CMP	#17, R1
	BLT	RI3
	SWAB	R0
	CLRB	R0
	BISB	3(SP), R0
 RI4:	ROL	R0
	ROL	R2
	SOB	R1, RI4
 RI2:	NEG	R2
	BVS	RI5
	BGT	RI3
 RI7:	ROR	(SP)+
	BCS	RI6
	NEG	R2
 RI6:	MOV	R2, (SP)
	JMP	@(R4)+
 ;
 RI5:	ROR	(SP)+
	BCS	RI6
 RI3:	ER.MATH
 RI1:	CLR	R2
	BR	RI7

	.MACRO	FPUSH,A
	.NTYPE	ATV,A
	.IF	EQ,ATV&60-40
		MOV	A, -(SP)
		MOV	A, -(SP)
		MOV	A, -(SP)
		MOV	A, -(SP)
	.IFF
	.IF	EQ	ATV&7-6
	.REPT 4
	MOV	6+A,-(SP)
	.ENDR
	.IFF
		MOV	6+A, -(SP)
		MOV	4+A, -(SP)
		MOV	2+A, -(SP)
		MOV	A, -(SP)
	.ENDC
	.ENDC
	.ENDM

;; call these with	JSR	R4, @#PUFxxS		(37 addressation mode)
PUF0S:	BR	PUF30S
	BR	PUF30S
	. = PUF0S + 6
	BR	PUF30S
	BR	PUF30S
	BR	PUF30S
	BR	PUF30S
	BR	PUF30S
	BR	PUF30S
	. = PUF0S + 24
	BR	PUF30S
	. = PUF0S + 30
PUF30S:	SUB	#6, SP
	PUSH	R4			; return address
	PUSH	10(SP)			; save caller R4
	MOV	-(R4), R4		; R4: callee address
	SUB	#PUF0S-14, R4		; offset
	ADD	SP, R4
	MOV	(R4)+, 4(SP)
	MOV	(R4)+, 6(SP)
	MOV	(R4)+, 10(SP)
	MOV	(R4)+, 12(SP)
	POP	R4
	RETURN

; Equivalent of	FPUSH off(SP)
.MACRO	.PUFSP	off
	JSR	R4, @#PUF0S + off
.ENDM

; call these with	JSR	R4, POFSP
POFSP:	PUSH	R5
	MOV	(R4)+, R5
	ADD	SP, R5
	MOV	4(SP), (R5)+
	MOV	6(SP), (R5)+
	MOV	10(SP), (R5)+
	MOV	12(SP), (R5)+
	POP	R5
	MOV	(SP), 10(SP)	; saved R4
	ADD	#10, SP
	RTS	R4


.MACRO	.POFSP	off
	JSR	R4, POFSP
	.WORD	off + 6
.ENDM



; RETURN B AS THE RESULT BECAUSE A IS ZERO OR THERE ABOUTS
RETB2:
	MOV	(R5), R3		; GET SIGNIFICAND
	SWAB	R1		; MOVE EXPONENT TO UPPER HALF
	ASLB	R3		; KILL HIDDEN BIT
	BIS	R1, R3		; ADD EXPONENT TO SIGNIFICAND
	ASL	(SP)+		; PICK UP SIGN IN CARRY
	ROR	R3		; GENERATE FINAL RESULT
	MOV	R3, (R5)		; AND STORE BACK ORIGINAL WORD
RETB1:
	TST	(SP)+		; KILL A SIGN ON STACK
;; POINT PAST B4 (A1)
;; POINT PAST A4
	MOV	#5, R0
1$:	MOV	-(R4), 10(R4)	; COPY B4 TO A4, B3 TO A3, B2 TO A2, B1 TO A1, RETURN ADDRESS TO B4
	SOB	R0, 1$
; POP REGISTERS FROM STACK AND RETURN WITH RESULT ON THE STACK
$RETAB::
	CALL	PO0..4
	POP	R5
	ADD	#12, SP		; POINT TO MOVED RETURN ADDRESS (B4)
	JMP	@(R4)+		; RETURN TO CALLER



; THESE ROUTINES PERFORM SOFTWARE SIMULATION OF DOUBLE PRECISION
; ADDITION AND SUBTRACTION OF THE FORM A (+-) B.  WHEN EVERYTHING
; IS SET UP (AT ABOUT 20$) THE STACK LOOKS LIKE THE FOLLOWING:
; (LOW MEMORY TO HIGH)
;
; SP==> POINTER TO SIGNIFICAND NOT IN R1-R4
;	RESULT EXPONENT
;	RESULT SIGN
;	B SIGN
;	A SIGN
;	R0
;	R1
;	R2
;	R3	(REGISTERS SAVED AT CALL)
;	R4
;	R5
;	RETURN TO CALLER
;	B1
;	B2
;	B3	(B SIGNIFICAND)
;	B4
;	A1
;	A2
;	A3	(A SIGNIFICAND)
;	A4
;
; ALL REGISTERS ARE PRESERVED.  DURING EXECUTION, THE SIGNIFICAND THAT
; NEEDS ALIGNMENT (BY SHIFTING RIGHT) IS HELD IN R1 (MSB) THRU R4 (LSB).
;
; WHEN FADD OR FSUB IS CALLED, THE ARGUMENTS HAVE ALREADY BEEN PLACED
; ON THE STACK AND THE STACK POINTER POINTS TO THE RETURN ADDRESS IN
; THE ABOVE STACK DESCRIPTION.
;
;
; FLOATING SUBTRACTION
;
$DSUB::				; SIMULATE REAL SUBTRACTION
	TST	(SP)		; ZERO?
	BEQ	$DADD		; YES, SO IT CAN'T BE NEGATED
	ADD	#100000, (SP)	; NEGATE SIGN OF B ON STACK
				; FALL THROUGH AND DO AN ADDITION (A-B=A+(-B))
; FLOATING ADDITION
$DADD::              		; SIMULATE REAL ADDITION
	TST	-(SP)
	JSR	R5, SAV5.0
; FIND SIGN AND EXPONENT FOR A
	CLR	-(SP)		; MAKE ROOM FOR SIGN
	MOV	SP, R4		; COPY STACK POINTER
	ADD	#30, R4		; POINT TO A1
	MOV	(R4), R0		; GET SIGN, EXPONENT, MSB OF SIGNIFICAND
	BEQ	RETB1		; A IS ZERO SO ANSWER IS B
	ASL	R0		; SIGN INTO CARRY, EXPONENT IN UPPER BYTE
	ROR	(SP)		; RECORD SIGN (IN CARRY)
	CLRB	R0		; CLEAR SIGNIFICAND BITS
	SWAB	R0		; EXPONENT IN LOW ORDER BYTE
	BISB	#200, (R4)	; SET HIDDEN BIT IS MSB OF SIGNIFICAND
	CLRB	1(R4)		; CLEAR SIGN AND EXPONENT
; FIND SIGN AND EXPONENT FOR B
	CLR	-(SP)		; MAKE ROOM FOR SIGN
	MOV	SP, R5		; COPY STACK POINTER
	ADD	#22, R5		; POINT TO B1
	MOV	(R5), R1	; GET SIGN, EXPONENT, MSB OF SIGNIFICAND
	BEQ	RETA1		; B IS ZERO SO ANSWER IS A
	ASL	R1		; SIGN INTO CARRY, EXPONENT IN UPPER BYTE
	ROR	(SP)		; RECORD SIGN
	CLRB	R1		; CLEAR SIGNIFICAND BITS
	SWAB	R1		; EXPONENT IN LOW ORDER BYTE
	BISB	#200, (R5)	; SET HIDDEN BIT IN MSB OF SIGNIFICAND
	CLRB	1(R5)		; CLEAR SIGN AND EXPONENT
;
; DETERMINE WHICH SIGNIFICAND NEEDS TO BE SHIFTED RIGHT TO ALIGN DECIMAL POINTS
;
	MOV	R0, R2		; COPY A'S EXPONENT
	SUB	R1, R0		; R0=AEXP-BEXP
	BPL	10$		; SHIFT B RIGHT
	NEG	R0		; MAKE EXPONENT DIFFERENCE POSITIVE
	CMP	#56., R0		; ARE EXPONENTS TOO DIFFERENT?
	BLT	RETB2		; YES, SO JUST RETURN B


	MOV	(SP), -(SP)	; RESULT SIGN=SIGN OF B
	PUSH	R1,		; RESULT EXPONENT=EXPONENT OF B
	ADD	#6, R5		; POINT TO END OF B (B4)
	PUSH	R5		; SAVE POINTER
	MOV	R4, R5		; POINT TO A1
	BR	DAD20		; ALIGN A
; SHIFT B RIGHT
10$:
	CMP	#56., R0		; EXPONENTS TOO DIFFERENT?
	BGE	DAD12
					; YES, SO RETURN A

; RETURN A AS THE RESULT BECAUSE B IS ZERO (OR SUFFICIENTLY CLOSE)
RETA2:
	MOV	R2, R0		; GET EXPONENT OF A
RETA1:
	MOV	(R4), R3		; GET SIGNIFICAND
	TST	(SP)+		; KILL B SIGN ON STACK
	SWAB	R0		; MOVE EXPONENT TO UPPER HALF
	ASLB	R3		; KILL HIDDEN BIT IN SIGNIFICAND
	BIS	R0, R3		; SET EXPONENT IN WITH SIGNIFICAND
	ASL	(SP)+		; PICK UP SIGN IN CARRY
	ROR	R3		; CREATE FINAL RESULT
	MOV	R3, (R4)	; STORE ORIGINAL WORD IN A1
RETA3:	MOV	14(SP), 24(SP)	; MOVE RETURN ADDRESS INTO PLACE
	BR	$RETAB		; AND EXIT


DAD12:	MOV	2(SP), -(SP)	; RESULT SIGN=SIGN OF A
	PUSH	R2		; RESULT EXPONENT=EXPONENT OF A
	ADD	#6, R4		; POINT TO END OF A (A4)
	PUSH	R4		; SAVE POINTER TO OTHER SIGNIFICAND
;
; R5 NOW POINTS TO SIGNIFICAND TO SHIFT, SO THE SIGNIFICAND BEING SHIFTED
; RIGHT IS PICKED UP AND PLACED INTO R1-R4.  IN ORDER TO OBTAIN TWO
; "GUARD" BITS, THE OTHER SIGNIFICAND IS SHIFTED LEFT AT MOST THRICE.
; THERE IS PLENTY OF ROOM IN THE BYTE OCCUPIED BY THE EXPONENT, AND
; THE NORMALIZE ROUTINE WILL SHIFT THE RESULT RIGHT IF NECESSARY.
;
DAD20:
	TST	R0		; ANY SHIFTING NECESSARY
	BEQ	25$		; NOPE
	MOV	#3, R1		; SHIFT A MAX OF 3 TIMES
	CMP	R1, R0		; LESS THAT 3 REQUIRED?
	BLE	22$		; NOPE
	MOV	R0, R1		; YES, SO USE EXACT COUNT
22$:
	MOV	(SP), R4		; POINT TO LARGER NUMBER
	ASL	(R4)		; SHIFT LEFT
	ROL	-(R4)
	ROL	-(R4)
	ROL	-(R4)
	DEC	2(SP)		; DECREMENT RESULT EXPONENT
	DEC	R0		; COUNT SHIFT FOR LATER TOO
	SOB	R1, 22$		; COUNT THE SHIFT AND LOOP
25$:
	MOV	(R5)+, R1	; PICK UP MSB
	MOV	(R5)+, R2
	MOV	(R5)+, R3
	MOV	(R5)+, R4	; LSB
40$:	TST	R0		; ANYTHING LEFT TO DO?
	BEQ	50$		; NOPE, OPERANDS ARE ALIGNED
45$:	CALL	CRO123		; SHIFT IN A ZERO MSB
	ROR	R4		; LSB
	SOB	R0, 45$		; COUNT THE SHIFT AND CONTINUE
; DETERMINE HOW TO DO ADDITION.  IF SIGNS OF A AND B ARE THE SAME THEN
; DO ADDITION.  IF SIGNS DIFFER THEN DO SUBTRACTION AND POSSIBLY FIX
; UP THE SIGN (A-B=-(B-A)).
50$:
	POP	R5		; POINT TO LARGER SIGNIFICAND
	CMP	4(SP), 6(SP)	; COMPARE SIGNS OF B AND A
	BNE	DAD70		; ITS SOME KIND OF SUBTRACTION
; SIGNS ARE THE SAME SO WE CAN DO AN ADDITION ((-A)+(-B)=-(A+B))
	CALL	ADC32F		; ADD LSB'S RIPPLE CARRY
	BR	DAD100		; GET EXPONENT AND NORMAIZE RESULT

$ZERO::
	CLR	-(SP)		; RESULT SIGN
				; RESULT IS ZERO
$FLZER::
	CALL	CL0123		; UNDERFLOW MAKES NUMBER ZERO
	CLR	R4
	CLR	(SP)		; POSITIVE ZERO!
FNORM2:
	ASLB	R1		; KILL HIDDEN BIT
	BIS	R0, R1		; ADD EXPONENT TO UPPER BYTE
	ASL	(SP)+		; GET SIGN OF RESULT IN CARRY
	ROR	R1		; AND PRESTO!  WE HAVE FLOATING RESULT
; STORE RESULT INTO STACK ON TOP OF A AND EXIT TO RETA2 WHICH CLEARS
; STACK, RESTORES REGISTERS, AND LEAVES THE RESULT ON THE STACK.
	MOV	SP, R5		; COPY STACK POINTER
	ADD	#32, R5		; POINT TO RESULT SLOT (A1)
	MOV	R1, (R5)+	; COPY SIGN, EXPONENT, MSB
	MOV	R2, (R5)+
	MOV	R3, (R5)+
	MOV	R4, (R5)+	; LSB
	CMP	(SP)+, (SP)+	; SP += 4. KILL SIGNS ON STACK
	BR	RETA3


; OPERAND SIGNS DIFFER, SO DO A SUBTRACTION
DAD70:
	COM	2(SP)		; FIX UP RESULT'S SIGN
	CALL	SBC4.1
	BPL	DAD100		; RESULT IS POSITIVE!!
	COM	R1		; DO 4 WORD NEGATE
	COM	R2		; REMEMBER THE CIA
	COM	R3		; INSTRUCTION ON 8'S?
	COM	R4
	ADD	#1, R4		; ADD ONE TO LSB (NOT 'INC R4' to affect CF)
	CALL	ADC321		; RIPPLE CARRY
	COM	2(SP)		; SET CORRECT RESULT SIGN
DAD100:
;	BR	$FNORM		; NORMALIZE RESULT AND EXIT
; fallback
;.SBTTL	$FNORM	FLOATING NORMALIZE
;
; THIS ROUTINE NORMALIZES THE FLOATING POINT SIGNIFICAND IN R1-R4.
; THE STATE OF THE STACK IS SIMILAR TO THAT DESCRIBED ABOVE FOR
; FLOATING ADDITION, EXCEPT THAT THE RESULT EXPONENT IS ON TOP
; OF THE STACK.
;
$FNORM::
	POP	R0	; GET RESULT EXPONENT
FNORM7:
	CLC			; PREVENT ROUNDING PROBLEMS
1$:
	BIT	#177400, R1	; RIGHT SHIFT REQUIRED?
	BEQ	5$		; NOPE
	CALL	CRO123		; SHIFT IN A ZERO, MSB
	ROR	R4		; LSB (LOW BIT FALLS INTO BIT BUCKET)
	INC	R0		; COUNT THE SHIFT
	BR	1$		; CONTINUE MAX 8 TIMES
5$:
	BCC	FNORM8		; NO ROUNDING
	ADC	R4		; ADD IN ROUNDING BIT
	PUSH	#1$		; RIPPLE CARRY MAY REQUIRE ONE MORE SHIFT, SO CHECK AGAIN
ADC321:	ADC	R3
	ADC	R2
	ADC	R1
	RETURN

FNORM8:	TSTB	R1		; IS NUMBER NORMALIZED?
	BMI	FNORM1		; YES!
	BNE	20$		; NOPE, BUT SOME BITS ARE SET LOW BYTE
	PUSH	R1	; CHECK IF RESULT IS ZERO
	BIS	R2, (SP)
	BIS	R3, (SP)		; BY OR'ING TOGETHER
	BIS	R4, (SP)+	; ALL THE WORDS
	BNE	10$		; NOPE
	CLR	R0		; YES, SO EXPONENT IS ZERO
	CLR	(SP)		; AS IS THE SIGN
	BR	FNORM1		; TAKE FAST EXIT
; NO BITS SET IN FIRST 8 BITS OF MSB OF SIGNIFICAND, SO PERFORM
; FAST LEFT BITE-SIZED SHIFT.
10$:
20$:
	DEC	R0		; COUNT ONE SHIFT
	ASL	R4		; SHIFT LEFT LSB
	ROL	R3		; SHIFT 4 WORDS
	ROL	R2
	ROLB	R1		; SHIFT LEFT MSB
	BPL	20$		; NOT YET NORMALIZED
; SIGNIFICAND IS NORMALIZED. ADD SIGN AND EXPONENT AND CHECK FOR
; OVERFLOW/UNDERFLOW.  THIS ENTRY POINT IS ALSO A FAST EXIT
; FROM THE ADD/SUBTRACT ROUTINE.
FNORM1::
	SWAB	R0		; PUT EXPONENT IN UPPER BYTE
	TSTB	R0		; ANY GARBAGE LEFT FROM EXPONENT?
	BEQ	FNORM2
			; YES!?! OVERFLOW/UNDERFLOW
FLTERR:
	ER.MATH

; SIMULATES DOUBLE PRECISION FLOATING POINT DIVISION.  THE
; TWO OPERANDS ARE ON THE STACK WHEN THIS ROUTINE IS CALLED. THE
; DIVISION PERFORMED IS A/B.  THE STATE OF THE STACK AT ABOUT 10$
; IS AS FOLLOWS:
;
; SP==> RESULT EXPONENT
;	RESULT SIGN
;	SHIFT COUNT
;	UNUSED	(REQUIRED FOR NORMALIZE)
;	R0
;	R1
;	R2
;	R3	(REGISTERS SAVED AT CALL)
;	R4
;	R5
;	RETURN TO CALLER
;	B1
;	B2
;	B3	(B SIGNIFICAND)
;	B4
;	A1
;	A2
;	A3	(A SIGNIFICAND)
;	A4
;
; ALL REGISTERS ARE PRESERVED.  DURING CALCULATIONS, R1-R4 HOLD THE
; CURRENT REMAINDER OF THE DIVISION.  THE QUOTIENT IS BUILT BIT BY
; BIT AT THE LOCATION POINTED TO BY R0.  THIS ALGORITHM USES A NON-
; RESTORING NORMAILZED DIVISION METHOD DESCRIBED IN THE FP11-B
; HARDWARE MANUAL.  R5 POINTS TO THE SIGNIFICAND OF THE DIVISOR.
;
;
; ENTRY POINT FOR DIVISION
				; SIMULATE REAL DIVISION
$DDIV::				; SAVE ALL REGISTERS
	TST	-(SP)
	JSR	R5, SAV5.0
	CLR	-(SP)		; UNUSED
	PUSH	#58.		; SHIFT COUNT (WITH 2 GUARD BITS)
	MOV	#200, R3	; USEFUL CONSTANT
;
; PICK UP SIGN AND EXPONENT OF B (THE DIVISOR)
;
	CLR	R2		; USED TO BUILD RESULT SIGN
	MOV	SP, R5		; COPY STACK POINTER
	ADD	#22, R5		; POINT TO MSB OF B
	MOV	(R5), R4	; GET SIGN, EXPONENT, MSB
	BNE	1$
	.E.DIVZ			; DIVISION BY ZERO!?!;  !!!O BR ZERDIV
1$:	ASL	R4		; SIGN INTO CARRY, ALIGN EXPONENT
	ADC	R2		; SIGN IN LSB OF R2
	CLRB	R4		; ZAP SIGNIFICAND BITS
	SWAB	R4		; EXPONENT TO LOWER BYTE
	NEG	R4		; REXP=(AEXP+200)-(BEXP+200)
	ADD	R3, R4		;     +200=(AEXP-BEXP)+200
	DEC	R4		; MINUS ONE FOR GUARD BITS
	BISB	R3, (R5)		; SET HIDDEN BIT
	CLRB	1(R5)		; KILL SIGN AND EXPONENT
; PICK UP SIGN AND EXPONENT OF A (THE DIVIDEND)
	MOV	SP, R0		; COPY STACK POINTER
	ADD	#32, R0		; POINT TO A SIGNIFICAND
	MOV	(R0), R1	; GET SIGN, EXPONENT, MSB
ZERO3:	BEQ	$ZERO		; RETURN ZERO AS A RESULT
	ASL	R1		; ALIGN EXPONENT
	ADC	R2		; RESULT SIGN IN LSB
	ROR	R2		; RESULT SIGN TO CARRY
	ROR	R2		; RESULT SIGN IN SIGN BIT
	PUSH	R2		; STORE RESULT SIGN
	CLRB	R1		; ZAP SIGNIFICAND BITS
	SWAB	R1		; EXPONENT TO LOWER BYTE
	ADD	R1, R4		; CALCULATE RESULT EXPONENT
	PUSH	R4		; STORE RESULT EXPONENT
	BISB	R3, (R0)	; SET HIDDEN BIT
	CLRB	1(R0)		; KILL SIGN AND EXPONENT
;
; PICK UP DIVIDEND IN R1-R4 AND AT THE SAME TIME ZERO
; THE SLOT WHERE THE RESULT IS BUILT
;
	MOV	(R0), R1	; MSB
	CLR	(R0)+
	MOV	(R0), R2	; SECOND WORD
	CLR	(R0)+
	MOV	(R0), R3	; THIRD WORD
	CLR	(R0)+
	MOV	(R0), R4	; LAST WORD
	CLR	(R0)		; R0 POINTS TO LAST WORD OF RESULT
;
; SUBTRACT DIVISOR FROM REMAINDER
;
DDIV20:	ADD	#6, R5		; POINT TO LSB OF DIVISOR
	CALL	SBC4.1
; COMPLEMENT SIGN OF REMAINDER SO THAT IT SHIFTS LEFT INTO LSB
; OF RESULT.
DDIV30:
	ADD	#100000, R1	; FLIP SIGN BIT
; SHIFT 8 WORDS LEFT.  MSB OF REMAINDER BECOMES LSB OF QUOTIENT
DDIV40:	CALL	ASL1.4
	ROL	(R0)		; SIGN BIT INTO LSB OF QUOTIENT
	ROL	-(R0)		; THIRD QUOTIENT WORD
	ROL	-(R0)		; SECOND QUOTIENT WORD
	ROL	-(R0)		; FIRST QUOTIENT WORD
	ADD	#6, R0		; POINT TO LAST QUOTIENT WORD
	DEC	4(SP)		; COUNT THIS SHIFT
	BNE	DDIV45
			; DONE!
; DONE WITH DIVISION.  PUT RESULT INTO R1-R4
; AND NORMAILZE RESULT.
	MOV	(R0), R4		; LSB OF RESULT
	MOV	-(R0), R3	; THIRD WORD
	MOV	-(R0), R2	; SECOND WORD
	MOV	-(R0), R1	; MSB
FNORM5:	BR	$FNORM		; NORMALIZE RESULT

DDIV45:
; NORMALIZE REMAINDER
	TST	R1		; REMAINDER POSITIVE?
	BMI	50$		; NEGATIVE, SPECIAL NORMALIZE
	TSTB	R1		; NORMALIZED?
	BMI	DDIV20		; YES!
	BR	DDIV40		; SHIFT LEFT TO NORMALIZE
50$:
	TSTB	R1		; NORMALIZED?
	BMI	DDIV40		; SHIFT LEFT TO NORMALIZE
;
; MAIN DIVISION LOOP.  IF REMAINDER IS POSITIVE THEN DIVISOR IS SUBTRACTED
; BUT IF REMAINDER IS NEGATIVE THEN DIVISOR IS ADDED TO CORRECT FOR THE
; PROBLEM IN THE PREVIOUS LOOP.  AFTER THE ARITHMETIC OPERATION, THE
; REMAINDER IS NORMALIZED IF NECESSARY, AND THE LOOP CONTINUES.
;
10$:	PUSH	#DDIV30   		; POINT TO LSB OF DIVISOR, MULTIPLE WORD ADD, RIPPLE CARRY
ADC32E:	ADD	#6, R5		; POINT TO LSB OF MULTIPLIER
ADC32F:	ADD	(R5), R4	; LSB
	CALL	ADC321		; POINT TO LSB OF MULTIPLIER, LSB, RIPPLE CARRY
	ADD	-(R5), R3	; THIRD WORD
	ADC	R2
	ADC	R1
	ADD	-(R5), R2	; SECOND WORD
	ADC	R1
	ADD	-(R5), R1	; FIRST WORD (MSB) NO CARRY!
	RETURN


; THIS ROUTINE SIMULATES A DOUBLE PRECISION FLOATING POINT MULTIPLY.
; THE TWO OPERANDS ARE ON THE STACK WHEN THIS ROUTINE IS CALLED. THE
; STATE OF THE STACK (AT ABOUT 10$) IS AS FOLLOWS:
;
; SP==> RESULT EXPONENT
;	RESULT SIGN
;	UNUSED (REQUIRED FOR NORMALIZE)
;	UNUSED (REQUIRED FOR NORMALIZE)
;	R0
;	R1
;	R2
;	R3	(REGISTERS SAVE AT CALL)
;	R4
;	R5
;	RETURN TO CALLER
;	B1
;	B2
;	B3	(B SIGNIFICAND)
;	B4
;	A1
;	A2
;	A3
;	A4
;
; ALL REGISTERS ARE PRESERVED.  DURING CALCULATIONS, THE RESULT IS BUILT
; IN R1-R4.  R0 POINTS TO THE MULTIPLIER (OR MULTIPLICAND, I CAN NEVER
; REMEMBER WHICH IS WHICH) WHICH IS ARRANGED TO BE THE SIGNIFICAND WITH
; THE FEWEST NUMBER OF SIGNIFICANT BITS.  R5 POINTS TO THE OTHER OPERAND
; WHICH GETS SHIFTED RIGHT AS THE MULTIPLICATION IS PERFORMED.
;
; SPECIAL CASE HANDLING OF MULTIPLY BY ZERO
;


; ENTRY POINT FOR MULTIPLY
$DMUL::				; SIMULATE DOUBLE REAL MULTIPLICATION
	TST	-(SP)
	JSR	R5, SAV5.0
	CMP	-(SP), -(SP)	; SKIP TWO WORDS (USED BY ADD/SUB)
	MOV	#200, R3		; USEFUL CONSTANT

; GET SIGN AND EXPONENT OF A
	CLR	R2		; HOLDS RESULT SIGN
	MOV	SP, R4		; COPY STACK POINTER
	ADD	#32, R4		; POINT TO A MSB
	MOV	(R4), R0	; PICK UP SIGN, EXPONENT, MSB OF SIGNIFICAND
	BEQ	ZERO3		; ZERO TIMES ANYTHING IS ZERO!
	ASL	R0		; SHIFT SIGN INTO CARRY, BYTE ALIGN EXPONENT
	ADC	R2		; SIGN TO LOW ORDER BIT
	CLRB	R0		; CLEAR MSB OF SIGNIFICAND
	SWAB	R0		; MOVE EXPONENT TO LOW BYTE
	BISB	R3, (R4)	; SET HIDDEN BIT IN SIGNIFICAND
	CLRB	1(R4)		; CLEAR SIGN/EXPONENT
; GET SIGN AND EXPONENT OF B
	MOV	SP, R5		; COPY STACK POINTER
	ADD	#22, R5		; POINT TO SIGNIFICAND OF B
	MOV	(R5), R1		; GET SIGN, EXPONENT AND MSB OF SIGNIFICAND
	BEQ	ZERO3		; RESULT IS ZERO!
	ASL	R1		; SIGN INTO CARRY, BYTE ALIGN EXPONENT
	ADC	R2		; XOR OF SIGNS IN LOW BIT
	ROR	R2		; LOW BIT INTO CARRY
	ROR	R2		; AND BACK INTO SIGN BIT
	PUSH	R2		; PUT RESULT SIGN ON STACK
	CLRB	R1		; CLEAR SIGNIFICAND PART
	SWAB	R1		; EXPONENT TO LOW BYTE
	BISB	R3, (R5)	; SET HIDDEN BIT
	CLRB	1(R5)		; CLEAR SIGN AND EXPONENT
; CALCULATE RESULT EXPONENT
	ADD	R1, R0		; RESULT EXPONENT IS SUM OF OPERAND EXPONENTS
	SUB	R3, R0		; (AEXP+200)+(BEXP+200)-200=(AEXP+BEXP)+200
	PUSH	R0		; SAVE RESULT EXPONENT (OVERFLOW CHECKED LATER)
.IF	NDF	TINYFP
; DETERMINE WHICH OPERAND HAS THE FEWEST NUMBER OF SIGNIFICANT BITS
	CLR	R0		; COUNT FOR A
	MOV	R4, R2		; COPY POINTER TO A'S SIGNIFICAND
	ADD	#10, R2		; POINT PAST END OF A
10$:
	INC	R0		; COUNT A WORD
	TST	-(R2)		; ANY BITS SET?
	BEQ	10$		; NO, SO COUNT ANOTHER WORD
	CLR	R1		; COUNT FOR B
	MOV	R5, R3		; POINT TO B
	ADD	#10, R3		; POINT PAST END OF B
20$:
	INC	R1		; COUNT A WORD
	TST	-(R3)		; ANY BITS SET?
	BEQ	20$		; NO, SO COUNT ONE MORE WORD
	CMP	R0, R1		; WHICH HAS FEWER BITS
	BGE	30$		; A DOES
	MOV	R5, R0		; B DOES, SO USE B FOR LEFT SHIFTING
	MOV	R4, R5		; USE A FOR RIGHT SHIFTING
	BR	40$		; AND START CALCULATING


.IFF
	MOV	#56., 4(SP)	; SHIFT COUNT FOR TINY MULTIPLY
.ENDC
30$:
	MOV	R4, R0		; COPY POINTER TO A
40$:
	MOV	#3, R1		; GET COUNT (3 GUARD BITS)
45$:
	ADD	#6, R5		; POINT TO END OF MULTIPLIER
;	CLC			; SHIFT IN A ZERO
	ROL	(R5)		; ROTATE LEFT
	ROL	-(R5)		; TO INCREASE
	ROL	-(R5)		; ACCURACY IN CASE WE
	ROL	-(R5)		; ROUND DURING NORMALIZATION
	DEC	(SP)		; ADJUST EXPONENT
	SOB	R1, 45$		; COUNT THIS SHIFT KEEP SHIFTING TO INTRODUCE GUARD BITS
; CLEAR RESULT (R1-R4)
;
; MAIN MULTIPLY LOOP.  AS THE SIGNIFICAND POINTED TO BY R0 IS SHIFTED
; RIGHT, TESTS ARE MADE FOR POSSIBLE BYTE OR WORD SHIFTS.  SINCE
; WE PICKED THE SHORTER SIGNIFICAND TO SHIFT RIGHT, THIS OPTIMIZATION
; MINIMIZES ITERATIONS THROUGH THIS LOOP.
;
; RESULT IS BUILT IN THESE REGISTERS
	CLR	R4
47$:	CLR	R3
48$:	CLR	R2
49$:	CLR	R1
50$:
.IF	NDF	TINYFP
	TSTB	6(R0)		; LAST BYTE ZERO?
	BNE	110$		; NOPE, SO DO SLOW MULTIPLY
	TSTB	7(R0)		; NEXT TO LAST BYTE ALSO ZERO?
	BNE	100$		; NOPE, DO BYTE SHIFT
	TST	4(R0)		; THIRD WORD ZERO?
	BNE	70$		; NO, SO DO WORD SHIFT
	TST	2(R0)		; SECOND WORD ZERO?
	BNE	60$		; NOPE, SO DO DOUBLE WORD SHIFT
	TST	(R0)		; FIRST WORD ZERO?
	BEQ	130$		; YES! ALL DONE!
; DO A THREE WORD RIGHT SHIFT OF MULTIPLIER AND RESULT
	MOV	(R0), 6(R0)	; FIRST WORD TO LAST WORD
	CLR	(R0)		; ZEROS SHIFTED INTO FIRST WORD
	MOV	R1, R4		; FIRST WORD TO LAST WORD
				; ZERO SHIFTED INTO
				; FIRST THREE
				; WORDS
	BR	47$		; CONTINUE
; DO A TWO WORD RIGHT SHIFT
60$:
	MOV	2(R0), 6(R0)	; SECOND WORD TO LAST WORD
	MOV	(R0), 4(R0)	; FIRST WORD TO THIRD WORD
	CLR	2(R0)		; ZEROES SHIFTED INTO FIRST
	CLR	(R0)		; TWO WORDS
	MOV	R2, R4		; SECOND WORD TO LAST WORD
	MOV	R1, R3		; FIRST WORD TO THIRD WORD
				; ZEROES SHIFTED INTO FIRST
				; TWO WORDS
	BR	48$		; CONTINUE
; SHIFT RIGHT ONE WORD
70$:
	MOV	4(R0), 6(R0)	; THIRD WORD TO LAST WORD
	MOV	2(R0), 4(R0)	; SECOND WORD TO THIRD WORD
	MOV	(R0)+, (R0)	; FIRST WORD TO SECOND WORD
	CLR	-(R0)		; ZEROES SHIFTED INTO FIRST WORD
	MOV	R3, R4		; THIRD WORD TO LAST WORD
	MOV	R2, R3		; SECOND WORD TO THIRD WORD
	MOV	R1, R2		; FIRST WORD TO SECOND WORD
				; ZEROES SHIFTED INTO FIRST WORD
	BR	49$		; CONTINUE

;
; SHIFT RIGHT ONE BYTE.  IT LOOKS UGLY, BUT IT'S MUCH FASTER THAN
; BIT SHIFTING!
;
100$:
	MOVB	4(R0), 6(R0)	; BYTE 4 TO BYTE 6
	SWAB	6(R0)		; 4/7 IN LAST WORD
	MOVB	2(R0), 4(R0)	; BYTE 2 TO BYTE 4
	SWAB	4(R0)		; 2/5 IN THIRD WORD
	MOVB	(R0), 2(R0)	; BYTE 0 TO BYTE 2
	SWAB	2(R0)		; 0/3 IN SECOND WORD
	CLRB	(R0)		; ZEROES SHIFTED INTO LOW BYTE
	SWAB	(R0)		; X/1 IN FIRST WORD
	CLRB	R4		; CLEAR BYTE 6
	BISB	R3, R4		; BYTE 4 TO BYTE 6
	SWAB	R4		; 4/7 IN LAST WORD
	CLRB	R3		; CLEAR BYTE 4
	BISB	R2, R3		; BYTE 2 TO BYTE 4
	SWAB	R3		; 2/5 IN THIRD WORD
	CLRB	R2		; CLEAR BYTE 2
	BISB	R1, R2		; BYTE 0 TO BYTE 2
	SWAB	R2		; 0/3 IN SECOND WORD
	CLRB	R1		; ZEROES SHIFTED INTO LOW BYTE
	SWAB	R1		; X/1 IN FIRST WORD
.IFF
	CLC			; NEEDED BECAUSE NO TSTB 6(R0) WAS DONE
.ENDC
; LAST BYTE CONTAINS INTERESTING BITS.  ROTATE MULTIPLICAND RIGHT AND
; CHECK IF ADDITION SHOULD BE DONE ON THIS PASS.
110$:
	ROR	(R0)+		; CARRY CLEARED BY TSTB AT 50$!
	ROR	(R0)+		; MULTIPLE WORD SHIFT
	ROR	(R0)+
	ROR	(R0)		; LSB
	BCC	120$		; ADDITION NOT NEEDED ON THIS PASS
	CALL	ADC32E		; POINT TO LSB OF MULTIPLIER, LSB, RIPPLE CARRY
; SHIFT RESULT RIGHT ONE BIT
120$:
	SUB	#6, R0		; POINT BACK TO START OF MULTIPLICAND
	CALL	CRO123		; ZERO SHIFTED INTO MSB OF RESULT, MULTIPLE WORD SHIFT
	ROR	R4		; LSB FALLS INTO BIT BUCKET
.IF	NDF	TINYFP
	BR	50$		; CONTINUE
.IFF
	DEC	4(SP)		; COUNT THIS LOOP
	BGT	50$		; MORE TO DO
.ENDC
;
; DONE MULTIPLYING.  PICK UP EXPONENT AND JUMP TO THE
; NORMALIZE ROUTINE WHICH CLEANS UP THE STACK AND RETURNS
; TO THE CALLER.
;
130$:
FNORMA:	BR	FNORM5		; NORMALIZE RESULT IN R1-R4


ZERDIV:
	.E.DIVZ


$CBIN:	CLR	R5
2$:	MOVB	(R3), R4
	SUB	#'0, R4
	ROR	R4
	BNE	C1
	INC	R3
	ROL	R5
	BCC	2$
OVF:
C3:	ER.MATH


$COCT:	CLR	R5
5$:	MOVB	(R3)+, R4
	SUB	#'0, R4
	BIT	#177770, R4
	BNE	C1
	ASL	R5
	BCS	C3
	ROL	R5
	BCS	C3
	ROL	R5
	BCS	C3
	BIS	R4, R5
	BR	5$


$DAL:	CLR	R0
	MOV	R1, R3
	BPL	1$
	NEG	R3
	BMI	OVF
	INC	R0
1$:	TST	R2
	BEQ	ZERDIV
	BPL	3$
	NEG	R2
	BIS	#2, R0
3$:	MOV	R2, R5
	CLR	R1
5$:	CMP	R2, R3
	BHI	4$
	ASL	R2
	BR	5$

2$:	CLC
	ROR	R2
	ASL	R1
	CMP	R2, R3
	BHI	4$
	SUB	R2, R3
	INC	R1
4$:	CMP	R2, R5
	BNE	2$
6$:	ASR	R0
	BCC	7$
	NEG	R1
	NEG	R3
7$:	TST	R0
	BEQ	8$
	NEG	R1
8$:
C1:	RETURN


;
;
; CALCULATES DOUBLE PRECISION POWER OF E.  THE RATIONAL APPROXIMATION
; REQUIRES A RANGE REDUCTION TO 0<=X<1/64.  THE RESULT IS GOOD TO ABOUT
; 18 PLACES.
;
; THE STATE OF THE STACK AT ABOUT 20$ IS:
;
; SP==>	X1
;	X2
;	X3	(RANGE REDUCED ARG)
;	X4
;	SCALE COUNT
;	POWER OF 2
;	SIGN FLAG
;	R5
;	R4
;	R3
;	R2
;	R1	(SAVED AT CALL)
;	R0
;	RETURN
;	N1
;	N2
;	N3	(ARGUMENT)
;	N4
;
$EXP::				; DOUBLE PRECISION EXPONENTIAL
	TST	(SP)
	BMI	1$
	CMP	(SP), #41660
	BLO	2$
	BHI	3$
	CMP	2(SP), #7463
	BLO	2$
3$:	CLR	$SAVJMP
	ER.MATH
1$:	CMP	(SP), #141660
	BLOS	2$
	JSR	R4, PUSH4Z
	.POFSP	6
	JMP	@(R4)+
2$:	TST	-(SP)		; PRESERVE REGISTERS
	JSR	R5, SAV5.0
	JSR	R4, PUSH4Z		; SIGN FLAG, POWER OF 2 REDUCTION, SCALE COUNTER
	TST	(SP)+
;
; APPROXIMATION IS EXP(X)=2^(X/LN(2))
;
	.PUFSP	24		; PUSH ARG
	.BASN			; X=X*1/LN(2)
	.WORD	DPUFL, LN2IN+2	; LN(2)
	.WORD	$DMUL
	.WORD	$EXP1
; DO RANGE REDUCTION TO GET 0<=X<1/64
$EXP1:
	MOV	(SP), R1	;	 GET SIGN/EXPONENT/MSB OF SIGNIFICAND
	MOV	R1, 14(SP)	; SET SIGN FLAG
	BIC	#100000, R1	;TAKE ABSOLUTE VALUE
	CMP	#36777, R1	; (1/64)*.99999....
	BGE	20$		; ARG IS IN RANGE!
	CMP	#40177, R1	; IS X<=1.0
	BGE	10$		; YES, DO FAST RANGE REDUCTION
	.BASN			; GET FRACTION PART, INTEGER PART TO R5
	.WORD	$FRACT
	.WORD	5$
5$:	MOV	R5, 12(SP)	; SAVE INTEGER PART
	BIC	#100000, (SP)	; GET ABSOLUTE VALUE
	MOV	(SP), R1		; GET MSB OF FRACTION PART
10$:
	CMP	#36777, R1	; (1/64)*.99999.....
	BGE	20$		; X IS IN RANGE NOW
	SUB	#200, R1		; DIVIDE X BY 2
	INC	10(SP)		; COUNT DIVISION
	BR	10$		; KEEP REDUCING
20$:
	MOV	R1, (SP)		; SAVE MSB OF REDUCED ARG
	MOV	SP, R1		; COPY STACK POINTER
	ADD	#10, R1		; POINT PAST X
;
; Y=X*X
;
	.FPUM1			; X
	.FPUM1			; X
	.BASN
	.WORD	$DMUL
	.WORD	21$
	.WORD	$DMUL
	.WORD	$PUSH8
	.WORD	$DADD
	.WORD	$PU20$		; .PUFSP 20; JMP @(R4)+
	.WORD	$DMUL
	.WORD	24$
	.WORD	$DADD
	.WORD	$PU25$
	.WORD	$DADD
	.WORD	$PU26$
	.WORD	$DSUB
	.WORD	$PU27$
	.WORD	$DDIV
	.WORD	$PU28$
;
; P=X*(P1*Y+P0)
;
21$:	MOV	#EXPTAB, R1	; POINT TO COEFFICIENT TABLE
	.PUFSP	0		; Y
	BR	$PUSH8
;
; Q=Y+Q0
;
24$:	.PUFSP	10		; Y
$PUSH8:	.FPUM1			; P1 or P0 or Q0
	JMP	@(R4)+
$PU25$:	.PUFSP	0		; Q
$PU20$:	.PUFSP	20		; P or X
	JMP	@(R4)+

$PU26$:	.PUFSP	10		; Q
	.PUFSP	30		; P
	JMP	@(R4)+

;; FIX UP SIGN AND ANY RANGE REDUCTION
ATAN31:	TST	R1		; RANGE REDUCTION?
	BEQ	40$		; NOPE
	FPUSH	0(R1)		; PUSH CORRECTION FACTOR
	.BASN
	.WORD	$DADD
	.WORD	40$
40$:	TST	10(SP)		; SIGN CORRECT?
	BPL	50$		; YES
	ADD	#100000, (SP)	; NOPE FLIP SIGN
50$:	.POFSP	22		; PUT ANSWER IN RESULT SLOT
	TST	(SP)+		; KILL JUNK ON STACK
	PUSH	#R.JR4		; address of JMP @(R4)+
PO0..4:	POP	(PC)+
AE.RET:	.WORD	0		; return address
	CALL	PO0123
	POP	R4
	BNE	AE321X
	MOV	$SAVJMP, R4
	CLR	$SAVJMP
	BR	AE321X

AE3210:	POP	AE.RET
	CALL	AD3210
	ADD	(SP)+, R2
	ADC	R1
	ADC	R0
	ADD	(SP)+, R1
	ADC	R0
	ADD	(SP)+, R0
AE321X:	JMP	@AE.RET



;
; THIS ROUTINE CALCULATES THE DOUBLE PRECISION SQUARE ROOT OF THE
; ARGUMENT PASSED ON THE STACK.  THE ALGORITHM USED IS SQRT 0150
; FROM HART, "COMPUTER APPROXIMATIONS".
;
$SQRT::
	PUSH	R4, R0		; SAVE R0
;
; FIRST, GET EXPONENT OF ARGUMENT
;
	MOV	4(SP), R0	; GET MSB OF ARGUMENT
	BMI	SQRERR		; SQUARE ROOT OF A NEGATIVE NUMBER!
SQRTR:
	BEQ	30$		; SQR(0) IS ZERO!
	MOV	R4, $SAVJMP
	ASL	R0		; ALIGN EXPONENT
	CLRB	R0		; CLEAR MSB OF SIGNIFICAND
	SWAB	R0		; EXPONENT TO LOW BYTE
	SUB	#200, R0		; CONVERT FROM EXCESS 200 TO BINARY
	PUSH	R0		; SAVE BINARY EXPONENT
;
; REDUCE RANGE OF ARGUMENT (N) SO THAT 0.5 < N <= 1.0
;
	.PUFSP	6		; COPY ARGUMENT
	BIC	#^C177, (SP)	; CLEAR EXPONENT (AND SIGN)
	BIS	#^F0.5, (SP)	; REDUCE RANGE
; GENERATE INITIAL APPROXIMATION TO SQUARE ROOT (GOOD TO 2 PLACES)
	.BASN
	.WORD	DDUP		; N
	.WORD	DPUFL, SQRT1+2	; MULTIPLIER
	.WORD	$DMUL		; X=N*0.59016206
	.WORD	DPUFL, SQRT0+2	; ADDITIVE CONSTANT
	.WORD	$DADD		; X=N*0.59016206+0.41730759
	.WORD	5$
;
; NOW DO THREE NEWTON ITERATIONS WHICH GIVE 18.125 PLACES OF
; ACCURACY.
;
5$:	MOV	#3, R0		; LOOP COUNT
10$:
	.BASN
	.WORD	DOVER		; PUSH N
	.WORD	DOVER		; PUSH X
	.WORD	$DDIV		; N/X
	.WORD	$DADD		; N/X+X
	.WORD	15$
15$:	SUB	#200, (SP)	; X=(N/X+X)/2
	SOB	R0, 10$
;
; CALCULATE FINAL ROOT FROM RANGE REDUCED ROOT
;
	MOV	20(SP), R0	; GET ORIGINAL EXPONENT
	ASR	R0		; DIVIDE BY 2
	BCC	20$		; IS WAS EVEN POWER OF TWO
	.BASN
	.WORD	DPUFL, SQRT2+2	; SQR(2)
	.WORD	$DMUL		; X=X*SQR(2)
	.WORD	20$
20$:
	SWAB	R0		; MOVE EXPONENT TO UPPER BYTE
	CLRB	R0		; KILL JUNK IN LOW BYTE
	ROR	R0		; SHIFT EXPONENT TO FIT
	ADD	R0, (SP)	; MULTIPLY BY POWER OF TWO
	BPL	25$		; RESULT IS POSITIVE
	ADD	#100000, (SP)	; CORRECT BORROW FROM EXPONENT
25$:
	.POFSP	24		; PUT INTO RESULT SLOT
	ADD	#12, SP		; KILL JUNK ON STACK
30$:
	POP	R0
	BR	P4SAV		; POP R4; JMP RSAVJ



; IF ARG>0 THEN EXP=(Q+P)/(Q-P)
$PU30$:
	.BASN			; (Q+P)/(Q-P)
	.WORD	$DDIV
	.WORD	$PU40$
; IF ARG < 0 THEN EXP=(Q-P)/(Q+P)
$PU27$:	TST	64(SP)		; SIGN FLAG
	BPL	$PU30$		; ARG WAS POSITIVE
	.PUFSP	10		; Q+P
	JMP	@(R4)+
$PU28$:	.POFSP	6		; ADJUST STACK
$PU40$:	MOV	50(SP), R0	; GET SCALE FACTOR
	BEQ	60$		; NO SCALING NEEDED
;
; BECAUSE OF RANGE REDUCTION, EXP=EXP^(SCALE FACTOR)
;
50$:
	.BASN			; SQUARE RESULT
	.WORD	DDUP
	.WORD	$DMUL
	.WORD	51$
51$:	SOB	R0, 50$		; COUNT IT, MORE TO DO
; NOW MULTIPLY BY POWER OF 2
60$:	MOV	52(SP), R0	; GET POWER
	SWAB	R0		; MOVE TO UPPER BYTE
	TSTB	R0		; TOO BIG?
	BEQ	70$		; NOPE
	CLRB	R0		; CLEAR -1 IN LOW BYTE
70$:	ASR	R0		; SHIFT TO EXPONENT SLOT
	ADD	R0, (SP)	; MULTIPLY BY 2^POWER
	.POFSP	72		; PUT RESULT IN RESULT SLOT
	ADD	#46, SP		; KILL JUNK ON STACK
	CALL	PO0..4		; POP REGS
	POP	R5
	TST	(SP)+		;
	JMP	@(R4)+

;
; DOUBLE PRECISION SINE AND COSINE APPROXIMATIONS.  THIS ROUTINE USES
; ALGORITHM NUMBER 3345 IN HART, "COMPUTER APPROXIMATIONS".  THE ARGUEMENT
; IS REDUCED TO 0 <= X <= PI/2, AND THEN A SIMPLE POLYNOMIAL IS USED
; TO APPROXIMATE THE DESIRED FUNCTION.
;
; THE STATE OF THE STACK AT ABOUT 10$: IS AS FOLLOWS:
;
; SP==> A1
;	A2
;	A3	( WORK AREA )
;	A4
;	R5
;	R4
;	R3
;	R2	( REGISTERS SAVED AT CALL )
;	R1
;	R0
;	QUADRANT/SIGN FLAG
;	RETURN ADDRESS
;	N1
;	N2
;	N3	( ARGUMENT/RESULT )
;	N4
;
$COS::					; DOUBLE PRECISION COSINE
	TST	-(SP)
	CLR	-(SP)			; CLEAR QUADRANT FLAG
	ASL	4(SP)			; SIGN OF ARGUMENT INTO CARRY
	BEQ	ONE			; COS(0.0)=1.0
	SEC				; COS(X)=SIN(PI/2 + X)
	BR	SINCOS			; FALL INTO COMMON CODE

SQRERR:
BADLOG:
	.E.ARG


ONE:	MOV	#^F1.0, 4(SP)		; FAST RETURN FOR COS(0)
	BR	SCEXI			; THAT WAS EASY!
$SIN::					; DOUBLE PRECISION SINE
	TST	-(SP)
	CLR	-(SP)			; CLEAR QUADRANT/SIGN FLAG
	ASL	4(SP)			; SIGN OF ARGUMENT INTO CARRY
	BEQ	SCEXI			; SIN(0.0)=0.0
	ROL	(SP)			; SIGN FLAG TO LOW BIT OF QUADRANT FLAG
SINCOS:
	ROL	(SP)			; QUADRANT/SIGN NOW KNOWN. (ZERO INTO CARRY)
	ROR	4(SP)			; TAKE ABSOLUTE VALUE OF ARGUMENT
; SAVE ALL THE REGISTERS
	JSR	R5, SAV5.0
; REDUCE RANGE TO FIRST QUADRANT
	.PUFSP	20			; X
	.BASN				; X=X/(PI/2)
	.WORD	DPUFL, TWODPI+2		; 2/PI
	.WORD	$DMUL
	.WORD	$FRACT			; GET FRACTION PART, INTEGER PART IN R5
	.WORD	5$
	.WORD	$DSUB
	.WORD	10$
5$:	ADD	R5, 24(SP)		; ADD QUADRANT TO QUADRANT FLAG WORD
	ASR	24(SP)			; IS ANGLE IN AN ODD QUADRANT?
	BCS	$PUSH1			; YES, SO X=X-1 (SUBTRACT PI/2 FROM ORIGINAL X)
10$:	BIC	#100000, (SP)		; TAKE ABSOLUTE VALUE OF X
	BEQ	20$			; X=0.0
	CMP	#^F1E-13, (SP)		; IS ARGUMENT TOO SMALL?
	BLO	14$			; NUMBER IS REASONABLE
;
; SPECIAL CASE OF X VERY TINY.  LIM (X-->0) SIN(X)==X
;
	.BASN				; AND EXIT
	.WORD	DPUFL, C0+2		; QUICK EVALUATION OF POLYNOMIAL
	.WORD	$DMUL
	.WORD	20$
; EVALUATE POLYNOMIAL
14$:
	MOV	#SCTAB, R0		; POINT TO COEFFICIENTS
	.BASN				; DO SIMPLE POLYNOMIAL APPROXIMATION P(X^2)
	.WORD	DDUP			; COPY X
	.WORD	$APPRX
	.WORD	$DMUL			; SIN=X*P(X^2)
	.WORD	20$
; CALCULATE SIGN OF RESULT
20$:
	ASL	(SP)			; SIGN TO CARRY
	BEQ	30$			; RESULT IS 0.0
	ADC	24(SP)			; PROPAGATE SIGN
	ASR	24(SP)			; SIGN FLAG TO CARRY
	ROR	(SP)			; SIGN TO RESULT
30$:
	.POFSP	26			; POP RESULT INTO RESULT SLOT
; CLEAN UP STACK AND EXIT
	CALL	PO0..4
	POP	R5
SCEXI:
	CMP	(SP)+, (SP)+		; QUADRANT/SIGN FLAG
	JMP	@(R4)+

P4SAVJ: .POFSP	10		; POP RESULT ON THE STACK
P4SAV:	POP	R4
	BR	RSAVJ


; THIS ROUTINE PROVIDES THE DOUBLE PRECISION TANGENT OF THE
; ARGUMENT PASSED ON THE STACK.
$TAN::
	PUSH	R4
	MOV	R4, $SAVJMP
	.PUFSP	2
	.BASN
	.WORD	$SIN
	.WORD	1$
	.WORD	$COS
	.WORD	TAN2
	.WORD	$DDIV
	.WORD	P4SAVJ
1$:	.PUFSP	12
TAN4:	JMP	@(R4)+
TAN2:	TST	(SP)
	BNE	TAN4
	.E.ARG




; THIS ROUTINE CALCULATES THE DOUBLE PRECISION NATURAL LOG OF THE
; ARGUMENT PASSED ON THE STACK. AFTER A RANGE REDUCTION TO
; 1/SQR(2) <= X <= SQR(2), ALGORITHM LOGE 2705 FROM HART, "COMPUTER
; APPROXIMATIONS" IS USED TO CALCULATE LN(X) TO 18 PLACES.
;
; AT ABOUT 10$: THE STATE OF THE STACK IS:
;
; SP==>	POWER OF 2 OF ARG
;	R2
;	R1
;	R0
;	RETURN ADDRESS
;	N1
;	N2
;	N3	(ARGUMENT)
;	N4
$LOG::				; NATURAL LOG
	JSR	R4, SAVER4
	PUSH	R0, R1, R2	; SAVE SOME REGS
;
; PUT EXPONENT OF ARG ON STACK AND REDUCE RANGE TO 0.5 <= X < 1
;
	MOV	10(SP), R1	; GET SIGN/EXPONENT/MSB OF SIGNIFICAND
	BEQ	BADLOG		; LOG OF ZERO!!!!!
	ASL	R1		; BYTE ALIGN EXPONENT
	BCS	BADLOG		; OOPS!  LOG OF NEGATIVE NUMBER
	SWAB	R1		; EXPONENT TO LOW BYTE
	CLR	R0		; TARGET OF EXPONENT
	BISB	R1, R0		; MOVE EXPONENT TO R0
	CLRB	R1		; ZAP EXPONENT
	BISB	#200, R1		; SET EXPONENT TO ZERO (EXCESS 200)
	SWAB	R1		; EXPONENT TO UPPER BYTE
	CLC			; ZERO TO SIGN
	ROR	R1		; MAKE INTO FLOATING POINT FORMAT
	SUB	#200, R0		; CONVERT EXPONENT FROM EXCESS 200
	PUSH	R0		; SAVE BINARY EXPONENT
;
; REDUCE RANGE TO 1/SQR(2) <= X <= SQR(2)
;
	CMP	#40065, R1	; 1/SQR(2)  (APPROX)
	BLT	10$		; ARG IS IN RANGE
	ADD	#200, R1		; MULTIPLY ARG BY 2
	DEC	(SP)		; ADJUST BINARY EXPONENT
;
; BUILD PARAMETERS USED IN RATIONAL APPROXIMATION
;
10$:
	MOV	SP, R0		; POINT PAST WHERE X WILL BE PUSHED
	.PUFSP	12		; PUSH ARG
	MOV	R1, (SP)	; CALCULATE X (REDUCED RANGE)
	MOV	R0, R1
	.FPUM1			; PUSH X, POINT PAST WHERE Z GOES
	.BASN
	.WORD	$PUSH1		; PUSH 1.0
	.WORD	$DSUB		; X-1
	.WORD	12$
	.WORD	$DADD
	.WORD	$DDIV
	.WORD	$LO14$
	.WORD	$APPRX
	.WORD	$LO16$
12$:	.PUFSP	10		; X
$PUSH1::
	JSR	R4, PFLT4
	.WORD	^F1.0		; 1.0
	JMP	@(R4)+

; CALCUALTE Z*P(Z^2)/Q(Z^2)
$LO14$:	MOV	#LOGTAB, R0	; POINT TO COEFFICIENTS
R.JR4:	JMP	@(R4)+


; IF RANGE REDUCTION THEN MULTIPLY RESULT BY N*LN(2) WHERE
; ARGUMENT=2^N*X
$LO16$:	MOV	20(SP), R0	; GET POWER OF 2 OF ARG
	BEQ	50$		; NO RANGE REDUCTION HAPPENED
	PUSH	R0		; RECORD SIGN
	BPL	20$		; POWER IS POSITIVE
	NEG	R0		; GET ABSOLUTE VALUE
;
; CONVERT POWER OF 2 TO FLOATING POINT NUMBER
;
20$:	MOV	#9., R1		; ASSUMED EXPONENT
21$:	DEC	R1		; NOPE, ADJUST EXPONENT
	ASLB	R0		; NORMALIZED?
	BCC	21$		; NO

; MULTIPLY POWER OF 2 BY LN(2)
40$:
	ADD	#200, R1	; CONVERT TO EXCESS 200 NOTATION
	SWAB	R1		; EXPONENT TO UPPER BYTE
	BIS	R1, R0		; SET EXPONENT
	ASL	(SP)+		; PICK UP SIGN IN CARRY
	ROR	R0		; CONVERT TO FLOATING POINT FORMAT
	JSR	R4, PUSH4Z	; PUSH NUMBER ON STACK
	MOV	R0, (SP)		; POWER OF 2
	.BASN
	.WORD	DPUFL, LN2+2	; LN(2)
	.WORD	$DMUL		; N*LN(2)
	.WORD	$DADD		; ADJUST FOR INITIAL RANGE REDUCTION
	.WORD	50$
; PUT ANSWER IN RESULT SLOT AND EXIT
50$:
	.POFSP	30		; MOVE RESULT
	ADD	#12, SP		; WIPE OUT JUNK
LOGEX:				; RESTORE REGS
	POP	R2, R1, R0, R4
	BNE	R.JR4
	MOV	$SAVJMP, R4
RSAVJ:	CLR	$SAVJMP
	JMP	@(R4)+


;
; THIS ROUTINE PROVEDES THE DOUBLE PRECISION INVERSE TANGENT OF
; THE ARGUMENT PASSED ON THE STACK.  THE ALGORITHM USED IS
; ARCTN 5076 FROM HART, "COMPUTER APPROXIMATIONS".
;
; THE STATE OF THE STACK AT ABOUT 10$: IS AS FOLLOWS:
;
; SP==>	SIGN FLAG
;	R3
;	R2
;	R1
;	R0
;	RETURN ADDRESS
;	N1
;	N2	(ARGUMENT)
;	N3
;	N4
$ATAN::				; DOUBLE PRECISION ARC-TANGENT
	JSR	R4, SAVER4
	JSR	R3, PU3210	; PUSH	R3, R2, R1, R0
	CLR	-(SP)		; ASSUME POSITIVE ARG
	CLR	R1		; ASSUME NO RANGE REDUCTION
	.PUFSP	14		; PUSH ARG
	MOV	(SP), R0	; GET SIGN/EXPONENT/MSB OF SIGNIFICAND
	BPL	10$		; ARG IS POSITIVE
	COM	10(SP)		; SET NEGATIVE FLAG
	BIC	#100000, R0	; CLEAR FOR RANGE COMPARES
	MOV	R0, (SP)	; TAKE ABSOLUTE VALUE OF ARG
	MOV	R0, 24(SP)	; TAKE ABSOLUTE VALUE OF ORIGINAL ARG
; CHECK IF RANGE REDUCTION NECESSARY
10$:
	CMP	#37724, R0	; TAN(PI/8) (APPROX)
	BGT	ATAN30		; X<TAN(PI/8) SO NO RANGE REDUCTION
	CMP	#40432, R0	; TAN(3*PI/8) (APPROX)
	BLE	ATAN20		; X>TAN(3*PI/8)
; TAN(PI/8) <= X < TAN(3*PI/8)  SO X=(X-1)/(X+1)
	.BASN
	.WORD	$PUSH1		; PUSH 1.0
	.WORD	$DSUB
	.WORD	11$
	.WORD	$DADD
	.WORD	$DDIV
	.WORD	$ATAN1
11$:	.PUFSP	24		; X
	BR	$PUSH1
$ATAN1:	MOV	#PIDV4, R1	; POINT TO PI/4
	BR	ATAN30		; DO APPROXIMATION


; X >= TAN(3*PI/8) SO X=-1/X
ATAN20:
	ADD	#10, SP		; WIPE OUT ARG ON STACK
	JSR	R4, PFLT4
	.WORD	^F-1.0		; PUSH -1.0
	.PUFSP	24		; X
	.BASN
	.WORD	$DDIV
	.WORD	21$
21$:	MOV	#$PIDV2, R1	; POINT TO PI/2
;
; APPROXIMATE ATN(X) WHERE X <= TAN(PI/8)
;
ATAN30:
	MOV	#ATNTAB, R0	; POINT TO COEFFICIENT TABLE
	.BASN			; PERFORM RATIONAL APROXIMATION
	.WORD	$APPRX
	.WORD	ATAN31


;; 	THIS ROUTINE RETURNS TNE RANDOM VALUE
$RAN::
	MOV	R4, $SAVJMP
	TST	(SP)
	BEQ	RAN1		; PREVIOUS VALUE
	BPL	2$		; NEXT VALUE
	CLR	$RNDSAV
	MOV	#$PIDV2, R2
	MOV	#$III, R3
	CALL	COPY4
2$:	MOV	#$III, R5
	MOV	(R5)+, R0
	MOV	(R5)+, R1
	MOV	(R5)+, R2
	MOV	(R5)+, R3
	BNE	3$
	MOV	#3, R3
	INC	R0
	BR	23$
3$:	CALL	PDAUG2
	SUB	#10, R5
	ADD	(R5)+, R0
	ADD	(R5)+, R1
	ADC	R0
	ADD	(R5)+, R2
	ADC	R1
	ADD	(R5), R3
	ADC	R2
	ADD	(R5)+, R0
	BPL	23$
	ADD	#100000, R0
23$:	MOV	R3, -(R5)
	MOV	R2, -(R5)
	MOV	R1, -(R5)
	MOV	R0, -(R5)
	MOV	#202, R5
4$:	DEC	R5
	CALL	PDAUG2
	BCC	4$
	CALL	SR8DBL
	CALL	PDAL2
	TST	$RNDSAV
	BEQ	RAN9
	ADD	#10, SP
	JSR	R3, PU3210	; PUSH	R3, R2, R1, R0
RAN8:	MOV	SP, R2
	MOV	#$RNDSAV, R3
	CALL	COPY4
RAN1:	MOV	#$RNDSAV, R2
	MOV	SP, R3
	CALL	COPY4
	BR	RSAVJ
RAN9:	JSR	R3, PU3210	; PUSH	R3, R2, R1, R0
	MOV	R4, R0
	CLR	R1		; SIGNAL REGISTER
	.PUFSP	10
	BIC	#100000, (SP)
13$:	CMP	#^F1.0, (SP)
	BGT	11$
	DEC	R1
	BGE	RAN15
	.BASN
	.WORD	RAN12
	.WORD	$DDIV
	.WORD	13$
11$:	MOV	SP, R2
	MOV	R2, R3
	ADD	#20, R3
	CALL	COPY4
	INC	R1
	BLE	RAN15
	.BASN
	.WORD	RAN12
	.WORD	$DMUL
	.WORD	13$
RAN12:	JSR	R4, PFLT4
	.WORD	41040
	JMP	@(R4)+
RAN15:	.BASN
	.WORD	DDROP
	.WORD	RAN12
	.WORD	$DDIV
	.WORD	$PUSH1		; 1.0
	.WORD	$DADD
	.WORD	$DSUB
	.WORD	17$
17$:	BIC	#100000, (SP)
	MOV	SP, R2
	MOV	#$III, R3
	CALL	COPY4
	MOV	R0, R4
	BR	RAN8


; THIS ROUTINE RETURNS THE FRACTIONAL PART OF THE ARGUMENT PASSED ON THE
; STACK.  THE LAST PART OF THE INTEGER IS RETURNED IN R5, AND THE FRACTIONAL
; PART OF THE ARG IS RETURNED ON THE STACK.  THIS ROUTINE IS CALLED BY
; THE TRIG FUNCTIONS AND THE EXP ROUTINE TO PERFORM RANGE REDUCTION.  NO
; REGISTERS ARE PRESERVED, AND ALL REGISTERS ARE DESTROYED.
$FRACT::			; RETURN FRACTION PART
	MOV	SP, R5
	PUSH	R4
	MOV	(R5)+, R1	; PICK UP NUMBER
	MOV	(R5)+, R2
	MOV	(R5)+, R3
	MOV	(R5), R4

; GET SIGN OF INTEGER PART AND SPLIT UP EXPONENT AND SIGNIFICAND
	CLR	R5		; ASSUME NO INTEGER PART
	MOV	R1, R0		; COPY SIGN/EXPONENT/MSB OF SIGNIFICAND
	PUSH	R0		; REMEMBER THE SIGN
	ASL	R0		; BYTE ALIGN EXPONENT
	CLRB	R0		; WIPE OUT SIGNIFICAND BITS
	SWAB	R0		; EXPONENT TO LOW BYTE
	BIC	#177400, R1	; CLEAR EXPONENT BITS
	BIS	#200, R1	; SET "HIDDEN" BIT
; DETERMINE IF NUMBER IS ALREADY A FRACTION OR IF INTEGER PART IS TOO
; BIG TO HAVE A FRACTIONAL PART.
	SUB	#200, R0	; CONVERT FROM EXCESS 200 TO BINARY
	BLE	60$		; NUMBER IS ALREADY FRACTION
	CMP	#56., R0	; NUMBER TOO BIG?
	BGT	20$		; NO
; NUMBER IS TOO BIG, SO IT HAS NO FRACTIONAL PART.  RETURN
; ZERO AS THE FRACTION.
	MOV	#77770, R5	; SET BIG INTEGER
10$:
	JSR	R4, PUSH4Z		; ZERO THE RESULT
	BR	59$		; .POFSP 12; RETURN TO CALLER
; SHIFT NUMBER LEFT TO PUT INTEGER PART TO LEFT OF IMPLIED BINARY
; POINT WITH FRACTION PART TO THE RIGHT.  R5 IS USED TO HOLD
; OVERFLOW WHICH SHIFTS OUT OF R1.  THIS OVERFLOW IS EITHER NOT
; SIGNIFICANT (FOR QUADRANT DETERMINATION) OR AN ERROR (FOR EXP).
20$:
	CALL	ASL1.4
	ROL	R5		; OVERFLOW INTO R5
	ADC	R5		; RECORD MORE OVERFLOW
	SOB	R0, 20$		; COUNT SHIFT IN EXPONENT, MORE SHIFTING TO DO
; SAVE LOWER BITS OF INTEGER PART IN R5 AND PREPARE TO NORMALIZE REMAINING
; FRACTION.
	SWAB	R5		; MAKE ROOM IN LOW BYTE
	SWAB	R1		; MOVE INTEGER TO LOW BYTE
	BISB	R1, R5		; MOVE INTEGER BITS
	TST	(SP)		; NEGATIVE NUMBER?
	BPL	30$		; NOPE
	NEG	R5		; YES, RECORD THIS FACT
30$:
	CLRB	R1		; KILL INTEGER BITS
	SWAB	R1		; MOVE SIGNIFICAND TO LOW BYTE
; TEST IF FRACTION NEEDS TO BE NORMALIZED.  IF REMAINING BITS ARE ZERO,
; NO NORMALIZE IS NEEDED, OR THE NUMBER MIGHT ALREADY BE NORMALIZED
; (50% CHANCE)
	TSTB	R1		; NUMBER NORMALIZED?
	BMI	50$		; YES, TAKE FAST EXIT
	PUSH	R1		; CHECK IF FRACTION IS ZERO
	BIS	R2, (SP)	; BY OR'ING TOGETHER
	BIS	R3, (SP)	; ALL THE BITS
	BIS	R4, (SP)+	; ANY BITS SET?
	BEQ	10$		; RESULT IS ZERO
; NORMALIZE FRACTION
40$:
	CALL	ASL1.4
	DEC	R0		; COUNT SHIFT IN EXPONENT
	TSTB	R1		; NUMBER NORMALIZED YET?
	BPL	40$		; NOPE
; CONVERT BACK TO FLOATING POINT FORMAT
50$:	ADD	#200, R0		; EXCESS 200 NOTATION FOR EXPONENT
	SWAB	R0		; MOVE EXPONENT TO UPPER BYTE
	ASLB	R1		; ALIGN SIGNIFICAND, KILL HIDDEN BIT
	BIS	R0, R1		; FILL IN EXPONENT
	ROR	R1		; CHANGE TO FLOATING FORMAT

	PUSH	R4, R3, R2, R1	; STORE NUMBER IN RESULT SLOT
59$:	.POFSP	12
60$:	TST	(SP)+		; POP SIGN ARG
	POP	R4
$ABS::	BIC	#100000, (SP)	; FRACTION ALWAYS POSITIVE
	JMP	@(R4)+


; THIS ROUTINE RETURNS INTEGER PART OF THE ARGUMENT PASSED ON
; THE STACK.ALL REGISTERS ARE DESTROYED.
$FIX::
	MOV	R4, $SAVJMP
	PUSH	R4		; SAVE RETURN REGISTRE
	.PUFSP	2		; PUSH ARGUMENT ON STACK
$FIX1:	.BASN
	.WORD	DDUP
	.WORD	$FRACT
	.WORD	1$
	.WORD	$DADD
	.WORD	P4SAVJ
1$:	TST	10(SP)
	BLE	3$
	BIS	#100000, (SP)
3$:	JMP	@(R4)+

; THIS ROUTINE RETURNS THE HIGHEST INTEGER VALUE LESS THAN
; ARGUMENT PASSED ON THE STACK.
$INT::
	TST	(SP)
	BPL	$FIX
	MOV	R4, $SAVJMP
	PUSH	R4
	.PUFSP	2
	.BASN
	.WORD	DDUP
	.WORD	$FRACT
	.WORD	1$
	.WORD	DDROP
	.WORD	$PUSH1
	.WORD	$DSUB		; x - 1
	.WORD	$FIX1
1$:	TST	(SP)
	BNE	JNEXT1		; @(R4)+
	ADD	#10, SP
	BR	$FIX1
; THIS ROUTINE IS CALLED BY THE TRANCENDENTAL FUNCTIONS TO EVALUATE
; A RATIONAL APPROXIMATION OF THE FORM X*P(X^2)/Q(X^2) WHERE X IS
; THE ARGUMENT PASSED ON THE STACK.  R0 POINTS PAST THE TABLE OF
; COEFFICIENTS TO USE IN THE APPROXIMATION.  R0 POINTS TO TWO BYTES
; OF INFORMATION. THE FIRST BYTE IS THE ORDER OF P AND THE SECOND BYTE
; IS THE ORDER OF Q.  DURING THE COMPUTATION, THE STATE OF THE STACK
; IS AS FOLLOWS:  (ITEMS IN PARENTHESES ARE QUAD WORD FLOATING POINT
; NUMBERS)
;
; SP==>	(Q)
;	(P)
;	(X^2)
;	(X)
;	R4
;	R3	REGISTERS SAVED AT CALL
;	R2
;	R1
;	UNUSED
;	(X)
;
;
; THE ALGORITHM USES HORNER'S METHOD TO EVALUATE THE POLYNOMIALS
;

$APPRX::			; RATIONAL APPROXIMATION
	JSR	R0, PU0123
	PUSH	R4, R5		; SAVE REGISTERS
;
; COPY X AND CALCULATE X^2
;
	MOV	SP, R3		; POINT PAST WHERE X WILL BE BUILT
	.PUFSP	14		; PUSH X
	MOV	SP, R5		; POINT PAST WHERE X^2 WILL BE BUILT
	FPUSH	-(R3)		; PUSH X
	FPUSH	-(R3)		; PUSH X
	.BASN
	.WORD 	$DMUL
	.WORD	5$
; PICK UP POLYNOMIAL ORDERS AND CALCULATE X*P(X^2)
5$:	MOVB	(R0), R2	; ORDER OF P
	MOVB	1(R0), R3	; ORDER OF Q
1$:	MOV	R0, R1
	.FPUM1			; PUSH P(N)
	MOV	R1, R0
	CLRB	FMABR
;; Multiply-Add
;; Repeat R2 times
FMA:	MOV	R5, R1
	.FPUM1
	.BASN
	.WORD	$DMUL
	.WORD	FMA1
FMA1:	MOV	R0, R1
	.FPUM1
	MOV	R1, R0
	.BASN
	.WORD	$DADD
	.WORD	1$
1$:	SOB	R2, FMA
FMABR:	BR	.		; Self-modifying

APPRX3:	MOV	R3, R2
	BEQ	40$		; JUST A SINGLE POLYNOMIAL EVALUATION?
	FPUSH	0(R5)		; X
	.BASN
	.WORD	$DMUL
	.WORD	13$
; CALCULATE Q(X^2)
13$:	MOV	R5, R1		; POINT PAST X^2
	.FPUM1			; X^2
	MOVB	#<14$-FMABR-2>/2, FMABR
	BR	FMA1
; CALCUALTE X*P(X^2)/Q(X^2), POP RESULT, RESTORE REGISTERS AND EXIT
14$:	.BASN
	.WORD	$DDIV
	.WORD	40$
40$:	.POFSP	42		; STORE RESULT
	ADD	#20, SP		; KILL JUNK
	POP	R5, R4, R3, R2, R1	; RESTORE REGS
POPTST:	TST	(SP)+		;SET CONDITION CODES
JNEXT1:	JMP	@(R4)+		;RETURN WITH COPARISION IN CONDITION CODES



;; THIS ROUTINE COMPARES TWO DOUBLE PRECISION REAL NUMBERS ON THE
;; STACK AND RETURNS THE RESULT OF THE COMPARISON IN THE CONDITION
;; CODES.
$DCMP::				; DOUBLE PRECISION COMPARE
	PUSH	R1
	MOV	SP, R1
	TST	(R1)+
	MOV	#4, R0
1$:	CMP	10(R1), (R1)+
	BNE	2$
	SOB	R0, 1$
	BR	9$		; R0==0 here. VALUES ARE EQUAL RETURN EQUAL COMPARISION
;; FIRST ARG HIGHER THAN SECOND ARG.  RESULT IS FIRST ARG
2$:	MOV	12(SP), R0	;MOV FIRST WORD OF FIRST ARG. BEFORE RETURN
	BHIS	9$
	MOV	2(SP), R0	; FIRST ARG LOWER THAN SECOND ARG.  RESULT IS NEGATED SECOND ARG
	NEG	R0		;NEGATE SIGN OF SECOND ARG.
9$:	POP	R1
	ADD	#20, SP		;WIPE OUT JUNK ON STACK
	TST	R0		;SET CONDITION CODES
	JMP	@(R4)+		;RETURN WITH COMPARISION RESULT IN CONDITION CODES


$BIN::	MOV	#2, R2
	BR	$UTOA
$OCT::	MOV	#8., R2
	BR	$UTOA
$HEX::	MOV	#16., R2
$UTOA:	POP	R5		; the number to print
	MOV	#$BUFOUT, R1
	CALL	UTOA
; keep space at the end		DEC	R2
	BR	PU12J4


$TRAP7:	TRAP	7

;; uint16 -> decimal ASCIZ
;;	R1: string address
;;	R2: radix base
;;	R5: uint to convert
UTOA10::
	MOV	#10., R2
UTOA::	PUSH	R1		; save string address
	MOV	R2, R3		; R3: radix base
	ADD	#16., R1	; max space for binary presentation
	CLRB	(R1)		; terminating '\0'
1$:	CLR	R0		; here R2: radix base
2$:	CMP	R2, R5
	BHI	4$
	ASL	R2
	BCC	2$
3$:	ROR	R2
	ASL	R0
	CMP	R2, R5
	BHI	4$
	SUB	R2, R5
	INC	R0
4$:	CMP	R2, R3		; compare with radix base
	BNE	3$
	ADD	#'0, R5
	CMP	R5, #'9
	BLOS	5$
	ADD	#'A-'0-10., R5
5$:	MOVB	R5, -(R1)	; reminder digit
	MOV	R0, R5		; quotient
	BNE	1$
	MOV	(SP), R2	; R2: string address
6$:	MOVB	(R1)+, (R2)+
	BNE	6$
	POP	R1
	RETURN

$ISTR::	MOV	#$BUFOUT, R1
	MOVB	#C.SP, (R1)+
	POP	R5
	BPL	1$
	MOVB	#'-, -1(R1)
	NEG	R5
1$:	CALL	UTOA10
	DEC	R1
PU12J4:	MOVB	#C.SP, -1(R2)
	SUB	R1, R2
	PUSH	R1, R2
	JMP	@(R4)+

$P.INT::
	MOV	#$BUFOUT, R1
	CALL	UTOA10
	MOVB	#200, -(R2)	; don't print NL
	.PRINT	R1
	RETURN



;; CONSTANTS USED IN APPROXIMATION
OQ0:	.FLT4	.208136948963436784218221E2
OP0:	.FLT4	.72134769672177719062488E1
OP1:	.FLT4	.577621944696111185E-1
EXPTAB:					; EXP COEFFICIENTS


;; CONSTANTS USED IN THE APPROXIMATION
IQ0:	.FLT4	-.120069589779605254717525E2
IQ1:	.FLT4	.19480966070088973051623E2
IQ2:	.FLT4	-.89111090279378312337E1
IP0:	.FLT4	-.240139179559210509868484E2
IP1:	.FLT4	.30957292821537650062264E2
IP2:	.FLT4	-.96376909336868659324E1
IP3:	.FLT4	.4210873712179797145
LOGTAB: .BYTE	3,3		; TABLE OF LOG COEFFICIENTS


;; CONSTANTS USED IN APPROXIMATION
PIDV4:	.FLT4	.78539816339744830961566	; PI/4
$PIDV2:	.FLT4	1.57079632679489661923132	; PI/2
Q0:	.FLT4	.445413400592906804445595E2
Q1:	.FLT4	.92324801072300974840693E2
Q2:	.FLT4	.62835930511032376833267E2
Q3:	.FLT4	.1550397755142198752523E2
P0:	.FLT4	.445413400592906803197511E2
P1:	.FLT4	.77477687719204208616481E2
P2:	.FLT4	.40969264832102256374186E2
P3:	.FLT4	.666057901700926265753E1
P4:	.FLT4	.1589740288482307048
ATNTAB:	.BYTE	4, 4		; TABLE OF COEFFICIENTS


$APLMI::
	.WORD	$DADD, $DSUB
$AMATH::
	.WORD	$SQRT, $SIN, $COS, $TAN, $ATAN, $EXP, $LOG, $INT, $FIX, $ABS, $DSGN



;.IIF EQ C$AMAL,	.END
