From c025bba177fbd3ce4196fd0b01788ae720263548 Mon Sep 17 00:00:00 2001
From: Stephen Heumann <stephenheumann@gmail.com>
Date: Mon, 22 Nov 2021 19:25:25 -0600
Subject: [PATCH] Implement nearbyint and fdim.

---
 math2.asm    | 115 +++++++++++++++++++++++++++++++++++++++++++++++++++
 math2.macros |  12 ++++++
 2 files changed, 127 insertions(+)

diff --git a/math2.asm b/math2.asm
index b4da369..34aa4c4 100644
--- a/math2.asm
+++ b/math2.asm
@@ -388,6 +388,77 @@ expm1l   entry
          rtl
          end
 
+****************************************************************
+*
+*  double fdim(double x, double y);
+*
+*  Returns x - y if x > y, or +0 if x <= y.
+*
+****************************************************************
+*
+fdim     start
+fdimf    entry
+fdiml    entry
+         using MathCommon2
+
+         phb
+         phk
+         plb
+
+         tsc                            compare x and y
+         clc
+         adc   #5
+         pea   0
+         pha
+         adc   #10
+         pea   0
+         pha
+         FCMPX
+         bmi   x_le_y
+         beq   x_le_y
+         
+         tsc                            if x > y (or unordered)
+         clc
+         adc   #5+10
+         pea   0
+         pha
+         sbc   #10-1                      (carry is clear)
+         pea   0
+         pha
+         FSUBX                            x = x - y
+         lda   5,s                        t1 = x
+         sta   t1
+         lda   5+2,s
+         sta   t1+2
+         lda   5+4,s
+         sta   t1+4
+         lda   5+6,s
+         sta   t1+6
+         lda   5+8,s
+         sta   t1+8
+         bra   ret                      else
+
+x_le_y   stz   t1                         t1 = +0.0
+         stz   t1+2
+         stz   t1+4
+         stz   t1+6
+         stz   t1+8
+
+ret      plx                            clean up stack
+         ply
+         tsc
+         clc
+         adc   #20
+         tcs
+         phy
+         phx
+         plb
+         
+         ldx   #^t1                     return a pointer to the result
+         lda   #t1
+         rtl
+         end
+
 ****************************************************************
 *
 *  int ilogb(double x);
@@ -749,6 +820,50 @@ ret      lda   #^t1                     return t1 (fractional part)
          creturn 4:iptr
          end
 
+****************************************************************
+*
+*  double nearbyint(double x);
+*
+*  Rounds x to an integer using current rounding direction,
+*  never raising the "inexact" exception.
+*
+****************************************************************
+*
+nearbyint start
+nearbyintf entry
+nearbyintl entry
+         using MathCommon2
+
+         phb                            place the number in a work area
+         plx
+         ply
+         phk
+         plb
+         pla
+         sta   t1
+         pla
+         sta   t1+2
+         pla
+         sta   t1+4
+         pla
+         sta   t1+6
+         pla
+         sta   t1+8
+         phy
+         phx
+         plb
+
+         FGETENV                        save environment
+         phx
+         ph4   #t1                      compute the value
+         FRINTX
+         FSETENV                        restore environment
+         
+         ldx   #^t1                     return a pointer to the result
+         lda   #t1
+         rtl
+         end
+
 ****************************************************************
 *
 *  double remainder(double x, double y);
diff --git a/math2.macros b/math2.macros
index 0f01849..f4c01ff 100644
--- a/math2.macros
+++ b/math2.macros
@@ -388,3 +388,15 @@
  LDX #$090A
  JSL $E10000
  MEND
+ MACRO
+&LAB FGETENV
+&LAB PEA $03
+ LDX #$090A
+ JSL $E10000
+ MEND
+ MACRO
+&LAB FSETENV
+&LAB PEA $01
+ LDX #$090A
+ JSL $E10000
+ MEND