math.dasm16

; *** floating point package **

; TODO: isolate these memory locations from BASIC.
; this will be hard since some of them, especially
; FAC and ARG, are re-used for string pointers.
; It will also involve some if-not-defined directives
; to different error routines if BASIC isn't included.
;
; also TODO: optimize some of these routines so that
; they use registers instead of memory.
;
; dependencies in basic.dasm16:
;    ERROR
;    FCERR
;
:RAM5D 
:DECEXP DAT 0 ; decimal exponent during FIN/FOUT
:RAM5E  DAT 0 ; Number of digits during FIN?
:RAM60  DAT 0 ; sign of exponent operator in FIN
;
; Floating Point Accumulators
; 
:FAC1    ; Floating Point Accumulato0r 
:RAM61   ; Exponent (only 8 bits used)
:FACEXP DAT 0
:RAM62   ; Mantissa (32-bit)
:FACHO DAT 0
:RAM64 DAT 0
:FACOV  ;low order mantissa byte for rounding
:RAM70  DAT 0
:FACSGN
:RAM66 DAT 0

:RAM68 ; FAC Overflow during conversions
:BITS    DAT 0

:FAC2
:ARG     ; Floating Point Argument
:RAM69   ; Exponent (only 8 bits used)
:ARGEXP DAT 0
:RAM6A   ; Mantissa (32-bit)
:ARGHO  DAT 0
:RAM6C  DAT 0
:ARGOV
:RAM56  DAT 0
:ARGSGN
:RAM6E  DAT 0
:ARISGN ; Result of signed comparison
:SGNCPR
:RAM6F  DAT 0
;
; Multiplication result - denormalized.
:RESHO 
:RAM26  DAT 0
:RAM27  DAT 0, 0

:RAM67 
:SERLEN         ; Number of terms in a series evaluation
:SGNFLG  DAT 0  ; OR, a sign flag somewhere in FIN.

; Temporary registers
:FACTEMP1
:RAM57  DAT 0, 0, 0
:FACTEMP2
:RAM5C  DAT 0, 0, 0
:FACTEMP3
:RAM4E  DAT 0, 0, 0

:RNDX   ; random number seed
        DAT 0x80, 0x4FC7, 0x5258

:FADDH
:ROMB849
            SET A, CON_HALF
            JMP FADD
            
:FSUB
:ROMB850
            JSR CONUPK
:FSUBT
:ROMB853    SET A, [FACSGN]
            XOR A, -1
            SET [FACSGN], A
            XOR A, [ARGSGN]
            ASR A, 15
            SET [SGNCPR], A
            SET A, [FACEXP]
            JMP FADDT

:ROMB862    JSR SHIFT_RIGHT
            JMP ROMB8A3

;
;  FLOATING POINT ADDITION.
; should not destroy I, J.
;
:FADD
:ROMB867    JSR CONUPK ; read from pointer in A,
                        ; store to ARG.
:FADDT  ; Add FAC and ARG.  (load FACEXP in A first)
:ROMB86A    ; A = FACEXP
            IFE A, 0       ; 0 + ARG = ARG
                JMP COPY_ARG_TO_FAC  ; so return ARG.
            SET [ARGOV], [FACOV] 
            SET X, ARGEXP ;  pointer
            SET A, [X]    ; A = actual exponent
:FADD2
:ROMB877    IFE A, 0 ; FAC + O = FAC
                RTS  ; so return FAC
            SET Y, A
            SUB A, [FACEXP]  ; A = ARGEXP - FACEXP
            IFE A, 0         ; equal exponents
                JMP FADD3
            IFE EX, -1       ; subtraction underflow
                JMP ROMB893
:ROMB881    SET [FACEXP], Y ; Store larger exponent
            SET [FACSGN], [ARGSGN] 
            ;SET [BITS], 0  ; had to add this for some rsn
            XOR A, 0xFFFF
            ADD A, 1       ; ADC #$00 after a subtract
            SET [ARGOV], 0
            SET X, FACEXP ; a pointer to larger exponent
            JMP ROMB897
            ; enter here when carry clear
:ROMB893    SET [FACOV], 0
:ROMB897    IFU A, -7  ; Big exponent difference 
                JMP ROMB862 
                ;which does this:
                ;JSR SHIFT_RIGHT ; 
                ;JMP FADD3
            ; I think this is the number of bits
            ; to shift right, in negative 8-bit
            SET Y, A  
            SET A, [FACOV]
:ROMB89E    SHR [X+1], 1 ; shift mantissa to the right
            ;rotate 0-bit from the left
            JSR SHIFT_RIGHT4 ; Finish shifting right 
:FADD3      ; 84 a000 0000 - 84 bccc cccd
:ROMB8A3    IFA [SGNCPR], -1 ; result positive
                JMP FADD4 ; add mantissae and normalize
            ;subtract smaller mantissa and normalize
:ROMB8A7    ; set Y to the other accumulator
            SET Y, FACEXP
            IFN X, ARGEXP
                SET Y, ARGEXP
:ROMB8AF    SUB A, [ARGOV]
			SET [FACOV], A
            SET A, [Y+2]
            SBX A, [X+2]
            SET [FACHO+1], A
            SET A, [Y+1]
            SBX A, [X+1]
            SET [FACHO], A
            ; dcpu: 84 e333 3334 - 84 bccc cccd
; ----------------------------------------------------------------------------
; NORMALIZE VALUE IN FAC
; ----------------------------------------------------------------------------
:NORMALIZE_FAC1
:ROMB8D2    
			IFU EX, 0 ; one borrow left to do
                JSR COMPLEMENT_FAC
:NORMALIZE_FAC2
:ROMB8D7    SET Y, 0
:ROMB8D9    SET A, Y
:ROMB8DA    SET C, 0
:ROMB8DB    ; if highest 8 bits of mantissa are
            ; clear, we shift them left here.
            IFB [FACHO], 0xFF00   ; any bits set
                JMP NORMALIZE_FAC4  ; skip to single-bit
:ROMB8DF    SHL [FACHO], 8
            SHL [FACHO+1], 8
            BOR [FACHO], EX
            SHL [FACOV], 8
            BOR [FACHO+1], EX
            ;SHL Y, 8
            ;BOR [FACOV], Y
            ADD A, 8
            IFN A, 32  
                JMP ROMB8DB
        	; we've tried moving 32 bits  
            ; give up, return 0
:ZERO_FAC
:ROMB8F7    SET [FACEXP], 0
            SET [FACSGN], 0
            SET Z, 1
            RTS
:FADD4 ; add fractions then normalize 
; At this point the routine at B999 has
; brought the mantissa in ARGHO to the same
; exponent as FACHO, so we can add them here.
:ROMB8FE
            ADD A, [ARGOV]
            SET [FACOV], A
            ADX [FACHO+1], [ARGHO+1]
            ADX [FACHO], [ARGHO]
            
:ROMB91A    IFE EX, 0
                RTS
            JMP ROMB938 ; Increase exponent to make room
            
; postshift
:NORMALIZE_FAC3
:ROMB91D    ADD A, 1
            SHL [FACHO], 1
            SHL [FACHO+1], 1
            BOR [FACHO], EX
            SHL [FACOV], 1
            BOR [FACHO+1], EX
:NORMALIZE_FAC4
:ROMB929    IFC [FACHO], 0x8000   
                JMP ROMB91D
            ; mantissa is normalized
:ROMB92B    SUB A, [FACEXP]
            AND A, 0xFF
:ROMB92E    IFE EX, 0 ; no borrow involved
                JMP ZERO_FAC ; zero FACEXP and FACSGN, return
:ROMB930    XOR A, -1
            ADD A, 1
            AND A, 0xFF
:ROMB934    SET [FACEXP], A
:ROMB936    
            IFC A, 0xFF00 ; this BCC is probably unconditional
                RTS
    ; Add 1 to exponent, and 
    ; shift manitssa to the right by 1.
    ; this leaves the value as a whole unchanged.
    ; carry bit is always set
:ROMB938    ;SET C, EX
            ADD [FACEXP], 1
            IFG [FACEXP], 0xFF
                JMP OVERR
            SHR [FACOV], 1
            SHR [FACHO+1], 1
            BOR [FACOV], EX
            SHR [FACHO], 1
            BOR [FACHO+1], EX
            ;IFN C, 0
                BOR [FACHO], 0x8000
            RTS

:COMPLEMENT_FAC  ; replace FAC1 with its 2s-complement
:ROMB947    XOR [FACSGN], -1 
:COMPLEMENT_FAC_MANTISSA
:ROMB94D    XOR [FACHO], -1
            XOR [FACHO+1], -1
            XOR [FACOV], 0xFFFF
            ADD [FACOV], 0x0001
            ADX [FACHO+1], 0
            ADX [FACHO], 0
            RTS

; increment fraction  only
:ROMB96F    ADD [FACHO+1], 1
            ADX [FACHO], 0
            RTS

:OVERR ; Print overflow error message
:ROMB97E
            SET X, 0x0F
            JMP ERROR

   ; 01 too many files
   ; 02 file open
   ; 03 file not open
   ; 04 file not found
   ; 05 device not present
; debugging help
:OVERR1 ; too many files
	SET X, 1
	JMP ERROR
:OVERR2 ; file open
	SET X, 2
	JMP ERROR
:OVERR3 ; too many files
	SET X, 3
	JMP ERROR
:OVERR4 ; file open
	SET X, 4
	JMP ERROR
:OVERR5 ; file open
	SET X, 5
	JMP ERROR
:OVERR6 ; file open
	SET X, 6
	JMP ERROR
:OVERR7 ; file open
	SET X, 7
	JMP ERROR

; I think this routine inserts a byte at the beginning
; of the mantissa, using BITS as a high overflow byte.
:SHIFT_RIGHT1
:ROMB983   SET X, RESHO-1 ; RESHO has no exponent
            ; shift 8 bits from
           ; [BITS] -> mantissa -> [FACOV]
:SHIFT_RIGHT2
:ROMB985    SHR [FACOV], 8
            SHR [X+2], 8
            BOR [FACOV], EX
            SHR [X+1], 8
            BOR [X+2], EX
            ASR [BITS], 8
            BOR [X+1], EX
:SHIFT_RIGHT          
        ; set X to pointer to FACEXP or ARGEXP before calling here
        ; set A to the actual exponent therein
:ROMB999    ADD A, 8
:ROMB99B    IFB A, 0x80    ; BMI
                JMP SHIFT_RIGHT2 ; make 8 more high bits
            IFC A, 0xFF    ; BEQ with low 8 bits
                JMP SHIFT_RIGHT2
:ROMB99F    SUB A, 8  ; back to normal
            SET Y, A ; pass exponent to Y
:ROMB9A2    SET A, [FACOV]  ;FAC overflow
            IFC Y, 0x80 ; SBC #8 resulted in carry set
                JMP SHIFT_RIGHT5
:ROMB9A6    ASR [X+1], 1
            ; might jump here if X+1 is already shifted
; ----------------------------------------------------------------------------
; ENTER HERE FOR SHORT SHIFTS WITH NO SIGN EXTENSION
; ----------------------------------------------------------------------------
:SHIFT_RIGHT4
:ROMB9B0    SET C, EX
            SHR [X+2], 1          
:ROMB9B0A   BOR [X+2], C
:ROMB9B6    SET C, EX
            SHR A, 1   ; overflow byte
            BOR A, C   ; etc
            
:ROMB9B7    ADD Y, 1    ; add one to exponent diff
            IFB Y, 0x00FF  ; low bits not equal 0
                JMP ROMB9A6  ; do magic bits again
:SHIFT_RIGHT5
:ROMB9BA    SET C, 0
            SET EX, 0
            RTS  ; A contains 0x00 through 0xFF00
                    ; as fetched from FACOV?




                    
; FP constant ONE
; default STEP for FOR statement
:FONE
:ROMB9BC  DAT 0x81, 0, 0

:POLY_LOG
          DAT 0x03   ; degree 4
          DAT 0x7F,0x5E56,0xCB79
          DAT 0x80,0x139B,0x0B64
          DAT 0x80,0x7638,0x9316
          DAT 0x82,0x38AA,0x3B20


:CON_SQR_HALF
:ROMB9D6  DAT 0x80,0x3504,0xF334   ; 0,5 * SQR(2)
:CON_SQR_TWO
:ROMB9DB  DAT 0x81,0x3504,0xF334   ; SQR(2)
:CON_NEG_HALF
:ROMB9E0  DAT 0x80,0x8000,0x0000   ; -1/2
:CON_LOG_TWO
:ROMB9E5  DAT 0x80,0x3172,0x17F8   ; LOG(2)
:CON_INVSQRT
          DAT 0x5F, 0x3759, 0xDF00 ; magic number

; for double it's 0x5fe6eb50c7b537a9
; see http://www.lomont.org/Math/Papers/2003/InvSqrt.pdf

; FAC1 <- ln FAC1
:LOG
        JSR SIGN
        IFE A, 0   ;can't LOG(0)
            JMP FCERR
        SET A, [FACEXP]
        SUB A, 0x80
        SET PUSH, A  ; store log 2 of FAC
        SET [FACEXP], 0x80
        SET A, CON_SQR_HALF
        JSR FADD              ;BA01
        SET A, CON_SQR_TWO    ;BA04
        JSR FDIV              ;BA08
        SET A, FONE           ;BA0B
        JSR FSUB              ;BA0F
        SET A, POLY_LOG
        JSR POLYNOMIAL_ODD
        SET A, CON_NEG_HALF
        JSR FADD
        SET A, POP  ; retrieve log 2 of FAC
        JSR ADDACC  
        SET A, CON_LOG_TWO
        ; fall through to FMULT


; FAC1 <- FAC1 * FAC2
:FMULT
:ROMBA28    JSR CONUPK
; times operator
:FMULTT
:ROMBA2B    IFE A, 0  
                RTS ; anything times 0 = 0
            JSR ADD_EXPONENTS ; add exponents and handle signs
:ROMBA3D    SET A, [FACOV]
            JSR ROMBA59  ; multiply A and add to RESHO
            SET A, [FACHO+1]
            JSR ROMBA59  
            SET A, [FACHO]
            JSR ROMBA59
            JMP ROMBB8F  ; ???

            ; BYTE-SHIFT ADDER
:ROMBA59    SET [FACOV], [RESHO+1]
            SET [RESHO+1], [RESHO]
            SET [RESHO], 0
            SET Y, A
            MUL Y, [ARGHO]
            SET X, EX
            ADD [RESHO+1], Y
            ADX [RESHO], X
            SET Y, A
            MUL Y, [ARGHO+1]
            SET X, EX
            ADD [FACOV], Y
            ADX [RESHO+1], X
            ADX [RESHO], 0
            RTS

; move float indexed by AY into second float accu
; return FAC1's exponent in A to enable special
; zero-handling and a few shortcuts.
:CONUPK
:ROMBA8C    SET [INDEX], A
:ROMBA8CI   SET [ARGEXP], [A]
            SET B, [A+1]
            SET [ARGHO], B
            BOR [ARGHO], 0x8000 ; high bit is always 1
            ASR B, 15  ; pure 0 or -1
            SET [ARGSGN], B
            XOR B, [FACSGN]
            ASR B, 15   ; pure 0 or -1
            SET [SGNCPR], B
            SET [ARGHO+1], [A+2]
            SET A, [FACEXP]
            RTS
; Add exponents of ARG and FAC
; (called by FMULT and FDIV)
; also check for overflow and set result sign
:ADD_EXPONENTS
:ROMBAB7    SET A, [ARGEXP]
:ADD_EXPONENTS1
:ROMBAB9    IFE A, 0
                JMP ZERO ; pop return address and set FAC=0
            ADD A, [FACEXP]
            IFC A, 0x100 ; BCC
                JMP ROMBAC4
            IFB A, 0x80  ; BMI but carry set
                JMP OVERR
:ROMBAC2    AND A, 0xFF ; CLC
            JMP ROMBAC6  ; .BY $2C to jump next BPL 
:ROMBAC4    IFC A, 0x80 ; BPL
                JMP ZERO  ; return FAC=0
:ROMBAC6    ADD A, 0x80
            AND A, 0xFF
            SET [FACEXP], A
            IFE A, 0
                JMP ROMB8FB ; set FACSGN too and RTS.
            ; put comparison result into SGNCPR
:ROMBACF    SET [FACSGN], [SGNCPR]
:ROMBAD3    RTS
:ROMB8FB    SET [FACSGN], A
            RTS
; if FAC is positive, give OVERR
; if FAC is negative, set FAC=0, pop one return,
; and RTS.  called from EXP function
:OUTOFRNG
:ROMBAD4    SET A, [FACSGN]
            XOR A, -1
            IFU A, 0
                JMP OVERR
; pop return address and set FAC=0
:ZERO
:ROMBADA    SET A, POP ; don't return to FMULT/FDIV.
            JMP ZERO_FAC ; zero FACEXP and FACSGN

; This quick-multiplies FAC by 10 through bit shifting
; and addition.
:MUL10
:ROMBAE2    JSR COPY_FAC_TO_ARG_ROUNDED ; ARG = FAC
:ROMBAE5    IFE A, 0   ; (A is [FACEXP] now) 
                RTS ; 0 * 10 = 0
:ROMBAE9    ADD A, 2  ; ARG *= 4 by increasing exponent
            IFB A, 0x100
                JMP OVERR
:ROMBAED    SET [SGNCPR], 0
            JSR FADD2  ; FAC += ARG
            ADD [FACEXP], 1  ; FAC *= 2
            IFB [FACEXP], 0x100
                JMP OVERR
            RTS

; special case in FDIV, set result m=1
:MOV1HO
    SET [FACHO], 0x8000
    SET [FACHO+1], 0
    SET [FACOV], 0
    RTS

; reciprocal multiplier for 1/10 for parsing float literals
:DIV10
        SUB [FACEXP], 3 ; FAC <- FAC / 16
        JSR COPY_FAC_TO_ARG_ROUNDED       ; ARG = FAC
        SET [SGNCPR], 0
; reciprocal multiplier for 1/1.25 (5, 10, 20, etc)
:DIV125
        SET X, 0xCCCC
        JMP MULRPT      ; FAC <- ARG * 8/5

; reciprocal multiplier for 1/1.5 (3, 6, 12, etc)
:DIV15
        SET X, 0xAAAA
        JMP MULRPT      ; FAC <- ARG * 4/3
; optimized multiplier for repeating bytes
; accepts mantissa in X
:MULRPT
        SET A, [ARGHO]
        MUL A, X
        SET Y, EX
        SET [FACHO], Y
        ADD Y, A  ; carry bit one place to the right
        SET A, EX
        ADD [FACHO], A
        ADD Y, A ; carry bit to the infinite-right
        SET [FACHO+1], Y
        SET [FACOV], Y
        SET A, [ARGHO+1]
        IFN A, 0
           JMP MULRPT2 ; Multiply next word of ARG
        SET Y, A
        JMP MULRPT3 ; Add rounding bit and return.
:MULRPT2        ; Multiply ARGHO+1 by same thing
        MUL A, X
        SET Y, EX
        ADD [FACHO+1], Y
        ADX [FACHO], 0
        ADD Y, A ; carry bit one place to the right
        SET A, EX
        ADD [FACHO+1], A
        ADX [FACHO], 0
        ADD Y, A ; carry bit to the infinite-right
:MULRPT3        ; Add rounding bit
        SET EX, 1
        ADX [FACOV], Y
        ADX [FACHO+1], 0
        ADX [FACHO], 0
        JMP NORMALIZE_FAC2 ; normalize

; FAC <- FAC / ARG
:DIV
:ROMBB07    SET [SGNCPR], 0
            JSR MOVFM
            JMP FDIVT

; FAC <- ARG / FAC
:FDIV
:ROMBB0F    JSR CONUPK ; copy to ARG; should return exp in A
:FDIVT
:ROMBB12    IFE A, 0  ; dividing by 0?
                JMP ROMBB8A ; oh shi--
            JSR ROUND
            SET A, 0
            SUB A, [FACEXP]
            AND A, 0xFF
            SET [FACEXP], A
            JSR ADD_EXPONENTS ; add exponents
            ; works fine up to here
            ADD [FACEXP], 1
            IFC [FACEXP], 0xFF
                JMP OVERR
            ; special case
            SET X, [FACHO]
            SET Y, [ARGHO]
            ; if dividend and divisor have same mantissa,
            ; set result's mantissa to 1.0 and return
            IFE X, Y
                IFE [FACHO+1], [ARGHO+1]
                    JMP MOV1HO
            IFN [FACHO+1], 0 ; no chance of reciprocal shortcut
                JMP FDIVFULL
            ; m=0; copy mantissa
            IFE X, 0x8000
                IFE [FACHO+1], 0
                    JMP MOVFAHO
            ; Reciprocal dividers disabled; they're not
            ; accurate enough for fizzbuzz (though they're
            ; still used for decimal rendering)
            ; multiply by reciprocal for m=1.25 (5, 10, 20)
            IFE X, 0xA000
                JMP DIV125
            ;; multiply by reciprocal for m=1.5 (3, 6, 12)
            IFE X, 0xC000
                JMP DIV15
            ; all others, do a 16-bit integer divide.  This
            ; will hold us over until we can do this properly.  
            ; we can squeak out a few more bits of accuracy
            ; on a small divisor by denormalizing it.
:FDIVFULL
            SET [FACOV], 0
            ADD [FACEXP], 15
            IFB X, 0x00FF ; Top 8 bits clear
                JMP FDIVDEN1
            SHR X, 8
            SUB [FACEXP], 8
:FDIVDEN1
            IFB X, 1 ; Top bit clear
                JMP FDIVGO
:FDIVDEN2   SHR X, 1
            SUB [FACEXP], 1
            IFC X, 1
                JMP FDIVDEN2
:FDIVGO
            DIV Y, X    ; ARGHO = ARGHO / FACHO
            SET [FACHO+1], EX
            SET [FACHO], Y
            JMP NORMALIZE_FAC2

            SET X, EX   ; X = right of decimal point

            ; here's the original division routine, it started
            ; where the special case section is now.
            ; it was close to working
            SET X, RESHO-1 ; pointer, will be incremented before storage
            SET A, 1 ; "stop" bit -- when this 1 goes off the
                     ; left side of A, we store the byte to [X+1].
:ROMBB29    SET Y, [ARGHO]
            SUB Y, [FACHO]
            IFN Y, 0
                JMP ROMBB3F
            SET Y, [ARGHO+1]
            SUB Y, [FACHO+1]
            ;6502: Carry bit is set if ARGHO >= FACHO
            ; or if the shift-left at ROMBB4F
            ; resulted in an overflow
            ;DCPU: EX = -1 represents carry clear case
            ;      EX = 0 represents carry set case
:ROMBB3F    SET B, EX ; simulate PHP
            SHL A, 1  ; shifting part of ROL
            SET C, EX ; store carry bit off left side of A
            IFE B, 0
                BOR A, 1 ; carry-add part of ROL
            IFE C, 0 ; no bit fell off off left side
                JMP ROMBB4C
            ; the 1 that is in C is the guard bit we initially
            ; set in A. this means we have enoiugh bits in A
            ; to store a byte into RESHO now.
            ADD X, 1  
            SET [X], A
:FDIVTEST   ;SET B, [FACEXP]
            ;DAT 0x5800
            IFE X, RESHO+1
                JMP ROMBB7A  ; just do two more bits
            IFG X, RESHO+1
                JMP ROMBB7E  ; Store last two bits in [FACOV]
            SET A, 1 ; "stop" bit of next byte
:ROMBB4C    SET EX, B   ; pop ARGHO-FACHO compare again
:ROMBB4D    IFE EX, 0     ; ARGHO >= FACHO
                JMP ROMBB5D  ; Subtract FACHO from ARGHO
            ; subtract 1 at left of ARGHO and re-normalize.
:ROMBB4F    SHL [ARGHO], 1
            SET C, EX
            SHL [ARGHO+1], 1
            BOR [ARGHO], EX
            IFN C, 0 ; bit fell off left side of mantissa
                JMP ROMBB3F ; rotate A left too.
            IFN EX, 0 ; ARG is normalized
                JMP ROMBB29 ; go to beginning of compare loop
            JMP ROMBB3F
            ; originally BB5D preserved  A. 
:ROMBB5D    SUB [ARGHO+1], [FACHO+1]
            SBX [ARGHO], [FACHO]
            JMP ROMBB4F
:ROMBB7A    SET A, 0x4000 ; "stop" bit of FACOV
            JMP ROMBB4C
:ROMBB7E    SHL A, 14 ; originally 6
            SET [FACOV], A
            SET EX, B  ; PLP?
            JMP ROMBB8F
:ROMBB8A    SET X, 0x14  ; OH SHI--
            JMP ERROR
:ROMBB8F    SET [FACHO], [RESHO]
            SET [FACHO+1], [RESHO+1]
            JMP NORMALIZE_FAC2 ; normalize before returning
            
    

; Move a FP number from memory pointer in A to FAC1
:MOVFM
:ROMBBA2    SET [FACEXP], [A]
            SET Y, [A+1]
            SET [FACHO], Y
            SET [FACHO+1], [A+2]
            ASR Y, 15
            SET [FACSGN], Y
            BOR [FACHO], 0x8000
            SET [FACOV], 0
            RTS


; Move a FP number from FAC1 to various points in memory
:MOV2F
:STORE_FAC_IN_TEMP2_ROUNDED
:ROMBBC7
            SET X, FACTEMP2    ; arithmetic register #4
            IFE X, 0 ; skip next
:STORE_FAC_IN_TEMP1_ROUNDED
:ROMBBCA    SET X, FACTEMP1    ; arithmetic register #3
            IFE X, 0 ; skip next
:ROMBBD0    SET X, [FORPNT] ; assign to a BASIC variable
:STORE_FAC_AT_X_ROUNDED
:ROMBBD4    JSR ROUND
            ; using X instead of $22
            SET [X+2], [FACHO+1] ; low mantissa
            SET A, [FACSGN]
            BOR A, 0x7FFF 
            AND A, [FACHO] ; high mantissa combined with sign bit
            SET [X+1], A
            SET [X], [FACEXP]
            SET [FACOV], 0 
            ; uncomment if something relies on INDEX
            SET [INDEX], X
            RTS
            
; Move from FAC2 to FAC1
:MOVFA
:COPY_ARG_TO_FAC
:ROMBBFC    SET [FACSGN], [ARGSGN]
            SET [FACEXP], [ARGEXP]
:MOVFAHO
            SET [FACHO+1], [ARGHO+1]
            SET [FACHO], [ARGHO]
            SET [FACOV], 0
            RTS

:COPY_FAC_TO_ARG_ROUNDED
:ROMBC0C    JSR ROUND
:MOVEF
:MAF
:ROMBC0F    SET [ARGSGN], [FACSGN]
            SET [ARGHO+1], [FACHO+1]
            SET [ARGHO], [FACHO]
            SET [ARGEXP], [FACEXP]
            SET [FACOV], 0
            ; Preserve side effect in 6502 version
            SET A, [FACEXP]
            RTS

; Round Accumulator #1 by Adjusting the Rounding Byte
:ROUND
:ROMBC1B    IFE [FACEXP], 0
                RTS
            IFA [FACOV], -1
                RTS 
            ; high bit of FACOV is set, increment mantissa
:INCREMENT_MANTISSA
:ROMBC23    ADD [FACHO+1], 1
            ADX [FACHO], 0  
            IFE EX, 0
                RTS
            ; mantissa overflowed;
            ; shift it to the right
            JMP ROMB938
            
; get sign of float accu in A
:SIGN
:ROMBC2B    SET A, [FACEXP]
            IFE A, 0
                RTS
:ROMBC2F    SET A, [FACSGN]
; Return pure +1 or -1.
:ROMBC31    ASR A, 15
            BOR A, 1
            RTS

; SGN function
:SGNFUNC
:ROMBC39    JSR SIGN

:GIVAYF ;Convert 16-Bit Signed Integer to Floating Point
:ROMB391    SET [VALTYP], 0   ; numeric
            SET [FACHO], A
            SET X, 0x90 ; FAC < 32768
            JMP FLOAT1 ; normalize
            
; Move signed 8-bit number from A into float

:FLOAT
:ROMBC3C
            SHL A, 8
            SET [FACHO], A ; Mantissa
            SET [FACHO+1], 0
            SET X, 0x88      ; FAC < 128
:FLOAT1
:ROMBC44 ; EX contains sign of number
            SET EX, A
; Zero out rest of FACHO
:ROMBC49
            SET [FACEXP], X
            SET [FACHO+1], 0
            SET [FACOV], 0
            SET [FACSGN], 0
            JMP NORMALIZE_FAC1
            ;... does a COMPLEMENT_FAC if EX = -1,
            ; then normalizes the thing

; ABS FUNCTION
:ROMBC58
        SET [FACSGN], 0
        RTS
            
; ----------------------------------------------------------------------------
; COMPARE FAC WITH PACKED # AT (Y,A)
; RETURN A=1,0,-1 AS (Y,A) IS <,=,> FAC
; ----------------------------------------------------------------------------
:FCOMP
:ROMBC5B    SET I, A    ; I points to exponent
:FCOMPI
:ROMBC5D    SET X, [I]  ; X contains exponent
:ROMBC63    ADD I, 1    ; I points to mantissa
            IFE X, 0
                JMP SIGN    ; return sign of FAC1?
:ROMBC67    SET A, [I]  ; A contains mantissa 
            XOR A, [FACSGN]
            IFU A, 0        ; Comparing different signs
                JMP ROMBC2F ; return sign of FAC1.
            ; Compare exponent, then mantissa
            ; First byte that's different,
            ; return comparison of that byte.
:ROMBC6D    SUB X, [FACEXP]
            IFN X, 0  ; different exponents
                JMP ROMBC92
            SET X, [I]
            BOR X, 0x8000
            SUB X, [FACHO]
            IFN X, 0
                JMP ROMBC92
            SET X, [I+1]
            SUB X, [FACHO+1]
            IFN X, 0
                JMP ROMBC92
            ; try subtraction again,
            ; this time with the rounding bit
:ROMBC88    SET EX, 0
            IFU [FACOV], 0
                SET EX, -1
:ROMBC8C    SET X, [I+1]

            SBX X, [FACHO+1]
            SET A, 0
:ROMBC90    IFE X, 0
                RTS ; give up on comparison?
; in 6502, C=1 meant memory was higher..
; in DCPU-16, EX=0 meant memory was higher.
:ROMBC92    SET A, [FACSGN]
            IFE EX, 0 ; no underflow in the subtract
                XOR A, -1
            JMP ROMBC31 ; return pure 0 or 1

; ----------------------------------------------------------------------------
; QUICK INTEGER FUNCTION
;
; CONVERTS FP VALUE IN FAC TO INTEGER VALUE
; IN FAC+1...FAC+4, BY SHIFTING RIGHT WITH SIGN
; EXTENSION UNTIL FRACTIONAL BITS ARE OUT.
;
; THIS SUBROUTINE ASSUMES THE EXPONENT < 32.
; ----------------------------------------------------------------------------

:QINT
:ROMBC9B    SET A, [FACEXP]
            IFE A, 0
                JMP QINT3 ; clear mantissa and return
:ROMBC9F    SUB A, 0xA0
            IFA [FACSGN], -1
                JMP ROMBCAF ; skip two lines if positive
            SET [BITS], -1  ; shift 1s from the left instead of 0s
            JSR COMPLEMENT_FAC_MANTISSA
:ROMBCAF    SET X, FACEXP ; pointer
            IFA A, -8   ; CMP 0xF9
                JMP QINT2
            JSR SHIFT_RIGHT  ; shift by 8 bits
            SET [BITS], 0
:ROMBCBA    RTS

:QINT2
:ROMBCBB    SET Y, A
            SET A, [FACSGN]
            AND A, 0x8000
            SHR [FACHO], 1
            BOR [FACHO], A
            SET A, 0
            JSR SHIFT_RIGHT4 ; shift rest of FACHO to the right
            SET [BITS], 0
:ROMBCCB    RTS

:INT
:ROMBCCC    IFG [FACEXP], 0xA0  ; too big to have decimal places
                RTS
            JSR QINT
            SET [FACOV], 0
            SET EX, 0
            IFU [FACSGN], 0
                SET EX, -1
            SET [FACSGN], 0
            SET [FACEXP], 0xA0 ; exponent is 2**32
            SET [RAM07], [RAM64]
            JMP NORMALIZE_FAC1 ; re-normalize
 
:QINT3               
:ROMBCE9    SET [FACHO], 0
            SET [FACHO+1], 0
            RTS

:DECFLG    DAT 0
:FIN ; convert ascii string to a float in FAC1
:ROMBCF3    SET Y, 0
            SET X, 0
            SET [DECEXP], 0
            SET [RAM5E], 0
            SET [DECFLG], 0
            SET [RAM60], 0
            SET [FACEXP], 0
            SET [FACHO], 0
            SET [FACHO+1], 0
            SET [FACSGN], 0
            ; parse result from last CHRGET
            IF_IS_DIGIT
                JMP FIN9
            IFE A, 0x2D ; minus
                SET X, -1
            SET [SGNFLG], X
:ROMBD06    IFN A, 0x2B ; plus sign
                JMP ROMBD0F 
            ; get digit after +/-
:ROMBD0A    JSR CHRGET
            ; parse a digit
:ROMBD0D    IF_IS_DIGIT
                JMP FIN9
:ROMBD0F    IFE A, 0x2E ; decimal point
                JMP ROMBD41
            IFN A, 0x45 ; E
                JMP ROMBD47
            ; handle exponent notation
            JSR CHRGET
            IF_IS_DIGIT
                JMP GETEXP
            IFE A, BASIC_MINUS_TOKEN
                JMP ROMBD2E
            IFE A, 0x2D  ; minus
                JMP ROMBD2E
            IFE A, BASIC_PLUS_TOKEN
                JMP ROMBD30
            IFE A, 0x2B  ; plus
                JMP ROMBD30
            JMP ROMBD35
            ; handle negative exponent
:ROMBD2E    SET [RAM60], -1
:ROMBD30    JSR CHRGET
            IF_IS_DIGIT
                JMP GETEXP
:ROMBD35    IFN [RAM60], -1 ; bpl
                JMP ROMBD47
            SET A, 0
            SUB A, [RAM5E]
            JMP ROMBD49 ; subtract DECEXP and use.
            ; handle decimal point
:ROMBD41    SHR [DECFLG], 1       ; move existing decimal point flag
            BOR [DECFLG], 0x8000  ; set decimal point flag
            IFC [DECFLG], 0x4000 ; if decimal point flag already set
                JMP ROMBD0A ; ignore second decimal
            ; Normalize number with E+xx/E-xx notation.
:ROMBD47    SET A, [RAM5E]   
:ROMBD49    SUB A, [DECEXP]
            SET [RAM5E], A
            IFE A, 0
                JMP ROMBD62 ; no decimal point
            IFA A, -1 ; BPL
                JMP ROMBD5B
            ; Decimally denormalize number.
            ; RAM5E is the decimal exponent
            ; DECEXP is the number of digits to the right 
:ROMBD52    JSR DIV10
            ADD [RAM5E], 1
            IFN [RAM5E], 0
                JMP ROMBD52
            JMP ROMBD62
:ROMBD5B    JSR MUL10
            SUB [RAM5E], 1
            IFN [RAM5E], 0
                JMP ROMBD5B
:ROMBD62    SET A, [SGNFLG]
            IFU A, 0  ; BMI
                JSR NEGOP ; originally via BD67
            RTS
; handle a digit
:FIN9
:ROMBD6A    SET PUSH, A
            IFB [DECFLG], 0x8000 ; after a decimal point
                ADD [DECEXP], 1 ; increase decimal exponent
            JSR MUL10  ; multiply decimal mantissa regardless
            SET A, POP
            SUB A, 0x30 ; zero
            JSR ADDACC  ; add contents of digit
            JMP ROMBD0A ; go to next character
            
; Add A to FAC
:FINLOG
:ADDACC
:ROMBD7E    SET PUSH, A
            JSR COPY_FAC_TO_ARG_ROUNDED ; ARG = FAC
            SET A, POP
            JSR FLOAT  ; move A into FAC
            
            SET A, [ARGSGN]
            XOR A, [FACSGN]
            ASR A, 15
            SET [SGNCPR], A
            SET A, [FACEXP]
            JMP FADDT

; get E+xx/E-xx exponent from string
:GETEXP
:ROMBD91    SET A, [RAM5E] ; number of received digits
            IFL A, 10
                JMP ROMBDA0
            SET A, 100
            IFB [RAM60], 0x8000 ; BMI
                JMP ROMBDAE
            JMP OVERR ; ?OVERFLOW ERROR
            ; i think this handles E+XX
:ROMBDA0    MUL A, 10
            AND A, 0xFF ; simulate some 8-bit quirk
            SET Y, [RAM7A]
            ADD A, [Y]
            SUB A, 0x30 ; un-digit
:ROMBDAE    SHL A, 8 ; 8-bit quirk simulation
            ASR A, 8 ; continues
            SET [RAM5E], A
            JMP ROMBD30 ; get next expodigit
            
; constants for float to string conversion
:CON_99999999_9   DAT 0x9B, 0x3EBC, 0x1FFD
:CON_999999999    DAT 0x9E, 0x6E6B, 0x27FD
:CON_BILLION      DAT 0x9E, 0x6E6B, 0x2800


; print "IN <line>"
:INPRT
:ROMBDC2    SET A, ROMA371
            JSR STROUT ;originally ROMBDDA
            SET A, [CURLIN]

; print number from A (AX in original)

:LINPRT
:ROMBDCD    SET [FACHO], A
            SET X, 0x90 ;  exponent = 2^16
            SET EX, 0 ; don't run COMPLEMENT_FAC from B8D2
            JSR ROMBC49  ; normalize this
            SET Y, FLOATSTR ; Cut off initial space
            JSR FOUT1     ; convert float to string
            JMP STROUT   ; print
   
; location for floating point result
:FLOATSTR   
            DAT 0x20
            .FILL 32 0x20
; output floating point number in FAC1
:FOUT
:ROMBDDD    SET Y, FLOATSTR+1
:FOUT1
:ROMBDDF    ;SET [FACOV], 0 ; TODO: cleanup
            SET A, 0x20    ; SPACE
            IFU [FACSGN], 0  ; negative
                SET A, 0x2D ; MINUS
            SET [Y], A
            SET [FACSGN], 0 ; was A -- 
            SET [RAM71], Y
            ADD Y, 1
            SET A, 0x30 ; zero
            SET X, [FACEXP]
            IFE X, 0
                JMP ROMBF04  ; write zero and exit
:ROMBDF8    SET A, 0
            IFE X, 0x80 ; exponent is a 1
                JMP ROMBE00
            IFG X, 0x80 ; number > 1
                JMP ROMBE09
; For numbers between 0 and 1, we skip 9 digits
:ROMBE00    SET A, CON_BILLION  ;  1,000,000,000
            JSR FMULT 
            SET A, -9 ; signify we've moved the decimal point
:ROMBE09    SET [DECEXP], A
:ROMBE0B    SET A, CON_999999999
            JSR FCOMP 
:ROMBE12    IFE A, 0  ; FAC1 = 999,999,999.5
                JMP ROMBE32
            IFC A, 0x8000 ; FAC1 > 999,999,999.5
                JMP ROMBE28 
:ROMBE16    SET A, CON_99999999_9
            JSR FCOMP
:ROMBE1D    IFE A, 0 ; fac1 = 99,999,999.5
                JMP ROMBE21 ; squeeze one more out
            IFC A, 0x8000 ; FAC1 > 99,999,999.5
                JMP ROMBE2F ; hundred millions digit done
:ROMBE21    ; Keep multiplying by 10 until
            ; the number is as big as we can print.
            JSR MUL10
            SUB [DECEXP], 1
            IFN [DECEXP], 0
                JMP ROMBE16
            ; Normalize downward.
:ROMBE28    JSR DIV10
:ROMBE2A    ADD [DECEXP], 1
            IFN [DECEXP], 1
                JMP ROMBE0B
            ; We've inflated our number, and the
            ; decimal exponent is in DECEXP.
:ROMBE2F    JSR FADDH ; add 0.5 to round the last visible digit
:ROMBE32    JSR QINT
:ROMBE35    SET X, 1
            SET A, [DECEXP]
            ADD A, 10
            IFB A, 0x8000 
                JMP ROMBE48
            IFG A, 10  ; More than 10 digits?
                JMP ROMBE48
:ROMBE42    ADD A, -1   
            SET X, A
            SET A, 2
:ROMBE48    SUB A, 2 
            SET [RAM5E], A
            ;Store the positive number of digits
            ; to the left of the decimal point.
            SET [DECEXP], X 
            SET A, X
            IFE A, 0
                JMP ROMBE53
            IFC A, 0x8000
                JMP ROMBE66
:ROMBE53    SET Y, [RAM71]   ; cached FLOATSTR index
            ADD Y, 1
            SET [Y], 0x2E ; decimal point
            IFE X, 0 ; 
                JMP ROMBE64
:ROMBE5E    ADD Y, 1    ; add 0 after decimal point
            SET [Y], 0x30
:ROMBE64    SET [RAM71], Y
:ROMBE66    SET B, FOUTBL
:ROMBE68    SET X, 0x80
:ROMBE6A    ADD [FACHO+1], [B+1]
:ROMBE6G    ADX [FACHO], [B]
            SET C, EX
            ADD X, 1  ; 6502: affects N, not C
            IFN C, 0
                JMP ROMBE8E
:ROMBE8A    IFC X, 0x80 ;BPL
                JMP ROMBE6A
            JMP ROMBE90 ;BMI
:ROMBE8E    IFB X, 0x80 ;BMI after BCD
                JMP ROMBE6A
:ROMBE90    SET A, X
            IFE C, 0
                JMP ROMBE97
:ROMBE93    XOR A, 0xFF
            ADD A, 12 ; ADC #10 when carry must bet set
:ROMBE97    ADD A, 0x2F ; ??
:ROMBE99    ADD B, 2 ; Try next divisor
            ;SET [VARPNT],B  ; Starting divisor cache
            SET Y, [RAM71] ; get cached string index
            ADD Y, 1
            SET X, A
:ROMBEA3    AND A, 0x7F
            SET [Y], A     ;Store a digit!
            SUB [DECEXP], 1
            IFN [DECEXP], 0 
                JMP ROMBEB2
            ADD Y, 1
            SET [Y], 0x2E ; Add decimal point
:ROMBEB2    SET [RAM71], Y ; Store string index
            ;SET B, [VARPNT]  ; Cached divisor value
            SET A, X
            XOR A, 0xFF
            AND A, 0x80  ; not sure the bittitude
            SET X, A
            IFE B, FOUTBLEND ; end of base 10 factors
                JMP ROMBEC4
            IFN B, FDCEND  ; end of TI$ factors
                JMP ROMBE6A  ; beginning of loop
:ROMBEC4    SET Y, [RAM71]
    ; shave off surplus decimal places
:ROMBEC6    SET A, [Y]
            SUB Y, 1
            IFE A, 0x30 ; zero
                JMP ROMBEC6
            IFE A, 0x2E ; decimal point
                JMP ROMBED3
            ADD Y, 1
:ROMBED3    SET A, 0x2B ; plus
            SET X, [RAM5E]
            IFE X, 0   ; Nothing more to print
                JMP ROMBF07
            ; What's in RAM5E anyway?
:ROMBED9    IFC X, 0x80 
                JMP ROMBEE3
:ROMBEDB    SET X, [RAM5E]
            MUL X, -1
            SET A, 0x2D ; minus
:ROMBEE3    SET [Y+2], A
            SET [Y+1], 0x45 ; E
:ROMBEEE    ; SEC
:ROMBEEF    SET A, X
            DIV A, 10
            ADD A, 0x30
            MOD X, 10
            ADD X, 0x30
            SET [Y+4], X
            SET [Y+3], A
            SET [Y+5], 0
            JMP ROMBF0C
:ROMBF04    SET [Y], A
:ROMBF07    SET [Y+1], 0
:ROMBF0C    SET A, FLOATSTR+1
            SET Z, 0
            RTS
            
; constant: 1/2 in floating point format
; for various rounding purposes.
:CON_HALF
:ROMBF11   DAT 0x80, 0x0000, 0x0000 

; divisors for decimal conversion
:FOUTBL
:ROMBF16   DAT 0xFA0A,0x1F00    ;-100,000,000
:ROMBF1A   DAT 0x0098,0x9680    ;  10,000,000
:ROMBF1E   DAT 0xFFF0,0xBDC0    ;  -1,000,000
:ROMBF22   DAT 0x0001,0x86A0    ;     100,000
:ROMBF26   DAT 0xFFFF,0xD8F0    ;     -10,000
:ROMBF2A   DAT 0x0000,0x03E8    ;        1000
:ROMBF2E   DAT 0xFFFF,0xFF9C    ;        -100
:ROMBF32   DAT 0x0000,0x000A    ;          10
:ROMBF36   DAT 0xFFFF,0xFFFF    ;          -1
:FOUTBLEND

; divisors for clock conversion (TI$)
:FDCEND
    DAT 0xFFDF,0x0A80 ;-2160000    10:00:00
    DAT 0x0003,0x4BC0 ;  216000     1:00:00
    DAT 0xFFFF,0x7360 ;  -36000       10:00
    DAT 0x0000,0x0E10 ;    3600        1:00
    DAT 0xFFFF,0xFDA8 ;    -600         :10
    DAT 0x0000,0x003C ;      60         :01 (1 second = 60 jiffies)
:FDCENDEND

; ----------------------------------------------------------------------------
; special case for X^2
; ----------------------------------------------------------------------------
:SQUARE
		JSR COPY_ARG_TO_FAC
		SET [ARISGN], 0
		JMP FMULTT
; ----------------------------------------------------------------------------
; SQR(X) = X^0.5
; ----------------------------------------------------------------------------
:SQR
        JSR COPY_FAC_TO_ARG_ROUNDED
        SET A, CON_HALF
        JSR MOVFM
        ; fall through to FPWRT
; ----------------------------------------------------------------------------
; EXPONENTIATION OPERATION
;
; ARG ^ FAC  =  EXP( LOG(ARG) * FAC )
; ----------------------------------------------------------------------------
:FPWRT
:ROMBF7B    IFE A, 0  ; ???
                JMP EXP
            IFE [ARGEXP], 0
                JMP ZERO_FAC ; return 0
			; special case for squares
            IFE A, 0x82
                IFE [FACHO+1], 0
                	IFE [FACHO], 0x8000
                		JMP SQUARE
            SET Y, 0
            SET X, FACTEMP3
            JSR STORE_FAC_AT_X_ROUNDED 
            SET A, [ARGSGN]
            IFA A, -1 ; BPL
                JMP ROMBF9E
            SET A, FACTEMP3
            JSR FCOMP
            IFE A, 0
                JMP ROMBF9E
            SET A, Y
            SET Y, [RAM07] 
:ROMBF9E    JSR COPY_ARG_TO_FAC ; TODO: original called ROMBBFE.
                    ; was this just for efficiency?
            SET PUSH, Y
            JSR LOG
            SET A, FACTEMP3
            JSR FMULT   ; FAC *= RAM4E
            JSR EXP ; EXP function
            SET A, POP
            ASR A, 1
            IFE EX, 0
                RTS
            

; MINUS operator
; Perform NOT and >
:NEGOP
:ROMBFB4    IFN [FACEXP], 0
                XOR [FACSGN], -1
            RTS

; float constants for EXP

; 1/LOG(2)
:CON_LOG_E
:ROMBFBF    DAT 0x81, 0x38aa, 0x3b29
; polynomial table
:POLY_EXP
:ROMBFC4    DAT 7 ; degree 8
            DAT 0x71,0x3458,0x3e56
            dat 0x74,0x167e,0xb31b
            dat 0x77,0x2fee,0xe385
            dat 0x7a,0x1d84,0x1c2a
            dat 0x7c,0x6359,0x580a
            dat 0x7e,0x75fd,0xe7c6
            dat 0x80,0x3172,0x1810
            dat 0x81,0,0

; EXP command
:EXP
:ROMBFED    SET A, CON_LOG_E  
            JSR FMULT
            SET A, [FACOV]
            ADD A, 0x5000
            IFN EX, 0
                JSR INCREMENT_MANTISSA
:ROME000    SET [ARGOV], A
            JSR MAF
            IFG [FACEXP], 0x87 ; FAC >= 256
:ROME00B        JSR OUTOFRNG
            JSR INT  ; does INT set [CHARAC]?
            SET A, [CHARAC]
            ADD A, 0x81
            IFC A, 0xFF ; BEQ in 8-bit
                JMP ROME00B
            SUB A, 1
            SET PUSH, A
            ; Swap FAC and ARG
            SET A, [FACEXP]
            SET X, [FACHO]
            SET Y, [FACHO+1]
            SET [FACEXP], [ARGEXP]
            SET [FACHO], [ARGHO]
            SET [FACHO+1], [ARGHO+1]
            SET [ARGEXP], A
            SET [ARGHO], X
            SET [ARGHO+1], Y
            SET [FACOV], [ARGOV]
            JSR FSUBT
            JSR NEGOP
            SET A, POLY_EXP ; EXP polynomial table
            JSR POLYNOMIAL
            SET [SGNCPR], 0
            SET A, POP
            JSR ADD_EXPONENTS1 ; add FAC/ARG exponents
            RTS    
            

; float numbers for RND
:ROME08D    DAT 0x98, 0x3544, 0x7A00  ; multiplied by this
:ROME092    DAT 0x68, 0x2881, 0x4600  ; added to this

; ----------------------------------------------------------------------------
; ODD POLYNOMIAL SUBROUTINE
;
; F(X) = X * P(X^2)
;
; WHERE:  X IS VALUE IN FAC
;    A POINTS AT COEFFICIENT TABLE
;    FIRST BYTE OF COEFF. TABLE IS N
;    COEFFICIENTS FOLLOW, HIGHEST POWER FIRST
;
; P(X^2) COMPUTED USING NORMAL POLYNOMIAL SUBROUTINE
; ----------------------------------------------------------------------------
:POLYNOMIAL_ODD
:ROME043    SET [RAM71], A
            JSR STORE_FAC_IN_TEMP1_ROUNDED
            SET A, FACTEMP1
            JSR FMULT
            JSR SERMAIN
            SET A, FACTEMP1
            JMP FMULT

; ----------------------------------------------------------------------------
; NORMAL POLYNOMIAL SUBROUTINE
;
; P(X) = C(0)*X^N + C(1)*X^(N-1) + ... + C(N)
;
; WHERE:  X IS VALUE IN FAC
;    A POINTS AT COEFFICIENT TABLE
;    FIRST BYTE OF COEFF. TABLE IS N
;    COEFFICIENTS FOLLOW, HIGHEST POWER FIRST
; ----------------------------------------------------------------------------
:POLYNOMIAL
:ROME059    SET [RAM71], A
:SERMAIN
:ROME05D    JSR STORE_FAC_IN_TEMP2_ROUNDED ; move FAC1 to FACTEMP2
            SET Y, [RAM71]
:ROME060    SET [SERLEN], [Y]
            ADD Y, 1
            SET [RAM71], Y
            SET A, Y
:ROME070    JSR FMULT
            SET A, [RAM71]
            ADD A, 3
            SET [RAM71], A
            JSR FADD
            SET A, FACTEMP2
            SUB [SERLEN], 1
            IFN [SERLEN], 0
                JMP ROME070
            RTS
            

; RND FUNCTION
:RND
:ROME097    JSR SIGN
            IFE A, -1
                JMP ROME0D3
            IFE A, 1
                JMP ROME0BE
            JSR ROMFFF3
            SET I, X
            ; apparently I'm supposed to re-seed
            ; FACHO with 32 bits of randomness
            ; coming from there... not sure
            ; what the best DCPU equialent is
            ; just yet.
:ROME0BE    SET A, RNDX ; RND seed in RAM
            JSR MOVFM
            SET A, ROME08D
            JSR FMULT
            SET A, ROME092
            JSR FADD
            ; jumble bytes
:ROME0D3    SET A, [FACHO]
            SET X, [FACHO+1]
            SHR A, 8
            BOR A, EX 
            SHL X, 8
            BOR X, EX
            SET [FACHO+1], A
            SET [FACHO], X
:ROME0E3    SET [FACSGN], 0
            SET [FACOV], [FACEXP]
            SET [FACEXP], 0x80 ; LET FAC < 1
            JSR NORMALIZE_FAC2 ; Normalize
            SET X, RNDX
:ROME0F6    JMP STORE_FAC_AT_X_ROUNDED 

; COS function
:COS
:ROME264    SET A, CON_PI_HALF
            JSR FADD
; SIN function
:SIN
:ROME26B    JSR COPY_FAC_TO_ARG_ROUNDED
            SET A, CON_PI_DOUB
            SET X, [ARGSGN]
            JSR DIV
            JSR COPY_FAC_TO_ARG_ROUNDED
            JSR INT   ; really?
            SET [SGNCPR], 0
            JSR FSUBT
; ----------------------------------------------------------------------------
; (FAC) = ANGLE AS A FRACTION OF A FULL CIRCLE
;
; NOW FOLD THE RANGE INTO A QUARTER CIRCLE
;
; <<< THERE ARE MUCH SIMPLER WAYS TO DO THIS >>>
; ----------------------------------------------------------------------------
            SET A, QUARTER
            JSR FSUB
            SET A, [FACSGN]
            SET PUSH, A
            IFA A, -1
            JMP SIN1
            JSR FADDH
            SET A, [FACSGN]
            IFU A, 0
                JMP ROME2A0
            XOR [RAM12], -1 ; ???
:SIN1
:ROME29D    JSR NEGOP
:ROME2A0    SET A, QUARTER
            JSR FADD
            SET A, POP
            IFA A, -1
                JMP ROME2AD
            JSR NEGOP
:ROME2AD    SET A, POLY_SIN
            JMP POLYNOMIAL_ODD

; TAN function
:TAN
:ROME2B4    JSR STORE_FAC_IN_TEMP1_ROUNDED
            SET [RAM12], 0
            JSR SIN
            SET X, [FACTEMP3]
            JSR ROME0F6 ; round FAC1 and move to X
            SET A, FACTEMP1
            JSR MOVFM   ; FAC1 <- FACTEMP1
            SET [FACSGN], 0
            SET A, [RAM12]
            JSR ROME2DC
            SET A, [FACTEMP3]
            JMP FDIV ; ohshi--

:ROME2DC    SET PUSH, A
            JMP SIN1

; ----------------------------------------------------------------------------
; "ATN" FUNCTION
; ----------------------------------------------------------------------------
:ATN
        SET A, [FACSGN]
    SET PUSH, A
    IFA A, -1
            JMP L3FDB
        JSR NEGOP
:L3FDB
        SET A, [FACEXP]
        SET PUSH, A
        IFL A, 0x81
            JMP L3FE9
        SET A, FONE
        JSR FDIV
; ----------------------------------------------------------------------------
; 0 <= X <= 1
; 0 <= ATN(X) <= PI/8
; ----------------------------------------------------------------------------
:L3FE9  SET A, POLY_ATN
        JSR POLYNOMIAL_ODD
        SET A, POP
        IFL A, 0x81
            JMP L3FFC
        SET A, CON_PI_HALF
        JSR FSUB
:L3FFC  SET A, POP
        IFU A, 0
            JMP NEGOP
        RTS

:POLY_ATN
        DAT 11
        DAT 0x76,0xB383,0xBDD3
        DAT 0x79,0x1EF4,0xA6F5
        DAT 0x7B,0x83FC,0xB010
        DAT 0x7C,0x0C1F,0x67CA
        DAT 0x7C,0xDE53,0xCBC1
        DAT 0x7D,0x1464,0x704C
        DAT 0x7D,0xB7EA,0x517A
        DAT 0x7D,0x6330,0x887E
        DAT 0x7E,0x9244,0x993A
        DAT 0x7E,0x4CCC,0x91C7
        DAT 0x7F,0xAAAA,0xAA13
        DAT 0x81,0x0000,0x0000

            
; float numbers for trigonometry

:CON_PI_HALF
:ROME2E0    DAT 0x81, 0x490F, 0xDAA2
:CON_PI_DOUB
:ROME2E5    DAT 0x83, 0x490F, 0xDAA2
:QUARTER    DAT 0x7F, 0, 0

; Polynomial table
:POLY_SIN
:ROME2EF    DAT 5  ; degree 6
            DAT 0x84, 0xE61A, 0x2D1B
            DAT 0x86, 0x2807, 0xfbf8
            DAT 0x87, 0x9968, 0x8901
            DAT 0x87, 0x2335, 0xdfe1
            DAT 0x86, 0xa55d, 0xe728
            DAT 0x83, 0x490f, 0xdaa2

:XFACCRASH  SET I, [X+1]
            SET J, [X+2]
            SET EX, [X]
            SET Z, [X+3]
            JMP CRASH
:FACCRASH   SET B, [ARGHO]
            SET C, [ARGHO+1]
            SET X, [ARGEXP]
            SET A, [ARGSGN]
            SET Y, [SGNCPR]
            SET I, [FACHO]
            SET J, [FACHO+1]
            SET EX, [FACEXP]
            SET Z, [FACSGN]
            ;SET Z, [FACOV]
            BOR 0xBCBC, 0xBCBC
            JMP CRASH