pxfm 0.1.29

/*
 * // Copyright (c) Radzivon Bartoshyk 6/2025. All rights reserved.
 * //
 * // Redistribution and use in source and binary forms, with or without modification,
 * // are permitted provided that the following conditions are met:
 * //
 * // 1.  Redistributions of source code must retain the above copyright notice, this
 * // list of conditions and the following disclaimer.
 * //
 * // 2.  Redistributions in binary form must reproduce the above copyright notice,
 * // this list of conditions and the following disclaimer in the documentation
 * // and/or other materials provided with the distribution.
 * //
 * // 3.  Neither the name of the copyright holder nor the names of its
 * // contributors may be used to endorse or promote products derived from
 * // this software without specific prior written permission.
 * //
 * // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
use crate::common::{dyad_fmla, f_fmla, min_normal_f64};
use crate::double_double::DoubleDouble;
use crate::dyadic_float::{DyadicFloat128, DyadicSign};
use crate::rounding::CpuRound;
use crate::sin_helper::sincos_eval_dd;
use crate::sin_table::SIN_K_PI_OVER_128;
use crate::sincos_dyadic::SIN_K_PI_OVER_128_F128;
use crate::sincos_reduce::LargeArgumentReduction;

// For 2^-7 < |x| < 2^16, return k and u such that:
//   k = round(x * 128/pi)
//   x mod pi/128 = x - k * pi/128 ~ u.hi + u.lo
// Error bound:
//   |(x - k * pi/128) - (u_hi + u_lo)| <= max(ulp(ulp(u_hi)), 2^-119)
//                                      <= 2^-111.
#[inline]
pub(crate) fn range_reduction_small(x: f64) -> (DoubleDouble, u64) {
    const MPI_OVER_128: [u64; 3] = [0xbf9921fb54400000, 0xbd70b4611a600000, 0xbb43198a2e037073];
    const ONE_TWENTY_EIGHT_OVER_PI_D: f64 = f64::from_bits(0x40445f306dc9c883);
    let prod_hi = x * ONE_TWENTY_EIGHT_OVER_PI_D;
    let kd = prod_hi.cpu_round();

    // Let y = x - k * (pi/128)
    // Then |y| < pi / 256
    // With extra rounding errors, we can bound |y| < 1.6 * 2^-7.
    let y_hi = f_fmla(kd, f64::from_bits(MPI_OVER_128[0]), x); // Exact
    // |u.hi| < 1.6*2^-7
    let u_hi = f_fmla(kd, f64::from_bits(MPI_OVER_128[1]), y_hi);

    let u0 = y_hi - u_hi; // Exact
    // |u.lo| <= max(ulp(u.hi), |kd * MPI_OVER_128[2]|)
    let u1 = f_fmla(kd, f64::from_bits(MPI_OVER_128[1]), u0); // Exact
    let u_lo = f_fmla(kd, f64::from_bits(MPI_OVER_128[2]), u1);
    // Error bound:
    // |x - k * pi/128| - (u.hi + u.lo) <= ulp(u.lo)
    //                                  <= ulp(max(ulp(u.hi), kd*MPI_OVER_128[2]))
    //                                  <= 2^(-7 - 104) = 2^-111.
    (DoubleDouble::new(u_lo, u_hi), unsafe {
        kd.to_int_unchecked::<i64>() as u64 // indeterminate values is always filtered out before this call, as well only lowest bits are used
    })
}

#[inline]
pub(crate) fn get_sin_k_rational(kk: u64) -> DyadicFloat128 {
    let idx = if (kk & 64) != 0 {
        64 - (kk & 63)
    } else {
        kk & 63
    };
    let mut ans = SIN_K_PI_OVER_128_F128[idx as usize];
    if (kk & 128) != 0 {
        ans.sign = DyadicSign::Neg;
    }
    ans
}

pub(crate) struct SinCos {
    pub(crate) v_sin: DoubleDouble,
    pub(crate) v_cos: DoubleDouble,
    pub(crate) err: f64,
}

#[inline]
pub(crate) fn sincos_eval(u: DoubleDouble) -> SinCos {
    // Evaluate sin(y) = sin(x - k * (pi/128))
    // We use the degree-7 Taylor approximation:
    //   sin(y) ~ y - y^3/3! + y^5/5! - y^7/7!
    // Then the error is bounded by:
    //   |sin(y) - (y - y^3/3! + y^5/5! - y^7/7!)| < |y|^9/9! < 2^-54/9! < 2^-72.
    // For y ~ u_hi + u_lo, fully expanding the polynomial and drop any terms
    // < ulp(u_hi^3) gives us:
    //   y - y^3/3! + y^5/5! - y^7/7! = ...
    // ~ u_hi + u_hi^3 * (-1/6 + u_hi^2 * (1/120 - u_hi^2 * 1/5040)) +
    //        + u_lo (1 + u_hi^2 * (-1/2 + u_hi^2 / 24))
    let u_hi_sq = u.hi * u.hi; // Error < ulp(u_hi^2) < 2^(-6 - 52) = 2^-58.
    // p1 ~ 1/120 + u_hi^2 / 5040.
    let p1 = f_fmla(
        u_hi_sq,
        f64::from_bits(0xbf2a01a01a01a01a),
        f64::from_bits(0x3f81111111111111),
    );
    // q1 ~ -1/2 + u_hi^2 / 24.
    let q1 = f_fmla(
        u_hi_sq,
        f64::from_bits(0x3fa5555555555555),
        f64::from_bits(0xbfe0000000000000),
    );
    let u_hi_3 = u_hi_sq * u.hi;
    // p2 ~ -1/6 + u_hi^2 (1/120 - u_hi^2 * 1/5040)
    let p2 = f_fmla(u_hi_sq, p1, f64::from_bits(0xbfc5555555555555));
    // q2 ~ 1 + u_hi^2 (-1/2 + u_hi^2 / 24)
    let q2 = f_fmla(u_hi_sq, q1, 1.0);
    let sin_lo = f_fmla(u_hi_3, p2, u.lo * q2);
    // Overall, |sin(y) - (u_hi + sin_lo)| < 2*ulp(u_hi^3) < 2^-69.

    // Evaluate cos(y) = cos(x - k * (pi/128))
    // We use the degree-8 Taylor approximation:
    //   cos(y) ~ 1 - y^2/2 + y^4/4! - y^6/6! + y^8/8!
    // Then the error is bounded by:
    //   |cos(y) - (...)| < |y|^10/10! < 2^-81
    // For y ~ u_hi + u_lo, fully expanding the polynomial and drop any terms
    // < ulp(u_hi^3) gives us:
    //   1 - y^2/2 + y^4/4! - y^6/6! + y^8/8! = ...
    // ~ 1 - u_hi^2/2 + u_hi^4(1/24 + u_hi^2 (-1/720 + u_hi^2/40320)) +
    //     + u_hi u_lo (-1 + u_hi^2/6)
    // We compute 1 - u_hi^2 accurately:
    //   v_hi + v_lo ~ 1 - u_hi^2/2
    // with error <= 2^-105.
    let u_hi_neg_half = (-0.5) * u.hi;

    let (mut v_lo, v_hi);

    #[cfg(any(
        all(
            any(target_arch = "x86", target_arch = "x86_64"),
            target_feature = "fma"
        ),
        target_arch = "aarch64"
    ))]
    {
        v_hi = f_fmla(u.hi, u_hi_neg_half, 1.0);
        v_lo = 1.0 - v_hi; // Exact
        v_lo = f_fmla(u.hi, u_hi_neg_half, v_lo);
    }

    #[cfg(not(any(
        all(
            any(target_arch = "x86", target_arch = "x86_64"),
            target_feature = "fma"
        ),
        target_arch = "aarch64"
    )))]
    {
        let u_hi_sq_neg_half = DoubleDouble::from_exact_mult(u.hi, u_hi_neg_half);
        let v = DoubleDouble::from_exact_add(1.0, u_hi_sq_neg_half.hi);
        v_lo = v.lo;
        v_lo += u_hi_sq_neg_half.lo;
        v_hi = v.hi;
    }

    // r1 ~ -1/720 + u_hi^2 / 40320
    let r1 = f_fmla(
        u_hi_sq,
        f64::from_bits(0x3efa01a01a01a01a),
        f64::from_bits(0xbf56c16c16c16c17),
    );
    // s1 ~ -1 + u_hi^2 / 6
    let s1 = f_fmla(u_hi_sq, f64::from_bits(0x3fc5555555555555), -1.0);
    let u_hi_4 = u_hi_sq * u_hi_sq;
    let u_hi_u_lo = u.hi * u.lo;
    // r2 ~ 1/24 + u_hi^2 (-1/720 + u_hi^2 / 40320)
    let r2 = f_fmla(u_hi_sq, r1, f64::from_bits(0x3fa5555555555555));
    // s2 ~ v_lo + u_hi * u_lo * (-1 + u_hi^2 / 6)
    let s2 = f_fmla(u_hi_u_lo, s1, v_lo);
    let cos_lo = f_fmla(u_hi_4, r2, s2);
    // Overall, |cos(y) - (v_hi + cos_lo)| < 2*ulp(u_hi^4) < 2^-75.

    let sin_u = DoubleDouble::from_exact_add(u.hi, sin_lo);
    let cos_u = DoubleDouble::from_exact_add(v_hi, cos_lo);

    let err = f_fmla(
        u_hi_3,
        f64::from_bits(0x3cc0000000000000),
        f64::from_bits(0x3960000000000000),
    );

    SinCos {
        v_sin: sin_u,
        v_cos: cos_u,
        err,
    }
}

#[cold]
#[inline(never)]
fn sin_accurate(y: DoubleDouble, sin_k: DoubleDouble, cos_k: DoubleDouble) -> f64 {
    let r_sincos = sincos_eval_dd(y);

    // k is an integer and -pi / 256 <= y <= pi / 256.
    // Then sin(x) = sin((k * pi/128 + y)
    //             = sin(y) * cos(k*pi/128) + cos(y) * sin(k*pi/128)

    let sin_k_cos_y = DoubleDouble::quick_mult(r_sincos.v_cos, sin_k);
    let cos_k_sin_y = DoubleDouble::quick_mult(r_sincos.v_sin, cos_k);

    let rr = DoubleDouble::full_dd_add(sin_k_cos_y, cos_k_sin_y);
    rr.to_f64()
}

#[cold]
pub(crate) fn sin_accurate_near_zero(x: f64) -> f64 {
    // Generated by Sollya:
    // d = [0, 0.0490874];
    // f_sin = sin(y)/y;
    // Q = fpminimax(f_sin, [|0, 2, 4, 6, 8, 10, 12|], [|107...|], d, relative, floating);
    const C: [(u64, u64); 7] = [
        (0x0000000000000000, 0x3ff0000000000000),
        (0xbc65555555555217, 0xbfc5555555555555),
        (0x3c011110f7d49e8c, 0x3f81111111111111),
        (0xbb65e5b30986fc80, 0xbf2a01a01a01a01a),
        (0xbb689e86245bd566, 0x3ec71de3a556c6e5),
        (0xbaccb3d55ccfca58, 0xbe5ae64567954935),
        (0x3a6333ef7a00ce40, 0x3de6120ca00ce3a2),
    ];
    let x2 = DoubleDouble::from_exact_mult(x, x);
    let mut p = DoubleDouble::quick_mul_add(
        x2,
        DoubleDouble::from_bit_pair(C[6]),
        DoubleDouble::from_bit_pair(C[5]),
    );
    p = DoubleDouble::quick_mul_add(x2, p, DoubleDouble::from_bit_pair(C[4]));
    p = DoubleDouble::quick_mul_add(x2, p, DoubleDouble::from_bit_pair(C[3]));
    p = DoubleDouble::quick_mul_add(x2, p, DoubleDouble::from_bit_pair(C[2]));
    p = DoubleDouble::quick_mul_add(x2, p, DoubleDouble::from_bit_pair(C[1]));
    p = DoubleDouble::quick_mul_add_f64(x2, p, f64::from_bits(0x3ff0000000000000));
    p = DoubleDouble::quick_mult_f64(p, x);
    p.to_f64()
}

/// Sine for double precision
///
/// ULP 0.5
pub fn f_sin(x: f64) -> f64 {
    let x_e = (x.to_bits() >> 52) & 0x7ff;
    const E_BIAS: u64 = (1u64 << (11 - 1u64)) - 1u64;

    let y: DoubleDouble;
    let k;

    let mut argument_reduction = LargeArgumentReduction::default();

    if x_e < E_BIAS + 16 {
        // |x| < 2^32 (with FMA) or |x| < 2^23 (w/o FMA)
        let ax = x.to_bits() & 0x7fff_ffff_ffff_ffff;
        if ax <= 0x3fa921fbd34a9550 {
            // |x| <= 0.0490874
            if x_e < E_BIAS - 26 {
                // |x| < 2^-26
                if x == 0.0 {
                    // Signed zeros.
                    return x;
                }

                // For |x| < 2^-26, |sin(x) - x| < ulp(x)/2.
                return dyad_fmla(x, f64::from_bits(0xbc90000000000000), x);
            }

            // Polynomial for sin(x)/x
            // Generated by Sollya:
            // d = [0, 0.0490874];
            // f_sin = sin(y)/y;
            // Q = fpminimax(f_sin, [|0, 2, 4, 6, 8|], [|1, D...|], d, relative, floating);

            let x2 = x * x;
            let x4 = x2 * x2;
            const C: [u64; 4] = [
                0xbfc5555555555555,
                0x3f8111111110e45a,
                0xbf2a019ffd7fdaaf,
                0x3ec71819b9bf01ef,
            ];
            let p01 = f_fmla(x2, f64::from_bits(C[1]), f64::from_bits(C[0]));
            let p23 = f_fmla(x2, f64::from_bits(C[3]), f64::from_bits(C[2]));
            let w0 = f_fmla(x4, p23, p01);
            let w1 = x2 * w0 * x;
            let r = DoubleDouble::from_exact_add(x, w1);
            let err = f_fmla(
                x2,
                f64::from_bits(0x3cb0000000000000), // 2^-52
                f64::from_bits(0x3be0000000000000), // 2^-65
            );
            let ub = r.hi + (r.lo + err);
            let lb = r.hi + (r.lo - err);
            if ub == lb {
                return ub;
            }
            return sin_accurate_near_zero(x);
        }

        // Small range reduction.
        (y, k) = range_reduction_small(x);
    } else {
        // Inf or NaN
        if x_e > 2 * E_BIAS {
            // sin(+-Inf) = NaN
            return x + f64::NAN;
        }

        // Large range reduction.
        (k, y) = argument_reduction.reduce(x);
    }

    let r_sincos = sincos_eval(y);

    // cos(k * pi/128) = sin(k * pi/128 + pi/2) = sin((k + 64) * pi/128).
    let sk = SIN_K_PI_OVER_128[(k & 255) as usize];
    let ck = SIN_K_PI_OVER_128[((k.wrapping_add(64)) & 255) as usize];

    let sin_k = DoubleDouble::from_bit_pair(sk);
    let cos_k = DoubleDouble::from_bit_pair(ck);

    let sin_k_cos_y = DoubleDouble::quick_mult(r_sincos.v_cos, sin_k);
    let cos_k_sin_y = DoubleDouble::quick_mult(r_sincos.v_sin, cos_k);

    // sin_k_cos_y is always >> cos_k_sin_y
    let mut rr = DoubleDouble::from_exact_add(sin_k_cos_y.hi, cos_k_sin_y.hi);
    rr.lo += sin_k_cos_y.lo + cos_k_sin_y.lo;

    let rlp = rr.lo + r_sincos.err;
    let rlm = rr.lo - r_sincos.err;

    let r_upper = rr.hi + rlp; // (rr.lo + ERR);
    let r_lower = rr.hi + rlm; // (rr.lo - ERR);

    // Ziv's accuracy test
    if r_upper == r_lower {
        return rr.to_f64();
    }

    sin_accurate(y, sin_k, cos_k)
}

#[cold]
#[inline(never)]
fn cos_accurate(y: DoubleDouble, msin_k: DoubleDouble, cos_k: DoubleDouble) -> f64 {
    let r_sincos = sincos_eval_dd(y);

    // After range reduction, k = round(x * 128 / pi) and y = x - k * (pi / 128).
    // So k is an integer and -pi / 256 <= y <= pi / 256.
    // Then sin(x) = sin((k * pi/128 + y)
    //             = sin(y) * cos(k*pi/128) + cos(y) * sin(k*pi/128)

    let sin_k_cos_y = DoubleDouble::quick_mult(r_sincos.v_cos, cos_k);
    let cos_k_sin_y = DoubleDouble::quick_mult(r_sincos.v_sin, msin_k);

    let rr = DoubleDouble::full_dd_add(sin_k_cos_y, cos_k_sin_y);
    rr.to_f64()
}

#[cold]
pub(crate) fn cos_accurate_near_zero(x: f64) -> f64 {
    // Generated by Sollya:
    // d = [0, 0.0490874];
    // f_sin = sin(y)/y;
    // Q = fpminimax(f_sin, [|0, 2, 4, 6, 8, 10|], [|107...|], d, relative, floating);
    const C: [(u64, u64); 6] = [
        (0xba2fa1c000000000, 0x3ff0000000000000),
        (0x3b1cd6ead3800000, 0xbfe0000000000000),
        (0x3c45112931063916, 0x3fa5555555555555),
        (0xbba1c1e3ab3b09e0, 0xbf56c16c16c16b2b),
        (0x3b937bc2f4ea7db6, 0x3efa01a019495fca),
        (0x3b307fd5e1b021b3, 0xbe927e0d57d894f8),
    ];
    let x2 = DoubleDouble::from_exact_mult(x, x);
    let mut p = DoubleDouble::quick_mul_add(
        x2,
        DoubleDouble::from_bit_pair(C[5]),
        DoubleDouble::from_bit_pair(C[4]),
    );
    p = DoubleDouble::quick_mul_add(x2, p, DoubleDouble::from_bit_pair(C[3]));
    p = DoubleDouble::quick_mul_add(x2, p, DoubleDouble::from_bit_pair(C[2]));
    p = DoubleDouble::quick_mul_add(x2, p, DoubleDouble::from_bit_pair(C[1]));
    p = DoubleDouble::quick_mul_add(x2, p, DoubleDouble::from_bit_pair(C[0]));
    p.to_f64()
}

/// Cosine for double precision
///
/// ULP 0.5
pub fn f_cos(x: f64) -> f64 {
    let x_e = (x.to_bits() >> 52) & 0x7ff;
    const E_BIAS: u64 = (1u64 << (11 - 1u64)) - 1u64;

    let y: DoubleDouble;
    let k;

    let mut argument_reduction = LargeArgumentReduction::default();

    if x_e < E_BIAS + 16 {
        // |x| < 2^32 (with FMA) or |x| < 2^23 (w/o FMA)
        let ax = x.to_bits() & 0x7fff_ffff_ffff_ffff;
        if ax <= 0x3fa921fbd34a9550 {
            // |x| <= 0.0490874
            if x_e < E_BIAS - 27 {
                // |x| < 2^-27
                if x == 0.0 {
                    // Signed zeros.
                    return 1.0;
                }
                // For |x| < 2^-26, |sin(x) - x| < ulp(x)/2.
                return 1.0 - min_normal_f64();
            }

            // Polynomial for cos(x)
            // Generated by Sollya:
            // d = [0, 0.0490874];
            // f_cos = cos(y);
            // Q = fpminimax(f_cos, [|0, 2, 4, 6, 8|], [|1, D...|], d, relative, floating);

            let x2 = x * x;
            let x4 = x2 * x2;
            const C: [u64; 4] = [
                0xbfe0000000000000,
                0x3fa55555555554a4,
                0xbf56c16c1619b84a,
                0x3efa013d3d01cf7f,
            ];
            let p01 = f_fmla(x2, f64::from_bits(C[1]), f64::from_bits(C[0]));
            let p23 = f_fmla(x2, f64::from_bits(C[3]), f64::from_bits(C[2]));
            let w0 = f_fmla(x4, p23, p01);
            let w1 = x2 * w0;
            let r = DoubleDouble::from_exact_add(1., w1);
            let err = f_fmla(
                x2,
                f64::from_bits(0x3cb0000000000000), // 2^-52
                f64::from_bits(0x3be0000000000000), // 2^-65
            );

            let ub = r.hi + (r.lo + err);
            let lb = r.hi + (r.lo - err);
            if ub == lb {
                return ub;
            }
            return cos_accurate_near_zero(x);
        } else {
            // // Small range reduction.
            (y, k) = range_reduction_small(x);
        }
    } else {
        // Inf or NaN
        if x_e > 2 * E_BIAS {
            // cos(+-Inf) = NaN
            return x + f64::NAN;
        }

        // Large range reduction.
        (k, y) = argument_reduction.reduce(x);
    }
    let r_sincos = sincos_eval(y);

    // After range reduction, k = round(x * 128 / pi) and y = x - k * (pi / 128).
    // So k is an integer and -pi / 256 <= y <= pi / 256.
    // Then cos(x) = cos((k * pi/128 + y)
    //             = cos(y) * cos(k*pi/128) - sin(y) * sin(k*pi/128)
    let sk = SIN_K_PI_OVER_128[(k.wrapping_add(128) & 255) as usize];
    let ck = SIN_K_PI_OVER_128[((k.wrapping_add(64)) & 255) as usize];
    let msin_k = DoubleDouble::from_bit_pair(sk);
    let cos_k = DoubleDouble::from_bit_pair(ck);

    let cos_k_cos_y = DoubleDouble::quick_mult(r_sincos.v_cos, cos_k);
    let cos_k_msin_y = DoubleDouble::quick_mult(r_sincos.v_sin, msin_k);

    // cos_k_cos_y is always >> cos_k_msin_y
    let mut rr = DoubleDouble::from_exact_add(cos_k_cos_y.hi, cos_k_msin_y.hi);
    rr.lo += cos_k_cos_y.lo + cos_k_msin_y.lo;
    let rlp = rr.lo + r_sincos.err;
    let rlm = rr.lo - r_sincos.err;

    let r_upper = rr.hi + rlp; // (rr.lo + ERR);
    let r_lower = rr.hi + rlm; // (rr.lo - ERR);

    // Ziv's accuracy test
    if r_upper == r_lower {
        return rr.to_f64();
    }
    cos_accurate(y, msin_k, cos_k)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn cos_test() {
        assert_eq!(f_cos(0.0), 1.0);
        assert_eq!(f_cos(0.0024312312), 0.9999970445588818);
        assert_eq!(f_cos(1.0), 0.5403023058681398);
        assert_eq!(f_cos(-0.5), 0.8775825618903728);
        assert!(f_cos(f64::INFINITY).is_nan());
        assert!(f_cos(f64::NEG_INFINITY).is_nan());
        assert!(f_cos(f64::NAN).is_nan());
    }

    #[test]
    fn sin_test() {
        assert_eq!(f_sin(0.00000002149119332890786), 0.00000002149119332890786);
        assert_eq!(f_sin(2.8477476437362989E-306), 2.8477476437362989E-306);
        assert_eq!(f_sin(0.0024312312), 0.002431228804879309);
        assert_eq!(f_sin(0.0), 0.0);
        assert_eq!(f_sin(1.0), 0.8414709848078965);
        assert_eq!(f_sin(-0.5), -0.479425538604203);
        assert!(f_sin(f64::INFINITY).is_nan());
        assert!(f_sin(f64::NEG_INFINITY).is_nan());
        assert!(f_sin(f64::NAN).is_nan());
    }
}