; ---------------------------------------------------------------
; Macros to support Floating point arithmetic:
;
; PUSH ea               Push number (FP or Long int) on Stack
; POP  ea               POP  number from stack
; FP_RCL label          Push variable on Stack
; FP_STO label          POP  variable from stack
; FP_DUP                Duplicate number on Stack
; FP_DROP               Drop number from stack
; FP_OVER               copy 2nd number to TOS
; FP_SWAP      exchange TOS and NOS
;
; FP_FIX                convert REAL*4 to INTEGER*4
; FP_FLT                convert INTEGER*4 to REAL*4
; FP_CMP                set D0 for NOS - TOS (no change to stack)
; FP_TST                test TOS (only D0, no change to stack)
; FP_ABS / FP_NEG
; FP_ADD / FP_SUB / FP_MUL / FP_DIV  (NOS - TOS     NOS / TOS)
; FP_SIN / FP_COS / FP_EXP / FP_LOG / FP_SQR / FP_ASIN / FP_ACOS
; FP_POW                NOS ** TOS
;
; the following constants are defined: (PUSH #FP_PI)
; 
; FP_PI                 Pi
; FP_2PI                2*Pi
; FP_4PI                4*Pi
; FP_PID2               Pi/2
; FP_PID4               Pi/4
; FP_2                  2
; FP_1D2                1/2
; FP_4                  4
; FP_10                 10
; FP_1D10               1/10

       OPENLIB       mathffp
       OPENLIB       mathtrans

_LVOSPFix:   EQU   -30
_LVOSPFlt:   EQU   -36
_LVOSPCmp:   EQU   -42
_LVOSPTst:   EQU   -48
_LVOSPAbs:   EQU   -54
_LVOSPNeg:   EQU   -60
_LVOSPAdd:   EQU   -66
_LVOSPSub:   EQU   -72
_LVOSPMul:   EQU   -78
_LVOSPDiv:   EQU   -84

_LVOSPAtan:  EQU   -30
_LVOSPSin:   EQU   -36
_LVOSPCos:   EQU   -42
_LVOSPTan:   EQU   -48
_LVOSPSincos: EQU  -54
_LVOSPSinh:  EQU   -60
_LVOSPCosh:  EQU   -66
_LVOSPTanh:  EQU   -72
_LVOSPExp:   EQU   -78
_LVOSPLog:   EQU   -84
_LVOSPPow:   EQU   -90
_LVOSPSqrt:  EQU   -96
_LVOSPTieee: EQU   -102
_LVOSPFieee: EQU   -108
_LVOSPAsin:  EQU   -114
_LVOSPAcos:  EQU   -120
_LVOSPLog10: EQU   -126


; initializing constants

FP_PI:   EQU   $C90FDB42
FP_2PI:  EQU   $C90FDB43
FP_4PI:  EQU   $C90FDB44
FP_PID2: EQU   $C90FDA41
FP_PID4: EQU   $C90FDA40
FP_0:    EQU   0
FP_1:    EQU   $80000041
FP_2:    EQU   $80000042
FP_1D2:  EQU   $80000040
FP_4:    EQU   $80000043
FP_10:   EQU   $A0000044
FP_1D10: EQU   $CCCCCD3D


; macro definitions
PUSH: MACRO    $\1
      MOVE.L   \1,-(A7)
      BSR      _PUSH
      ENDM

POP:  MACRO    $\1
      BSR      _POP
      MOVE.L   (A7)+,\1
      ENDM

FP_RCL: MACRO $\1
      MOVE.L   \1(PC),-(A7)
      BSR      _PUSH
      ENDM

FP_STO: MACRO $\1
      PEA      \1(PC)
      BSR      _FP_STO
      ENDM

FP_DUP:  MACRO
      BSR      _FP_DUP
      ENDM

FP_DROP: MACRO
      BSR      _FP_DROP
      ENDM

FP_OVER: MACRO
      BSR      _FP_OVER
      ENDM

FP_SWAP: MACRO
      BSR      _FP_SWAP
      ENDM

FP_FIX:  MACRO
      MOVE.W      #_LVOSPFix,-(A7)
      BSR         _MATH11
      ENDM

FP_ROUND: MACRO
      BSR         _SPRound
      ENDM

FP_FLT:  MACRO
      MOVE.W      #_LVOSPFlt,-(A7)
      BSR         _MATH11
      ENDM

FP_CMP:  MACRO
      MOVE.L      FP_STACK(PC),A6
      MOVE.L      (A6),D0
      MOVE.L      4(A6),D1
      LINKSYS     SPCmp,mathffp
      ENDM

FP_TST:  MACRO
      MOVE.L      FP_STACK(PC),A6
      MOVE.L      (A6),D1
      LINKSYS     SPTst,mathffp
      ENDM

FP_ABS:  MACRO
      MOVE.W      #_LVOSPAbs,-(A7)
      BSR         _MATH11
      ENDM

FP_NEG:  MACRO
      MOVE.W      #_LVOSPNeg,-(A7)
      BSR         _MATH11
      ENDM

FP_ADD:  MACRO
      MOVE.W      #_LVOSPAdd,-(A7)
      BSR         _MATH21
      ENDM

FP_SUB:  MACRO
      MOVE.W      #_LVOSPSub,-(A7)
      BSR         _MATH21
      ENDM

FP_MUL:  MACRO
      MOVE.W      #_LVOSPMul,-(A7)
      BSR         _MATH21
      ENDM

FP_DIV:  MACRO
      MOVE.W      #_LVOSPDiv,-(A7)
      BSR         _MATH21
      ENDM

FP_SIN:  MACRO
      MOVE.W      #_LVOSPSin,-(A7)
      BSR         _MATHT11
      ENDM

FP_COS:  MACRO
      MOVE.W      #_LVOSPCos,-(A7)
      BSR         _MATHT11
      ENDM

FP_EXP:  MACRO
      MOVE.W      #_LVOSPExp,-(A7)
      BSR         _MATHT11
      ENDM

FP_LOG:  MACRO
      MOVE.W      #_LVOSPLog,-(A7)
      BSR         _MATHT11
      ENDM

FP_SQR:  MACRO
      MOVE.W      #_LVOSPSqrt,-(A7)
      BSR         _MATHT11
      ENDM

FP_ASIN: MACRO
      MOVE.W      #_LVOSPAsin,-(A7)
      BSR         _MATHT11
      ENDM

FP_ACOS: MACRO
      MOVE.W      #_LVOSPAcos,-(A7)
      BSR         _MATHT11
      ENDM

FP_POW:  MACRO
      MOVE.W      #_LVOSPPow,-(A7)
      BSR         _MATHT21
      ENDM

; --------------------------------------------------------------
; defining stack area
      LEA      FP_STACK,A6
      MOVE.L   A6,A5
      SUBQ     #8,A5    ; vorsicht ist die Mutter der Porzellankiste
      MOVE.L   A5,(A6)
      BRA      FP_MACS_EX
      DS.L  30
FP_STACK:
      DS.L  10
; --------------------------------------------------------------
; defining some floatingpoint subroutines
; --------------------------------------------------------------
_PUSH:
      MOVEM.L  A5/A6,-(A7)
      LEA      FP_STACK,A5
      MOVE.L   (A5),A6
      MOVE.L   12(A7),-(A6)
      MOVE.L   A6,(A5)
      MOVEM.L  (A7)+,A5/A6
      MOVE.L   (A7),4(A7) ; remove parameter
      ADDQ.L   #4,A7
      RTS
_POP:
      MOVE.L   (A7),-(A7)
      MOVEM.L  A5/A6,-(A7)
      LEA      FP_STACK,A5
      MOVE.L   (A5),A6
      MOVE.L   (A6)+,12(A7)
      MOVE.L   A6,(A5)
      MOVEM.L  (A7)+,A5/A6
      RTS
_FP_STO:
      MOVEM.L  A5/A6,-(A7)
      MOVE.L   FP_STACK(PC),A6
      MOVE.L   12(A7),A5
      MOVE.L   (A6)+,(A5)
      LEA      FP_STACK,A5
      MOVE.L   A6,(A5)
      MOVEM.L  (A7)+,A5/A6
      MOVE.L   (A7),4(A7) ; remove parameter
      ADDQ.L   #4,A7
      RTS
_FP_DUP:
      MOVEM.L  A5/A6,-(A7)
      LEA      FP_STACK,A5
      MOVE.L   (A5),A6
      MOVE.L   (A6),-(A6)
      MOVE.L   A6,(A5)
      MOVEM.L   (A7)+,A5/A6
      RTS
_FP_DROP:
      MOVE.L   A6,-(A7) 
      LEA      FP_STACK,A6
      ADDQ.L   #4,(A6)
      MOVE.L   (A7)+,A6
      RTS
_FP_OVER:
      MOVEM.L  A5/A6,-(A7)
      LEA      FP_STACK,A5
      MOVE.L   (A5),A6
      MOVE.L   4(A6),-(A6)
      MOVE.L   A6,(A5)
      MOVEM.L  (A7)+,A5/A6
      RTS
_FP_SWAP:
      MOVEM.L  A5/A6,-(A7)
      MOVE.L   FP_STACK(PC),A6
      MOVE.L   (A6),A5
      MOVE.L   4(A6),(A6)
      MOVE.L   A5,4(A6)
      MOVEM.L  (A7)+,A5/A6
      RTS
_MATH11:
      MOVEM.L     D0/D1/A0/A1/A6,-(A7)
      MOVE.W      24(A7),D1    ; get parameter (offset of library routine)
      MOVE.L      FP_STACK(PC),A6
      MOVE.L      (A6),D0
      MOVE.L      mathffp(PC),A6
      JSR         0(A6,D1.W)
      MOVE.L      FP_STACK(PC),A6
      MOVE.L      D0,(A6)
      MOVEM.L     (A7)+,D0/D1/A0/A1/A6
      MOVE.L      (A7),2(A7) ; remove parameter
      ADDQ.L      #2,A7
      RTS
_MATHT11:
      MOVEM.L     D0/D1/A0/A1/A6,-(A7)
      MOVE.W      24(A7),D1    ; get parameter (offset of library routine)
      MOVE.L      FP_STACK(PC),A6
      MOVE.L      (A6),D0
      MOVE.L      mathtrans(PC),A6
      JSR         0(A6,D1.W)
      MOVE.L      FP_STACK(PC),A6
      MOVE.L      D0,(A6)
      MOVEM.L     (A7)+,D0/D1/A0/A1/A6
      MOVE.L      (A7),2(A7) ; remove parameter
      ADDQ.L      #2,A7
      RTS
_MATH21:
      MOVEM.L     D0/D1/D2/A5/A6,-(A7)
      MOVE.W      24(A7),D2    ; get parameter (offset of library routine)
      MOVE.L      FP_STACK(PC),A6
      MOVE.L      4(A6),D0
      MOVE.L      (A6),D1
      MOVE.L      mathffp(PC),A6
      JSR         0(A6,D2.W)
      LEA         FP_STACK(PC),A5
      MOVE.L      (A5),A6
      ADDQ        #4,A6
      MOVE.L      D0,(A6)
      MOVE.L      A6,(A5)
      MOVEM.L     (A7)+,D0/D1/D2/A5/A6
      MOVE.L      (A7),2(A7) ; remove parameter
      ADDQ.L      #2,A7
      RTS

_MATHT21:
      MOVEM.L     D0/D1/D2/A5/A6,-(A7)
      MOVE.W      24(A7),D2    ; get parameter (offset of library routine)
      MOVE.L      FP_STACK(PC),A6
      MOVE.L      4(A6),D0
      MOVE.L      (A6),D1
      MOVE.L      mathtrans(PC),A6
      JSR         0(A6,D2.W)
      LEA         FP_STACK(PC),A5
      MOVE.L      (A5),A6
      ADDQ        #4,A6
      MOVE.L      D0,(A6)
      MOVE.L      A6,(A5)
      MOVEM.L     (A7)+,D0/D1/D2/A5/A6
      MOVE.L      (A7),2(A7) ; remove parameter
      ADDQ.L      #2,A7
      RTS

_SPRound:
      MOVEM.L     D0/D1,-(A7)
      FP_DUP
; keep old TOS
      PUSH        #FP_10
; trap 1 < TOS > 0
      FP_MUL
      FP_FIX
; O.K. floatingpoint compare would be better, but I don't know how SPTst works
      POP         D0
      TST.L       D0
      BPL.S       SPRound_1
      FP_NEG
; we can only round positive numbers correct
SPRound_1:
      PUSH        #FP_1D2
      FP_ADD
; add 0.5
      FP_FIX
; truncate to integer
      POP         D1
      TST.L       D0      ; test again for negative number
      BPL.S       SPRound_2
      NEG         D1      ; make negative number
SPRound_2:
      PUSH        D1
      MOVEM.L     (A7)+,D0/D1
      RTS
FP_MACS_EX:
      NOP