xref: /hal_rpi_pico-latest/src/rp2_common/pico_double/double_math.c

/*
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <math.h>
#include "pico/double.h"

// opened a separate issue https://github.com/raspberrypi/pico-sdk/issues/166 to deal with these warnings if at all
GCC_Pragma("GCC diagnostic push")
GCC_Pragma("GCC diagnostic ignored \"-Wconversion\"")
GCC_Pragma("GCC diagnostic ignored \"-Wsign-conversion\"")

typedef uint64_t ui64;
typedef uint32_t ui32;
typedef int64_t i64;

#define PINF ( HUGE_VAL)
#define MINF (-HUGE_VAL)
#define PZERO (+0.0)
#define MZERO (-0.0)


#define PI       3.14159265358979323846
#define LOG2     0.69314718055994530941
// Unfortunately in double precision ln(10) is very close to half-way between to representable numbers
#define LOG10    2.30258509299404568401
#define LOG2E    1.44269504088896340737
#define LOG10E   0.43429448190325182765
#define ONETHIRD 0.33333333333333333333

#define PIf       3.14159265358979323846f
#define LOG2f     0.69314718055994530941f
#define LOG2Ef    1.44269504088896340737f
#define LOG10Ef   0.43429448190325182765f
#define ONETHIRDf 0.33333333333333333333f

#define DUNPACK(x,e,m) e=((x)>>52)&0x7ff,m=((x)&0x000fffffffffffffULL)|0x0010000000000000ULL
#define DUNPACKS(x,s,e,m) s=((x)>>63),DUNPACK((x),(e),(m))

typedef union {
    double d;
    ui64 ix;
} double_ui64;

static inline double ui642double(ui64 ix) {
    double_ui64 tmp;
    tmp.ix = ix;
    return tmp.d;
}

static inline ui64 double2ui64(double d) {
    double_ui64 tmp;
    tmp.d = d;
    return tmp.ix;
}

#if PICO_DOUBLE_PROPAGATE_NANS
static inline bool disnan(double x) {
    ui64 ix= double2ui64(x);
    // checks the top bit of the low 32 bit of the NAN, but it I think that is ok
    return ((uint32_t)(ix >> 31)) > 0xffe00000u;
}

#define check_nan_d1(x) if (disnan((x))) return (x)
#define check_nan_d2(x,y) if (disnan((x))) return (x); else if (disnan((y))) return (y);
#else
#define check_nan_d1(x) ((void)0)
#define check_nan_d2(x,y) ((void)0)
#endif

static inline int dgetsignexp(double x) {
    ui64 ix=double2ui64(x);
    return (ix>>52)&0xfff;
}

static inline int dgetexp(double x) {
    ui64 ix=double2ui64(x);
    return (ix>>52)&0x7ff;
}

static inline double dldexp(double x,int de) {
    ui64 ix=double2ui64(x),iy;
    int e;
    e=dgetexp(x);
    if(e==0||e==0x7ff) return x;
    e+=de;
    if(e<=0) iy=ix&0x8000000000000000ULL; // signed zero for underflow
    else if(e>=0x7ff) iy=(ix&0x8000000000000000ULL)|0x7ff0000000000000ULL; // signed infinity on overflow
    else iy=ix+((ui64)de<<52);
    return ui642double(iy);
}

double WRAPPER_FUNC(ldexp)(double x, int de) {
    check_nan_d1(x);
    return dldexp(x, de);
}


static inline double dcopysign(double x,double y) {
    ui64 ix=double2ui64(x),iy=double2ui64(y);
    ix=((ix&0x7fffffffffffffffULL)|(iy&0x8000000000000000ULL));
    return ui642double(ix);
}

double WRAPPER_FUNC(copysign)(double x, double y) {
    check_nan_d2(x,y);
    return dcopysign(x, y);
}
static inline int diszero(double x)  { return dgetexp    (x)==0; }
//static inline int dispzero(double x) { return dgetsignexp(x)==0; }
//static inline int dismzero(double x) { return dgetsignexp(x)==0x800; }
static inline int disinf(double x)   { return dgetexp    (x)==0x7ff; }
static inline int dispinf(double x)  { return dgetsignexp(x)==0x7ff; }
static inline int disminf(double x)  { return dgetsignexp(x)==0xfff; }

static inline int disint(double x) {
    ui64 ix=double2ui64(x),m;
    int e=dgetexp(x);
    if(e==0) return 1;       // 0 is an integer
    e-=0x3ff;                // remove exponent bias
    if(e<0) return 0;        // |x|<1
    e=52-e;                  // bit position in mantissa with significance 1
    if(e<=0) return 1;       // |x| large, so must be an integer
    m=(1ULL<<e)-1;           // mask for bits of significance <1
    if(ix&m) return 0;       // not an integer
    return 1;
}

static inline int disoddint(double x) {
    ui64 ix=double2ui64(x),m;
    int e=dgetexp(x);
    e-=0x3ff;                // remove exponent bias
    if(e<0) return 0;        // |x|<1; 0 is not odd
    e=52-e;                  // bit position in mantissa with significance 1
    if(e<0) return 0;        // |x| large, so must be even
    m=(1ULL<<e)-1;           // mask for bits of significance <1 (if any)
    if(ix&m) return 0;       // not an integer
    if(e==52) return 1;      // value is exactly 1
    return (ix>>e)&1;
}

static inline int disstrictneg(double x) {
    ui64 ix=double2ui64(x);
    if(diszero(x)) return 0;
    return ix>>63;
}

static inline int disneg(double x) {
    ui64 ix=double2ui64(x);
    return ix>>63;
}

static inline double dneg(double x) {
    ui64 ix=double2ui64(x);
    ix^=0x8000000000000000ULL;
    return ui642double(ix);
}

static inline int dispo2(double x) {
    ui64 ix=double2ui64(x);
    if(diszero(x)) return 0;
    if(disinf(x)) return 0;
    ix&=0x000fffffffffffffULL;
    return ix==0;
}

static inline double dnan_or(double x) {
#if PICO_DOUBLE_PROPAGATE_NANS
    return NAN;
#else
    return x;
#endif
}

double WRAPPER_FUNC(trunc)(double x) {
    check_nan_d1(x);
    ui64 ix=double2ui64(x),m;
    int e=dgetexp(x);
    e-=0x3ff;                // remove exponent bias
    if(e<0) {                // |x|<1
        ix&=0x8000000000000000ULL;
        return ui642double(ix);
    }
    e=52-e;                  // bit position in mantissa with significance 1
    if(e<=0) return x;       // |x| large, so must be an integer
    m=(1ULL<<e)-1;           // mask for bits of significance <1
    ix&=~m;
    return ui642double(ix);
}

double WRAPPER_FUNC(round)(double x) {
    check_nan_d1(x);
    ui64 ix=double2ui64(x),m;
    int e=dgetexp(x);
    e-=0x3ff;                // remove exponent bias
    if(e<-1) {               // |x|<0.5
        ix&=0x8000000000000000ULL;
        return ui642double(ix);
    }
    if(e==-1) {              // 0.5<=|x|<1
        ix&=0x8000000000000000ULL;
        ix|=0x3ff0000000000000ULL;        // ±1
        return ui642double(ix);
    }
    e=52-e;                  // bit position in mantissa with significance 1, <=52
    if(e<=0) return x;       // |x| large, so must be an integer
    m=1ULL<<(e-1);           // mask for bit of significance 0.5
    ix+=m;
    m=m+m-1;                 // mask for bits of significance <1
    ix&=~m;
    return ui642double(ix);
}

double WRAPPER_FUNC(floor)(double x) {
    check_nan_d1(x);
    ui64 ix=double2ui64(x),m;
    int e=dgetexp(x);
    if(e==0) {               // x==0
        ix&=0x8000000000000000ULL;
        return ui642double(ix);
    }
    e-=0x3ff;                // remove exponent bias
    if(e<0) {                // |x|<1, not zero
        if(disneg(x)) return -1;
        return PZERO;
    }
    e=52-e;                  // bit position in mantissa with significance 1
    if(e<=0) return x;       // |x| large, so must be an integer
    m=(1ULL<<e)-1;           // mask for bit of significance <1
    if(disneg(x)) ix+=m;     // add 1-ε to magnitude if negative
    ix&=~m;                  // truncate
    return ui642double(ix);
}

double WRAPPER_FUNC(ceil)(double x) {
    check_nan_d1(x);
    ui64 ix=double2ui64(x),m;
    int e=dgetexp(x);
    if(e==0) {               // x==0
        ix&=0x8000000000000000ULL;
        return ui642double(ix);
    }
    e-=0x3ff;                 // remove exponent bias
    if(e<0) {                // |x|<1, not zero
        if(disneg(x)) return MZERO;
        return 1;
    }
    e=52-e;                  // bit position in mantissa with significance 1
    if(e<=0) return x;       // |x| large, so must be an integer
    m=(1ULL<<e)-1;           // mask for bit of significance <1
    if(!disneg(x)) ix+=m;    // add 1-ε to magnitude if positive
    ix&=~m;                  // truncate
    return ui642double(ix);
}

double WRAPPER_FUNC(asin)(double x) {
    check_nan_d1(x);
    double u;
    u=(1-x)*(1+x);
    if(disstrictneg(u)) return dnan_or(PINF);
    return atan2(x,sqrt(u));
}

double WRAPPER_FUNC(acos)(double x) {
    check_nan_d1(x);
    double u;
    u=(1-x)*(1+x);
    if(disstrictneg(u)) return dnan_or(PINF);
    return atan2(sqrt(u),x);
}

double WRAPPER_FUNC(atan)(double x) {
    check_nan_d1(x);
    if(dispinf(x)) return  PI/2;
    if(disminf(x)) return -PI/2;
    return atan2(x,1);
}

double WRAPPER_FUNC(sinh)(double x) {
    check_nan_d1(x);
    return dldexp((exp(x)-exp(dneg(x))),-1);
}

double WRAPPER_FUNC(cosh)(double x) {
    check_nan_d1(x);
    return dldexp((exp(x)+exp(dneg(x))),-1);
}

double WRAPPER_FUNC(tanh)(double x) {
    check_nan_d1(x);
    double u;
    int e;
    e=dgetexp(x);
    if(e>=5+0x3ff) {             // |x|>=32?
        if(!disneg(x)) return  1;  // 1 << exp 2x; avoid generating infinities later
        else           return -1;  // 1 >> exp 2x
    }
    u=exp(dldexp(x,1));
    return (u-1)/(u+1);
}

double WRAPPER_FUNC(asinh)(double x) {
    check_nan_d1(x);
    int e;
    e=dgetexp(x);
    if(e>=32+0x3ff) {                                // |x|>=2^32?
        if(!disneg(x)) return      log(     x )+LOG2;  // 1/x^2 << 1
        else           return dneg(log(dneg(x))+LOG2); // 1/x^2 << 1
    }
    if(x>0) return      log(sqrt(x*x+1)+x);
    else    return dneg(log(sqrt(x*x+1)-x));
}

double WRAPPER_FUNC(acosh)(double x) {
    check_nan_d1(x);
    int e;
    if(disneg(x)) x=dneg(x);
    e=dgetexp(x);
    if(e>=32+0x3ff) return log(x)+LOG2;           // |x|>=2^32?
    return log(sqrt((x-1)*(x+1))+x);
}

double WRAPPER_FUNC(atanh)(double x) {
    check_nan_d1(x);
    return dldexp(log((1+x)/(1-x)),-1);
}

double WRAPPER_FUNC(exp2)(double x) {
    check_nan_d1(x);
    int e;
    // extra check for disminf as this catches -Nan, and x<=-4096 doesn't.
    if (disminf(x) || x<=-4096) return 0;    // easily underflows
    else if (x>=4096)           return PINF; // easily overflows
    e=(int)round(x);
    x-=e;
    return dldexp(exp(x*LOG2),e);
}
double WRAPPER_FUNC(log2)(double x) { check_nan_d1(x); return log(x)*LOG2E;  }
double WRAPPER_FUNC(exp10)(double x) { check_nan_d1(x); return pow(10,x); }
double WRAPPER_FUNC(log10)(double x) { check_nan_d1(x); return log(x)*LOG10E; }

// todo these are marked as lofi
double WRAPPER_FUNC(expm1)(double x) { check_nan_d1(x); return exp(x)-1; }
double WRAPPER_FUNC(log1p)(double x) { check_nan_d1(x); return log(1+x); }
#if !HAS_DOUBLE_COPROCESSOR
double WRAPPER_FUNC(fma)(double x,double y,double z) { check_nan_d1(x); return x*y+z; }
#endif

// general power, x>0, finite
static double dpow_1(double x,double y) {
    int a,b,c;
    double t,rt,u,v,v0,v1,w,ry;
    a=dgetexp(x)-0x3ff;
    u=log2(dldexp(x,-a)); // now log_2 x = a+u
    if(u>0.5) u-=1,a++;              // |u|<=~0.5
    if(a==0) return exp2(u*y);
    // here |log_2 x| >~0.5
    if(y>= 4096) { // then easily over/underflows
        if(a<0) return 0;
        return PINF;
    }
    if(y<=-4096) { // then easily over/underflows
        if(a<0) return PINF;
        return 0;
    }
    ry=round(y);
    v=y-ry;
    v0=dldexp(round(ldexp(v,26)),-26);
    v1=v-v0;
    b=(int)ry; // guaranteed to fit in an int; y=b+v0+v1
    // now the result is exp2( (a+u) * (b+v0+v1) )
    c=a*b;      // integer
    t=a*v0;
    rt=round(t);
    c+=(int)rt;
    w=t-rt;
    t=a*v1;
    w+=t;
    t=u*b;
    rt=round(t);
    c+=(int)rt;
    w+=t-rt;
    w+=u*v;
    return dldexp(exp2(w),c);
}

static double dpow_int2(double x,int y) {
    double u;
    if(y==1) return x;
    u=dpow_int2(x,y/2);
    u*=u;
    if(y&1) u*=x;
    return u;
}

// for the case where x not zero or infinity, y small and not zero
static inline double dpowint_1(double x,int y) {
    if(y<0) x=1/x,y=-y;
    return dpow_int2(x,y);
}

// for the case where x not zero or infinity
static double dpowint_0(double x,int y) {
    int e;
    if(disneg(x)) {
        if(disoddint(y)) return dneg(dpowint_0(dneg(x),y));
        else             return      dpowint_0(dneg(x),y);
    }
    if(dispo2(x)) {
        e=dgetexp(x)-0x3ff;
        if(y>=2048) y= 2047;  // avoid overflow
        if(y<-2048) y=-2048;
        y*=e;
        return dldexp(1,y);
    }
    if(y==0) return 1;
    if(y>=-32&&y<=32) return dpowint_1(x,y);
    return dpow_1(x,y);
}

double WRAPPER_FUNC(powint)(double x,int y) {
    GCC_Like_Pragma("GCC diagnostic push")
    GCC_Like_Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
    if(x==1.0||y==0) return 1;
    GCC_Like_Pragma("GCC diagnostic pop")
    check_nan_d1(x);
    if(diszero(x)) {
        if(y>0) {
            if(y&1) return x;
            else    return 0;
        }
        if((y&1)) return dcopysign(PINF,x);
        return PINF;
    }
    if(dispinf(x)) {
        if(y<0) return 0;
        else    return PINF;
    }
    if(disminf(x)) {
        if(y>0) {
            if((y&1)) return MINF;
            else      return PINF;
        }
        if((y&1)) return MZERO;
        else      return PZERO;
    }
    return dpowint_0(x,y);
}

// for the case where y is guaranteed a finite integer, x not zero or infinity
static double dpow_0(double x,double y) {
    int e,p;
    if(disneg(x)) {
        if(disoddint(y)) return dneg(dpow_0(dneg(x),y));
        else             return      dpow_0(dneg(x),y);
    }
    p=(int)y;
    if(dispo2(x)) {
        e=dgetexp(x)-0x3ff;
        if(p>=2048) p= 2047;  // avoid overflow
        if(p<-2048) p=-2048;
        p*=e;
        return dldexp(1,p);
    }
    if(p==0) return 1;
    if(p>=-32&&p<=32) return dpowint_1(x,p);
    return dpow_1(x,y);
}

double WRAPPER_FUNC(pow)(double x,double y) {
    GCC_Like_Pragma("GCC diagnostic push")
    GCC_Like_Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")

    if(x==1.0||diszero(y)) return 1;
    check_nan_d2(x, y);
    if(x==-1.0&&disinf(y)) return 1;
    GCC_Like_Pragma("GCC diagnostic pop")

    if(diszero(x)) {
        if(!disneg(y)) {
            if(disoddint(y)) return x;
            else             return 0;
        }
        if(disoddint(y)) return dcopysign(PINF,x);
        return PINF;
    }
    if(dispinf(x)) {
        if(disneg(y)) return 0;
        else          return PINF;
    }
    if(disminf(x)) {
        if(!disneg(y)) {
            if(disoddint(y)) return MINF;
            else             return PINF;
        }
        if(disoddint(y)) return MZERO;
        else             return PZERO;
    }
    if(dispinf(y)) {
        if(dgetexp(x)<0x3ff) return PZERO;
        else                 return PINF;
    }
    if(disminf(y)) {
        if(dgetexp(x)<0x3ff) return PINF;
        else                 return PZERO;
    }
    if(disint(y)) return dpow_0(x,y);
    if(disneg(x)) return PINF;
    return dpow_1(x,y);
}

double WRAPPER_FUNC(hypot)(double x,double y) {
    check_nan_d2(x, y);
    int ex,ey;
    ex=dgetexp(x); ey=dgetexp(y);
    if(ex>=0x3ff+400||ey>=0x3ff+400) { // overflow, or nearly so
        x=dldexp(x,-600),y=dldexp(y,-600);
        return dldexp(sqrt(x*x+y*y), 600);
    }
    else if(ex<=0x3ff-400&&ey<=0x3ff-400) { // underflow, or nearly so
        x=dldexp(x, 600),y=dldexp(y, 600);
        return dldexp(sqrt(x*x+y*y),-600);
    }
    return sqrt(x*x+y*y);
}

double WRAPPER_FUNC(cbrt)(double x) {
    check_nan_d1(x);
    int e;
    if(disneg(x)) return dneg(cbrt(dneg(x)));
    if(diszero(x)) return dcopysign(PZERO,x);
    e=dgetexp(x)-0x3ff;
    e=(e*0x5555+0x8000)>>16;  // ~e/3, rounded
    x=dldexp(x,-e*3);
    x=exp(log(x)*ONETHIRD);
    return dldexp(x,e);
}

// reduces mx*2^e modulo my, returning bottom bits of quotient at *pquo
// 2^52<=|mx|,my<2^53, e>=0; 0<=result<my
static i64 drem_0(i64 mx,i64 my,int e,int*pquo) {
    int quo=0,q,r=0,s;
    if(e>0) {
        r=0xffffffffU/(ui32)(my>>36);  // reciprocal estimate Q16
    }
    while(e>0) {
        s=e; if(s>12) s=12;    // gain up to 12 bits on each iteration
        q=(mx>>38)*r;          // Q30
        q=((q>>(29-s))+1)>>1;  // Q(s), rounded
        mx=(mx<<s)-my*q;
        quo=(quo<<s)+q;
        e-=s;
    }
    if(mx>=my) mx-=my,quo++; // when e==0 mx can be nearly as big as 2my
    if(mx>=my) mx-=my,quo++;
    if(mx<0) mx+=my,quo--;
    if(mx<0) mx+=my,quo--;
    if(pquo) *pquo=quo;
    return mx;
}

double WRAPPER_FUNC(fmod)(double x,double y) {
    check_nan_d2(x, y);
    ui64 ix=double2ui64(x),iy=double2ui64(y);
    int sx,ex,ey;
    i64 mx,my;
    DUNPACKS(ix,sx,ex,mx);
    DUNPACK(iy,ey,my);
    if(ex==0x7ff) return dnan_or(PINF);
    if(ey==0) return PINF;
    if(ex==0) {
        if(!disneg(x)) return PZERO;
        return MZERO;
    }
    if(ex<ey) return x;  // |x|<|y|, including case x=±0
    mx=drem_0(mx,my,ex-ey,0);
    if(sx) mx=-mx;
    return fix642double(mx,0x3ff-ey+52);
}

double WRAPPER_FUNC(remquo)(double x,double y,int*quo) {
    check_nan_d2(x, y);
    ui64 ix=double2ui64(x),iy=double2ui64(y);
    int sx,sy,ex,ey,q;
    i64 mx,my;
    DUNPACKS(ix,sx,ex,mx);
    DUNPACKS(iy,sy,ey,my);
    if(quo) *quo=0;
    if(ex==0x7ff) return PINF;
    if(ey==0)     return PINF;
    if(ex==0)     return PZERO;
    if(ey==0x7ff) return x;
    if(ex<ey-1)   return x;  // |x|<|y|/2
    if(ex==ey-1) {
        if(mx<=my) return x;   // |x|<=|y|/2, even quotient
        // here |y|/2<|x|<|y|
        if(!sx) { // x>|y|/2
            mx-=my+my;
            ey--;
            q=1;
        } else { // x<-|y|/2
            mx=my+my-mx;
            ey--;
            q=-1;
        }
    }
    else {
        if(sx) mx=-mx;
        mx=drem_0(mx,my,ex-ey,&q);
        if(mx+mx>my || (mx+mx==my&&(q&1)) ) { // |x|>|y|/2, or equality and an odd quotient?
            mx-=my;
            q++;
        }
    }
    if(sy) q=-q;
    if(quo) *quo=q;
    return fix642double(mx,0x3ff-ey+52);
}

double WRAPPER_FUNC(drem)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }

double WRAPPER_FUNC(remainder)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }

GCC_Pragma("GCC diagnostic pop") // conversion