;**************************************************************************
;**** FP Math Routines: ****
; ** Very similar to _ecvt() **
FLTBCD:	push	exp1		; FP to 7 digit BCD # +exp
	tst	exp1
	brpl	FLBCDP
	rcall	FLTNEG
FLBCDP:	mov	temp,exp1
	subi	temp,0x7f
	mov	DEXP,temp
FLBCD1:	ldi	zl,low(k9999999*2) ; Fit between 9,999,999 to 999,999.9 (Gordon)
	ldi	zh,high(k9999999*2)
	rcall	KTOAC2
	rcall	FCMP
	breq	FLBCD4
	brpl	FLBCD3
	ldi	zl,low(k999999p9*2)
	ldi	zh,high(k999999p9*2)
	rcall	KTOAC2
	rcall	FCMP
	brpl	FLBCD4
	dec	DEXP
	ldi	zl,low(k10*2)	; 10.0f
	ldi	zh,high(k10*2)	; 10.0f
	rcall	KTOAC2
	rcall	PUSH2
FLBCD2:	rcall	POP2
	rcall	FMUL		; keep multiplying or dividing by 10.0f
	rjmp	FLBCD1		;  until it fits!
FLBCD3:	inc	DEXP
	ldi	zl,low(kp1*2)	;  0.1f (x.x * 0.1f = x.x/10.0f)
	ldi	zh,high(kp1*2)	;  0.1f (x.x * 0.1f = x.x/10.0f)
	rcall	KTOAC2
	rcall	PUSH2
	rjmp	FLBCD2
FLBCD4:	pop	temp		; orig exp1
	mov	sign1,temp
	andi	temp,0x7f
	neg	temp
	add	temp,DEXP
	subi	temp,0x7b
	mov	DEXP,temp
	andi	sign1,0x80	; r5 shares the mant sign and first BCD digit
	mov	r5,sign1
ITOD:	rcall	FTOI		; Ok, convert to BIG integer number!
	ldi	zl,low(I1E6*2)
	ldi	zh,high(I1E6*2)
	rcall	DECDIG		; see how many 1,000,000's
	or	r5,temp
	ldi	zl,low(I1E5*2)
	ldi	zh,high(I1E5*2)
	rcall	DECDIG		; how many 100,000's
	mov	r6,temp
	ldi	zl,low(I1E4*2)
	ldi	zh,high(I1E4*2)
	rcall	DECDIG		; how many 10,000's
	swap	r6		; BCD share 100,000s & 10,000s
	or	r6,temp
	ldi	zl,low(I1E3*2)
	ldi	zh,high(I1E3*2)
	rcall	DECDIG		; how many 1,000's
	mov	r7,temp
	ldi	zl,low(I1E2*2)
 	ldi	zh,high(I1E2*2)
	rcall	DECDIG		; how many 100's
	swap	r7		; BCD shar 1,000s & 100s
	or	r7,temp
	ldi	zl,low(I1E1*2)
	ldi	zh,high(I1E1*2)
	rcall	DECDIG		; how many 10's
	mov	r8,temp
	swap	r8
	or	r8,mant1	; BCD share 10s & 1s
	ret			; exp is in 2s comp @ reg 'DEXP'
;
DECDIG:	rcall	KTOAC2
	clr	temp
DECDIG1: cp	mant1,mant2
	cpc	mant1m,mant2m
	cpc	mant1h,mant2h
	brcs	DECDIGX
	sub	mant1,mant2
	sbc	mant1m,mant2m
	sbc	mant1h,mant2h
	inc	temp
	brne	DECDIG1
DECDIGX: ret
;
FTOIMEM: rcall	FSRAMA1
FTOI:	rcall	UNPACK
	brne	FTOI1
	rjmp	MINRES
FTOI1:	ldi	temp,22
	sub	temp,exp1
	brpl	FTOI2
	rjmp	MAXRES
FTOI2:	cpi	temp,24
	brlo	FTOI3
	rjmp	MINRES
FTOI3:	cpi	temp,8	; use fast byte-move, if possible
	brlo	FTOI4
	mov	mant1,mant1m
	mov	mant1m,mant1h
	mov	mant1h,quot
	clr	quot
	subi	temp,8
	rjmp	FTOI3
FTOI4:	tst	temp
	breq	FTOI6
FTOI5:	lsr	quot
	ror	mant1h
	ror	mant1m
	ror	mant1
	dec	temp
	brne	FTOI5
FTOI6:
;	tst	sign1
;	brpl	FTOIX
;	rjmp	NEGMANT
FTOIX:	ret
;
;** ACC1 -= *r30 **
FSUBMEM: rcall	FSRAMA2	; Acc1 -= *r30
;** ACC1 -= ACC2 **
FSUB:	rcall	UNPACK
	cpi	exp2,0x80
	breq	FSADX
	ldi	temp,0x80
	eor	sign2,temp
	rjmp	FADD1
FSUNDER: brmi	FSADX
FRET2:	rcall	SWAPACC
FSADX:	rjmp	REPACK
;** ACC1 += 0.5f **
FADDp5:	ldi	exp2,0x3f
	clr	mant2
	clr	mant2m
	clr	mant2h
	rjmp	FADD
FADDMEM: rcall	FSRAMA2	; Acc1 += *r30
;** ACC1 += ACC2 **
FADD:	rcall	UNPACK
	cpi	exp2,0x80
	breq	FSADX
FADD1:	cpi	exp1,0x80
	breq	FRET2
FADD2:	mov	temp,exp1
	sub	temp,exp2
	brvs	FSADX
	brpl	FADD3
	rcall	SWAPACC
	rjmp	FADD2
FADD3:	cpi	temp,24	; are we more than 24 bits diff?
	brlo	FADD3a	; no, we can add it.
	clr	mant2	; yes, acc2 is to small
	clr	mant2m
	clr	mant2h
FADD3a: cpi	temp,8
	brlo	FADD3b
	mov	mant2,mant2m
	mov	mant2m,mant2h
	clr	mant2h
	subi	temp,8
	rjmp	FADD3a
FADD3b: tst	temp
	breq	FADD3d
FADD3c:	lsr	mant2h
	ror	mant2m
	ror	mant2
	dec	temp
	brne	FADD3c
FADD3d:	mov	temp,sign1
	eor	temp,sign2
	brmi	FADD4
	rcall	UADD
	brcc	FSADX
	ror	mant1h
	ror	mant1m
	ror	mant1
	subi	exp1,-1	; Add +1
	brvc	FSADX
	rjmp	MAXRES
FADD4:	rcall	USUB
	breq	FCLR
	brcc	FADD5
	rcall	NEGMANT
FADD5:	tst	mant1h	; normalize
	brmi	FSADX
	lsl	mant1
	rol	mant1m
	rol	mant1h
	subi	exp1,1
	brvc	FADD5
	rjmp	MAXRES
FCLR:	rjmp	MINRES
;
UADD:	add	mant1,mant2
	adc	mant1m,mant2m
	adc	mant1h,mant2h
	ret
;
USUB:	sub	mant1,mant2
	sbc	mant1m,mant2m
	sbc	mant1h,mant2h
	ret
;
;** ACC1 = -ACC1 **
FLTNEG:	rcall	UNPACK
	com	sign1
	rjmp	REPACK
;
NEGMANT: ldi	temp,-1
	sub	mant1,temp
	sbc	mant1m,temp
	sbc	mant1h,temp
	ret
;
;** MANT1 <<= 1 **
RLMANT1: lsl	mant1
	rol	mant1m
	rol	mant1h
	rol	mant1t
	ret
;
;** Straight 24bit Unsigned ACC1 / ACC2 **
UDIVMANT: clr	quot
	clr	quotm
	clr	quoth
	ldi	count,24
UDIVLP:	cp	mant1,mant2
	cpc	mant1m,mant2m
	cpc	mant1h,mant2h
	brlo	UDIV1
	sub	mant1,mant2
	sbc	mant1m,mant2m
	sbc	mant1h,mant2h
	sec
	rjmp	UDIV2
UDIV1:	clc
UDIV2:	rol	quot
	rol	quotm
	rol	quoth
	lsl	mant1
	rol	mant1m
	rol	mant1h
	dec	count
	brne	UDIVLP
	push	mant1
	mov	mant1,quot
	mov	mant1m,quotm
	mov	mant1h,quoth
	pop	temp
	lsr	temp
	brcc	udivx
	inc	mant1
	brne	udivx
	inc	mant1m
	brne	udivx
	inc	mant1h
udivx:
	ret
;
;** ACC1 = 1/*r30 **
F1OVERMEM: rcall FSRAMA1
;** ACC1 = 1/ACC1 **
F1OVERX: rcall	AC1TOAC2
	clr	mant1
	clr	mant1m
	ldi	temp,0x80
	mov	mant1h,temp
	ldi	exp1,0x3f	; ieee 1.0f
	rjmp	FDIV		; do 1/x
;
;** Acc1 /= *r30 **
FDIVMEM: rcall	FSRAMA2
	rjmp	FDIV
;** ACC1 /= 10.0f */
FDIV10:	clr	mant2		; x/10.0f  could use fmul * 0.1f?
	clr	mant2m
	ldi	mant2h,0x20
	ldi	exp2,0x41	;Put 10.0 -> Acc2
;** ACC1 /= ACC2 **
FDIV:	tst	exp2		; test for x/0.0
	breq	FDIVZ
	tst	exp1
	breq	MINRES		; 0.0f/x = return 0.0f
FDIVa:	rcall	UNPACK
	breq	MINRES
	eor	sign1,sign2	; get result sign
	sec
	sbc	exp1,exp2	; sub exps.
	brvs	MAXRES		; overflow ?
	lsr	mant2h		; no, de-norm & div mants
	ror	mant2m
	ror	mant2
	lsr	mant1h
	ror	mant1m
	ror	mant1
	rcall	UDIVMANT
	tst	mant1h		; need norm?
	brmi	FDIVX		; no, exit
	lsl	mant1
	rol	mant1m
	rol	mant1h
	subi	exp1,1		; --exp
	brvs	MAXRES
FDIVX:	rjmp	REPACK		; re-pack
FDIVZ:	rjmp	MINRES
MAXRES:	ldi	temp,0x7f
	mov	exp1,temp
	or	mant1h,temp
	ldi	temp,0xff
	mov	mant1,temp
	mov	mant1m,temp
	ret
MINRES:	clr	mant1	;Result = 0.0f
	clr	mant1m
	clr	mant1h
	clr	exp1
	clr	sign1
	ret
;
;** ACC1 *= *r30 **
FMULMEM: rcall	FSRAMA2		; Acc1 *= *r30
	rjmp	FMUL
;** ACC1 *= 10.0f **
FMUL10:	ldi	exp2,0x41	; IEEE 10.0f
	ldi	mant2h,0x20
	clr	mant2m
	clr	mant2
;** ACC1 *= ACC2 **
FMUL:	rcall	UNPACK
	breq	MINRES
	cpi	exp2,0x80
	breq	MINRES
	eor	sign1,sign2
	sec
	adc	exp1,exp2
	brvs	MAXRES	;FMULMAX
	rcall	UMUL
	tst	mant1h
	brmi	FMULX
	lsl	quoth
	rol	mant1
	rol	mant1m
	rol	mant1h
	rjmp	REPACK
FMULX:	subi	exp1,-1
	brvs	MAXRES	;FMULMAX
	rjmp	REPACK
;
;** 24bit unsigned Multiply
UMUL:	push	temp
	push	count
	clr	quot
	clr	quotm
	clr	quoth
	clr	temp
	ldi	count,24
UMULLP:	lsl	quot
	rol	quotm
	rol	quoth
	rol	mant1
	rol	mant1m
	rol	mant1h
	brcc	UMUL1
	add	quot,mant2
	adc	quotm,mant2m
	adc	quoth,mant2h
	adc	mant1,temp
	adc	mant1m,temp
	adc	mant1h,temp
UMUL1:	dec	count
	brne	UMULLP
	pop	count
	pop	temp
	ret
;
;** ACC1 = (*r30) * (*r30) **
FSQRMEM: rcall	FSRAMA1
;** ACC1 *= ACC1 **
FSQR:	rcall	AC1TOAC2
	rjmp	FMUL
;
sqrthalf:
	clr	mant2
	clr	mant2m
	clr	mant2h
	ldi	exp2,0x40
	rjmp	FDIVa		; div by 2 to get our guess
SQERR:	rjmp	MINRES		; add your own error handler here.
;** ACC1 = sqrt(*r30) **
FSQRTMEM: rcall FSRAMA1
;** ACC1 = sqrt(ACC1) **
FSQRT:	tst	exp1
	breq	SQERR		; trap sqrt(0) or sqrt(-x)!
	brmi	SQERR
	mov	r5,mant1	; save org Num. user must preserve r5-r8!
	mov	r6,mant1m
	mov	r7,mant1h
	mov	r8,exp1
	rcall	sqrthalf	; get first guess
	rcall	AC1TOAC2	; put it in Acc2
	ldi	temp,4		; number of iterations
SQRTLP:	push	temp
	rcall	PUSH2		; save 'guess' results
	mov	mant1,r5	; restore orig Number
	mov	mant1m,r6
	mov	mant1h,r7
	mov	exp1,r8
	rcall	FDIV
	rcall	POP2
	rcall	FADD
	rcall	sqrthalf
	rcall	AC1TOAC2
	pop	temp
	dec	temp
	brne	SQRTLP
	ret
;
;** Compare ACC1 == ACC2 **
;** return w/temp = 0,1,or -1 (Z,N flags set) **
FLTCPFLASH: rcall KTOAC2
	rjmp	FLTCP
;** compare two sram floats **
FLTCP2:	push	zl	; save r30,r31, (pointer to acc2 variable)
	push	zh
	mov	zl,yl
	mov	zh,yh
	rcall	FSRAMA1
	pop	zh
	pop	zl
FLTCPMEM: rcall FSRAMA2		; compare Acc1 w/Memory
FLTCP:	rcall	FCMP		; do compare
	tst	temp		; set status reg accordingly
	ret			; return to caller
;
FCMP:	tst	exp1		; is acc1 pos?
	brmi	FCMP1		; no. test acc2
	tst	exp2		; yes, is acc2 pos?
	brmi	A1GTA2		; no, return Acc1 > Acc2
	cp	exp1,exp2	; both are '+'
	brlo	A1LTA2
	brne	A1GTA2
	cp	mant1,mant2	; exps are equal, are mants?
	cpc	mant1m,mant2m
	cpc	mant1h,mant2h
	brlo	A1LTA2
	breq	A1EQA2
A1GTA2:	ldi	temp,1		; ac1 > ac2
	ret
A1LTA2:	ldi	temp,0xff	; ac1 < ac2
	ret
A1EQA2:	clr	temp		; ac1 = ac2
	ret
;** we're here, acc1 must be '-' **
FCMP1:	tst	exp2		; if acc1 is '-' & acc2 is '+'
	brpl	A1LTA2		; then acc1 is < acc2
	cp	exp2,exp1	; both are '-', is exp1 more negative?
	brlo	A1GTA2
	brne	A1LTA2
	cp	mant2,mant1
	cpc	mant2m,mant1m
	cpc	mant2h,mant1h
	brlo	A1GTA2
	breq	A1EQA2		; return acc1 == acc2
	rjmp	A1LTA2
;
;*********************************************************************
; * These routines are for future atof(), atoi(), atol(), etc.
;** Fast Multiply by 10 **
UMUL10:	rcall	AC1TOAC2
	rcall	RLMANT1
	rcall	RLMANT1
	rcall	UADD
	sbci	mant1t,-1	; adci 0
	rjmp	RLMANT1
;
ADDDIG:	rcall	UMUL10
	clr	mant2m
	clr	mant2h
	mov	mant2,temp
	andi	mant2,0x0f
	rcall	UADD
	sbci	mant1t,-1	; just add 'C'
	ret
;*********************************************************************
;
;** convert an integer to FP **
ITOFMEM: rcall	FSRAMA1		; psuedo 32bit long?
	mov	mant1t,exp1
ITOF:	mov	temp,mant1
	or	temp,mant1m
	or	temp,mant1h
	or	temp,mant1t
	brne	ITOF1
	rjmp	MINRES
ITOF1:	tst	mant1t
	brpl	ITOF2
	rcall	NEGMANT
ITOF2:	ldi	exp1,30
	tst	mant1t
ITOF3:	brmi	ITOF4
	dec	exp1
	lsl	mant1
	rol	mant1m
	rol	mant1h
	rol	mant1t
	rjmp	ITOF3
ITOF4:	mov	mant1,mant1m
	mov	mant1m,mant1h
	mov	mant1h,mant1t
	rjmp	REPACK
;
;********************************************************************
;** Memory Move funcs **
;** Copy SRAM Float to Acc1 **
FSRAMA1: ld	mant1,z+
	ld	mant1m,z+
	ld	mant1h,z+
	ld	exp1,z+
	ret
;
;** Copy SRAM Float to Acc2 **
FSRAMA2: ld	mant2,z+
	ld	mant2m,z+
	ld	mant2h,z+
	ld	exp2,z+
	ret
;
;
;** Copy ACC2 = ACC1 **
AC1TOAC2:
	mov	exp2,exp1
	mov	mant2,mant1
	mov	mant2m,mant1m
	mov	mant2h,mant1h
	mov	sign2,sign1
	ret
;
;** ACC1 <-> ACC2 **
SWAPACC: push	mant1
	push	mant1m
	push	mant1h
	push	exp1
	push	sign1
	mov	mant1,mant2
	mov	mant1m,mant2m
	mov	mant1h,mant2h
	mov	exp1,exp2
	mov	sign1,sign2
	pop	sign2
	pop	exp2
	pop	mant2h
	pop	mant2m
	pop	mant2
	ret
;
;** Get 1 byte from program memory & inc pointer **
_lpmbyte: lpm
	adiw	zl,1
	ret
;
;** Put a Flash const -> ACC1
KTOAC1:	rcall	_lpmbyte
	mov	mant1,r0
	rcall	_lpmbyte
	mov	mant1m,r0
	rcall	_lpmbyte
	mov	mant1h,r0
	rcall	_lpmbyte
	mov	exp1,r0
	ret
;
;** Put a Flash const -> ACC2
KTOAC2:	rcall	_lpmbyte
	mov	mant2,r0
	rcall	_lpmbyte
	mov	mant2m,r0
	rcall	_lpmbyte
	mov	mant2h,r0
	rcall	_lpmbyte
	mov	exp2,r0
	ret
;
;** 'Hold' Acc1 in sram **
PUSH1:	sts	MATHTMP,mant1
	sts	MATHTMP+1,mant1m
	sts	MATHTMP+2,mant1h
	sts	MATHTMP+3,exp1
	ret
;
;** 'Restore' Acc1 from sram **
POP1:	lds	mant1,MATHTMP
	lds	mant1m,MATHTMP+1
	lds	mant1h,MATHTMP+2
	lds	exp1,MATHTMP+3
	ret
;
;** 'Hold' Acc2 in sram **
PUSH2:	sts	MATHTMP+4,mant2
	sts	MATHTMP+5,mant2m
	sts	MATHTMP+6,mant2h
	sts	MATHTMP+7,exp2
	ret
;
;** 'Restore' Acc2 from sram **
POP2:	lds	mant2,MATHTMP+4
	lds	mant2m,MATHTMP+5
	lds	mant2h,MATHTMP+6
	lds	exp2,MATHTMP+7
	ret
;
;********************************************************************
;** IEEE conversions **
;** Convert ACC1 & ACC2 from IEEE to work format **
UNPACK:	push	temp	
	mov	sign1,exp1
	ldi	temp,0x80
	lsl	mant1h
	rol	exp1
	eor	exp1,temp	; AVR has no 'eori'
	lsl	temp
	ror	mant1h		; restore Hidden '1' bit
	andi	sign1,0x80
;** Un-Pack Acc2 **
	mov	sign2,exp2
	ldi	temp,0x80
	lsl	mant2h
	rol	exp2
	eor	exp2,temp
	lsl	temp
	ror	mant2h
	andi	sign2,0x80
	pop	temp
	cpi	exp1,0x80	; return w/ACC1=0 ?
	ret
;
;** Convert Acc1 from work format to IEEE **
REPACK:	push	temp
	lsl	mant1h	
	ldi	temp,0x80
	eor	temp,exp1
	lsl	sign1
	ror	temp
	ror	mant1h
	mov	exp1,temp
	pop	temp
	ret
;
tx:	sbis	USR,5
	rjmp	tx
	out 	UDR,temp
	ret
;