MathsStuff

; MATH ZP LOCATIONS
MATHWORKSPACE = &A0 ; 160d, START OF MATH ROUTINES WORKSPACE BLOCK
; MATH WORKSPACE BLOCK LAYOUT
;MATHWORKSPACE
    MWS_INPUT1  = MATHWORKSPACE + 0      ; (b)
    MWS_INPUT2  = MATHWORKSPACE + 1      ; (b)
    MWS_INPUT3  = MATHWORKSPACE + 2      ; (b)
    MWS_INPUT4  = MATHWORKSPACE + 3      ; (b)
    MWS_OUTPUT1 = MATHWORKSPACE + 4     ; (b)
    MWS_OUTPUT2 = MATHWORKSPACE + 5     ; (b)
    MWS_OUTPUT3 = MATHWORKSPACE + 6     ; (b)
    MWS_OUTPUT4 = MATHWORKSPACE + 7     ; (b)

;
; ADD16
;   INPUT:  MWS_INPUT1 (w), MWS_INPUT3 (w)
;   OUTPUT: OUTPUT1 (w)
.MATH_ADD16
{
    PHA
    CLC
    LDA MWS_INPUT1
    ADC MWS_INPUT3
    STA MWS_OUTPUT1         ; store sum of LSBs
    LDA MWS_INPUT2
    ADC MWS_INPUT4          ; add the MSBs using carry from
    STA MWS_OUTPUT2         ; the previous calculation
    PLA
    RTS
}


;
; SUBTRACT16
;   INPUT:  MWS_INPUT1 (w), MWS_INPUT3 (w)
;   OUTPUT: OUTPUT1 (w)
.MATH_SUBTRACT16
{
    PHA
    SEC                     ; set carry for borrow purpose
    LDA MWS_INPUT1
    SBC MWS_INPUT3          ; perform subtraction on the LSBs
    STA MWS_OUTPUT1
    LDA MWS_INPUT2          ; do the same for the MSBs, with carry
    SBC MWS_INPUT4          ; set according to the previous result
    STA MWS_OUTPUT2
    PLA
    RTS
}


;
; MULTIPLY8
;   INPUT:  MWS_INPUT1 (b), MWS_INPUT2 (b)
;   OUTPUT: OUTPUT1 (w)
.MATH_MULTIPLY8
{
    ; A * 256 + X = MWS_INPUT1 * MWS_INPUT2
    PUSHAXY
    LDA #$00
    LDX #$08
    CLC
.m0
    BCC m1
    CLC
    ADC MWS_INPUT2
.m1
    ROR A
    ROR MWS_INPUT1
    DEX
    BPL m0
    LDX MWS_INPUT1
    ; store A, X in MWS_OUTPUT2, MWS_OUTPUT1
    STA MWS_OUTPUT2
    STX MWS_OUTPUT1
    POPAXY
    RTS
}