*pragmapush list
                *pragma list
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Arithmetic package
;
; This section contains routines that handle floating point and integer arithmetic.
;
; Most routines take a pointer to a value accumulator in X. Some take two pointers with the second in U.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Match operands for a numeric calculation. This works as follows:
;
; * If both operands are the same, ensure the type is numeric and return
; * If one operand is floating point, convert the other to floating point, as long as it is numeric
; * If one or both oeprands are not numeric, raise a type mismatch
; The operands are in (X) and (U)
val_matchtypes  ldb val.type,x                  ; get the type of first argument
                cmpb #valtype_int               ; is it integer?
                beq val_matchtypes0             ; brif so
                cmpb #valtype_float             ; is it floating point?
                beq val_matchtypes1             ; brif so
TMERROR         ldb #err_tm                     ; raise a type mismatch
                jmp ERROR
val_matchtypes0 ldb val.type,u                  ; get type of second operand
                cmpb #valtype_int               ; is it integer?
                bne val_matchtypes2             ; brif not
val_matchtypes3 rts
val_matchtypes2 cmpb #valtype_float             ; is it floating point?
                bne TMERROR                     ; brif not - raise error
                pshs u                          ; save pointer to second operand
                bsr val_int32tofp               ; convert first argument to floating point
                puls u,pc                       ; restore second operand pointer and return
val_matchtypes1 ldb val.type,u                  ; get second argument type
                cmpb #valtype_float             ; is it floating point?
                beq val_matchtypes3             ; brif so - we're good
                cmpb #valtype_int               ; is it integer?
                bne TMERROR                     ; brif not - invalid type combination
                pshs x,u                        ; save value pointers
                leax ,u                         ; convert (U) to floating point
                bsr val_int32tofp
                puls x,u,pc                     ; restore argument pointers and return
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Negate the 32 bit integer (for fp mantissa) at (X)
val_negint32    ldd zero                        ; subtract integer value from zero
                subd val.int+2,x
                std val.int+2,x
                ldd zero
                sbcb val.int+1,x
                sbca val.int,x
                std val.int,x
                rts
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Convert integer value at (X) to floating point value at (X). Enter at val_uint32tofp to treat the 32 bit value as
; unsigned. Otherwise enter at val_int32tofp to treat it as signed.
val_uint32tofp  clr val.fpsign,x                ; for positive sign
                bra val_int32tofpp              ; go process as positive
val_int32tofp   ldb val.int,x                   ; get sign to A
                sex
                sta val.fpsign,x                ; set sign of result
                bpl val_int32tofpp              ; brif positive - don't need to do a two's complement adjustment
                bsr val_negint32                ; negate the integer value
val_int32tofpp  ldb valtype_float               ; set result to floating point
                stb val.type,x
                ldb #0xa0                       ; exponent to have binary point to the right of the mantissa
                stb val.fpexp,x                 ; set the exponent
                clrb                            ; clear out extra precision bits
                ; fall through to normalize the value at (X)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Normalize floating point value at (X); this will shift the mantissa until there is a one in the leftmost
; bit of the mantissa. The algorithm is as follows:
;
; 1. Shift the mantissa left until a 1 bit is found in the high bit of the mantissa.
; 1a. If more than 40 bits of left shifts occur, determine that the value is zero and return
; 2. Adjust exponent based on number of shifts
; 2a. If new exponent went below -127, then underflow occurred and zero out value
; 2b. If new exponent went above +127, raise an overflow
; 3. If bit 7 of the extra precision byte is clear, return the resulting value
; 4. Add one to the mantissa
; 5. If a carry in (4) occurred, then set high bit of mantissa and bump exponent
; 6. If new exponent carries, then raise overflow
; 7. Return result.
;
; Note that if we carried in (4), the only possible result is that the mantissa
; rolled over to all zeroes so there is no need to shift the entire mantissa right
; nor is there any reason to check for additional rounding.
;
; The above algorithm has some optimizations in the code sequence below.
fp_normalize    pshs b                          ; save extra bits
                clrb                            ; set shift counter/exponent adjustment
fp_normalize0   lda val.fpmant,x                ; set flags on high word of mantissa
                bne fp_normalize2               ; brif we don't have a full byte to shift
                addb #8                         ; account for a while byte of shifts
                ldu val.fpmant+1,x              ; shift mantissa left 8 bits
                stu val.fpmant,x
                lda val.fpmant+3,x
                sta val.fpmant+2,x
                lda ,s                          ; and include extra bits
                sta val.fpmant+3,x
                clr ,s                          ; and blank extra bits
                cmpb #40                        ; have we shifted 40 bits?
                blo fp_normalize0               ; brif not - keep shifting
                bra fp_normalize7               ; go zero out the value
fp_normalize1   incb                            ; account for one bit of shifting
                lsl ,s                          ; shift mantissa and extra bits left (will not be more than 7 shifts)
                rol val.fpmant+3,x
                rol val.fpmant+2,x
                rol val.fpmant+1,x
                rol val.fpmant,x
fp_normalize2   bpl fp_normalize1               ; brif we have to do a bit shift
                pshs b                          ; apply exponent counter to exponent
                lda val.fpexp,x
                suba ,s+
                bls fp_normalize6               ; brif we underflowed to zero; overflow is impossible
fp_normalize3   lsl ,s+                         ; set C if the high bit of extra precision is set
                bcs fp_normalize5               ; brif bit set - we have to do rounding
fp_normalize4   rts                             ; return if no rounding
fp_normalize5   ldu val.fpmant+2,x              ; add one to mantissa
                leau 1,u
                stu val.fpmant+2,x
                bne fp_normalize4               ; brif low word doesn't carry
                ldu val.fpmant,x
                leau 1,u
                stu val.fpmant,x
                bne fp_normalize4               ; brif high word doesn't carry
                ror val.fpmant,x                ; shift right C in to high bit of mantissa (already set to get here)
                inc val.fpexp,x                 ; bump exponent for a right shift
                bne fp_normalize4               ; brif it doesn't overflow (> +127)
OVERROR2        jmp OVERROR                     ; raise overflow
fp_normalize6   clr val.fpmant,x                ; clear mantissa
                clr val.fpmant+1,x
                clr val.fpmant+2,x
                clr val.fpmant+3,x
fp_normalize7   clr val.fpexp,x                 ; clear exponent and sign
                clr val.fpsign,x
                puls b,pc                       ; clean up stack and return
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Addition and subtraction of values; must enter with values of matching types
;
; Calculates (X) + (U) -> (Y) (addition)
; Calculates (X) - (U) -> (Y) (subtraction)
val_add         ldb val.type,x                  ; get type of left operand
                stb val.type,y                  ; set result type
                cmpb #valtype_float             ; is it float?
                beq fp_add                      ; brif so
                ldd val.int+2,x                 ; do the addition
                addd val.int+2,u
                std val.int+2,y
                ldd val.int,x
                adcb val.int+1,u
                adca val.int,u
                std val.int,y
                bvs OVERROR2                    ; brif calculation overflowed
                rts
val_sub         ldb val.type,x                  ; get type of left operand
                stb val.type,y                  ; set result type
                cmpb #valtype_float             ; floating point?
                beq fp_sub                      ; brif so
                ldd val.int+2,x                 ; do the subtraction
                subd val.int+2,u
                std val.int+2,y
                ldd val.int,x
                sbcb val.int+1,u
                sbca val.int,u
                std val.int,y
                bvs OVERROR2                    ; brif overflow
                rts
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; FP subtraction: just invert the sign of the second operand and add; operands must be writable and they should be
; considered to be clobbered
fp_sub          com val.fpsign,u                ; negate right operand
                ; fall through to addition
; FP addition: this requires that *both operands* are writable and they may be clobbered
fp_add          ldb val.fpexp,u                 ; is the second operand zero?
                beq fp_add0                     ; brif so - it's a no-op - copy the left operand to the output
                lda val.fpexp,x                 ; is left operand zero?
                bne fp_add1                     ; brif not - we have to do the add
                leau ,x                         ; copy the right operand to the output
fp_add0         ldd ,u                          ; copy the value across
                std ,y
                ldd 2,u
                std 2,y
                ldd 4,u
                std 4,y
                rts
fp_add1         subb val.fpexp,x                ; get difference in exponents
                beq fp_add6                     ; brif they're the same - no denormalizing is needed
                bhi fp_add2                     ; brif second one is bigger, need to right-shift the mantissa of first
                exg x,u                         ; swap the operands (we can do that for addition)l second is now biggest
                negb                            ; invert the shift count
fp_add2         cmpb #32                        ; are we shifting more than 32 bits?
                blo fp_add0                     ; brif so - we're effectively adding zero so bail out
fp_add3         cmpb #8                         ; have 8 bits to move?
                bhs fp_add5                     ; brif not
                lda val.fpmant+2,x              ; shift 8 bits right
                sta val.fpmant+3,x
                lda val.fpmant+1,x
                sta val.fpmant+2,x
                lda val.fpmant,x
                sta val.fpmant+1,x
                clr val.fpmant,x
                subb #8                         ; account for 8 shifts
                bra fp_add3                     ; see if we have a whole byte to shift
fp_add4         lsr val.fpmant,x                ; shift right one bit
                ror val.fpmant+1,x
                ror val.fpmant+2,x
                ror val.fpmant+3,x
fp_add5         decb                            ; done all shifts?
                bmi fp_add4                     ; brif not - do a shift
fp_add6         ldb val.fpexp,u                 ; set exponent of result
                stb val.fpexp,y
                ldb val.fpsign,u                ; fetch sign of larger value
                stb val.fpsign,y                ; set result sign
                cmpb val.fpsign,x
                bne fp_add8                     ; brif not - need to subtract the operands
                ldd val.fpmant+2,u              ; add the mantissas
                addd val.fpmant+2,x
                std val.fpmant+2,y
                ldd val.fpmant,u
                adcb val.fpmant+1,x
                adca val.fpmant,x
                std val.fpmant,y
                clrb                            ; clear extra precision bits
                bcc fp_add7                     ; brif no carry
                ror val.fpmant,y                ; shift carry into mantissa
                ror val.fpmant+1,y
                ror val.fpmant+2,y
                ror val.fpmant+3,y
                rorb                            ; keep bits for founding
                inc val.fpexp,y                 ; bump exponent to account for shift
                lbeq OVERROR                    ; brif it overflowed
fp_add7         leax ,y                         ; point to result
                jmp fp_normalize                ; go normalize the result
fp_add8         ldd val.fpmant+2,u              ; subtract operands
                subd val.fpmant+2,x
                std val.fpmant+2,y
                ldd val.fpmant,u
                sbcb val.fpmant+1,x
                sbca val.fpmant,x
                std val.fpmant,y
                bcc fp_add7                     ; brif we didn't carry - no need to fix up
                ldd zero                        ; negate the mantissa bits since we use sign+magnitude
                subd val.fpmant+2,y
                std val.fpmant+2,y
                ldd zero
                sbcb val.fpmant+1,y
                sbca val.fpmant,y
                std val.fpmant,y
                neg val.fpsign,y                ; invert sign of result since we went past zero
                clrb                            ; clear extra precision bits
                bra fp_add7                     ; go normalize the result and return
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Pack a floating point value at (X)
fp_packval      ldb val.fpsign,x                ; get sign
                bmi fp_packval                  ; brif negative - the default 1 bit will do
                ldb val.fpmant,x                ; clear high bit of mantissa for positive
                andb #0x7f
                stb val.fpmant,x
fp_packval0     rts
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Unpack a floating point value at (X)
fp_unpackval0   ldb val.fpmant,x                ; get high byte of mantissa
                sex                             ; now A is value for sign byte
                sta val.fpsign,x                ; set sign
                orb #0x80                       ; set high bit of mantissa
                stb val.fpmant,x
                rts
                *pragmapop list