r2rs-nmath 0.1.1

// Translation of nmath's bessel_i
//
// Copyright (C) 2020 neek-sss
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

use num_traits::{Float, ToPrimitive};
use strafe_type::{FloatConstraint, Positive64, Real64};

use crate::{
    func::{
        bessel::{bessel_k, bessel_k_ex},
        betagam::gamma_cody,
    },
    traits::TrigPI,
};

pub fn bessel_i<P: Into<Positive64>, R: Into<Real64>>(x: P, alpha: R, expo: bool) -> Real64 {
    let mut x = x.into().unwrap();
    let mut alpha = alpha.into().unwrap();

    let mut nb = 0;
    let mut ncalc = 0;
    let mut ize = 0;
    let mut na = 0.0;

    ize = if expo { 2 } else { 1 };
    na = alpha.to_f64().unwrap().floor();
    if alpha < 0.0 {
        /* Using Abramowitz & Stegun  9.6.2 & 9.6.6
         * this may not be quite optimal (CPU and accuracy wise) */
        return (bessel_i(x, -alpha, expo).unwrap()
            + if alpha == na {
                0.0
            } else {
                bessel_k(x, -alpha, expo).unwrap()
                    * (if ize == 1 {
                        2.0
                    } else {
                        (2.0) * (-2.0 * x).exp()
                    })
                    / std::f64::consts::PI
                    * (-alpha).sin_pi()
            })
        .into();
    } /* nb-1 <= alpha < nb */
    nb = 1 + na as usize;
    alpha -= nb as f64 - 1.0;
    // vmax = vmaxget();
    let mut bi = vec![0.0; nb];
    // bi = R_alloc(
    // nb as u64,
    // ::std::mem::size_of::<f64>() as u64,
    // ) as *mut f64;
    I_bessel(&mut x, &mut alpha, &mut nb, &mut ize, &mut bi, &mut ncalc);
    if ncalc != nb as i32 {
        /* error input */
        if ncalc < 0 {
            warn!(
                "bessel_i({}): ncalc (={}) != nb (={}); alpha={}. Arg. out of range?",
                x.to_f64().unwrap(),
                ncalc,
                nb,
                alpha.to_f64().unwrap()
            );
        } else {
            warn!(
                "bessel_i({},nu={}): precision lost in result",
                x.to_f64().unwrap(),
                alpha + nb as f64 - 1.0
            );
        }
    }
    x = bi[nb - 1];
    // vmaxset(vmax);
    x.into()
}

/// Modified version of `bessel_i` that accepts a work array instead of
/// allocating one.
pub fn bessel_i_ex(mut x: f64, mut alpha: f64, expo: f64, mut bi: &mut [f64]) -> f64 {
    let mut nb = 0;
    let mut ncalc = 0;
    let mut ize = 0;
    let mut na = 0.0;
    /* NaNs propagated correctly */
    if x.is_nan() || alpha.is_nan() {
        return x + alpha;
    }
    if x < 0.0 {
        warn!("value out of range in bessel_i");
        return f64::nan();
    }
    ize = expo as i32;
    na = alpha.floor();
    if alpha < 0.0 {
        /* Using Abramowitz & Stegun  9.6.2 & 9.6.6
         * this may not be quite optimal (CPU and accuracy wise) */
        return bessel_i_ex(x, -alpha, expo, bi)
            + (if alpha == na {
                0.0
            } else {
                (bessel_k_ex(x, -alpha, expo, bi)
                    * (if ize == 1 {
                        2.0
                    } else {
                        (2.0) * (-2.0 * x).exp()
                    })
                    / std::f64::consts::PI)
                    * (-alpha).sin_pi()
            });
    } /* nb-1 <= alpha < nb */
    nb = 1 + na as usize;
    alpha -= nb as f64 - 1.0;
    I_bessel(&mut x, &mut alpha, &mut nb, &mut ize, &mut bi, &mut ncalc);
    if ncalc != nb as i32 {
        /* error input */
        if ncalc < 0 {
            warn!(
                "bessel_i({}): ncalc (={}) != nb (={}); alpha={}. Arg. out of range?",
                x, ncalc, nb, alpha
            );
        } else {
            warn!(
                "bessel_i({},nu={}): precision lost in result",
                x,
                alpha + nb as f64 - 1.0
            );
        }
    }
    x = bi[nb - 1];
    return x;
}

/// From <http://www.netlib.org/specfun/ribesl> Fortran translated by f2c,...
/// ------------------------------=#---- Martin Maechler, ETH Zurich
/// -------------------------------------------------------------------
///
/// This routine calculates Bessel functions I_(N+ALPHA) (X)
/// for non-negative argument X, and non-negative order N+ALPHA,
/// with or without exponential scaling.
///
///
/// Explanation of variables in the calling sequence
///
/// X     - Non-negative argument for which
/// I's or exponentially scaled I's (I*EXP(-X))
/// are to be calculated. If I's are to be calculated,
/// X must be less than exparg_BESS (IZE=1) or xlrg_BESS_IJ (IZE=2),
/// (see bessel.h).
/// ALPHA - Fractional part of order for which
/// I's or exponentially scaled I's (I*EXP(-X)) are
/// to be calculated.  0 <= ALPHA < 1.0.
/// NB    - Number of functions to be calculated, NB > 0.
/// The first function calculated is of order ALPHA, and the
/// last is of order (NB - 1 + ALPHA).
/// IZE   - Type. IZE = 1 if unscaled I's are to be calculated,
/// = 2 if exponentially scaled I's are to be calculated.
/// BI    - Output vector of length NB. If the routine
/// terminates normally (NCALC=NB), the vector BI contains the
/// functions I(ALPHA,X) through I(NB-1+ALPHA,X), or the
/// corresponding exponentially scaled functions.
/// NCALC - Output variable indicating possible errors.
/// Before using the vector BI, the user should check that
/// NCALC=NB, i.e., all orders have been calculated to
/// the desired accuracy. See error returns below.
///
///
/// *******************************************************************
/// *******************************************************************
///
/// Error returns
///
/// In case of an error, NCALC != NB, and not all I's are
/// calculated to the desired accuracy.
///
/// NCALC < 0:  An argument is out of range. For example,
/// NB <= 0, IZE is not 1 or 2, or IZE=1 and ABS(X) >= EXPARG_BESS.
/// In this case, the BI-vector is not calculated, and NCALC is
/// set to MIN0(NB,0)-1 so that NCALC != NB.
///
/// NB > NCALC > 0: Not all requested function values could
/// be calculated accurately. This usually occurs because NB is
/// much larger than ABS(X).  In this case, BI\[N\] is calculated
/// to the desired accuracy for N <= NCALC, but precision
/// is lost for NCALC < N <= NB.  If BI\[N\] does not vanish
/// for N > NCALC (because it is too small to be represented),
/// and BI\[N\]/BI\[NCALC\] = 10**(-K), then only the first NSIG-K
/// significant figures of BI\[N\] can be trusted.
fn I_bessel(
    x: &mut f64,
    alpha: &mut f64,
    nb: &mut usize,
    ize: &mut i32,
    bi: &mut [f64],
    ncalc: &mut i32,
) {
    /*-------------------------------------------------------------------
    Mathematical constants
    -------------------------------------------------------------------*/
    static const__: f64 = 1.585;

    /* Local variables */
    let mut nend = 0;
    let mut intx = 0;
    let mut nbmx = 0;
    let mut k = 0;
    let mut l = 0;
    let mut n = 0;
    let mut nstart = 0;
    let mut pold = 0.0;
    let mut test = 0.0;
    let mut p = 0.0;
    let mut em = 0.0;
    let mut en = 0.0;
    let mut empal = 0.0;
    let mut emp2al = 0.0;
    let mut halfx = 0.0;
    let mut aa = 0.0;
    let mut bb = 0.0;
    let mut cc = 0.0;
    let mut psave = 0.0;
    let mut plast = 0.0;
    let mut tover = 0.0;
    let mut psavel = 0.0;
    let mut sum = 0.0;
    let mut nu = 0.0;
    let mut twonu = 0.0;

    nu = *alpha;
    twonu = nu + nu;

    /*-------------------------------------------------------------------
    Check for X, NB, OR IZE out of range.
    ------------------------------------------------------------------- */
    if *nb > 0 && *x >= 0.0 && (0.0 <= nu && nu < 1.0) && (1 <= *ize && *ize <= 2) {
        *ncalc = *nb as i32; /* argument out of range */

        if *ize == 1 && *x > 709.0 {
            k = 1; /* the limit *is* = Inf */
            while k <= *nb {
                bi[k - 1] = f64::infinity(); /* The limit (-x).exp() * I_nu(x) --> 0 : */
                k += 1
            } /* fine, since *x <= xlrg_BESS_IJ <<< LONG_MAX */
            return;
        } /* small x  < 1e-4 */
        if *ize == 2 && *x > 1e5 {
            k = 1;
            while k <= *nb {
                bi[k - 1] = 0.0;
                k += 1
            }
            return;
        }
        intx = *x as usize;
        if *x >= 1e-4 {
            let mut current_block_152: u64;
            /* "non-small" x ( >= 1e-4 ) */
            /* -------------------------------------------------------------------
            Initialize the forward sweep, the P-sequence of Olver
            ------------------------------------------------------------------- */
            nbmx = *nb as i32 - intx as i32;
            n = intx + 1;
            en = (n + n) as f64 + twonu;
            plast = 1.0;
            p = en / *x;
            /* ------------------------------------------------
            Calculate general significance test
            ------------------------------------------------ */
            test = 1e16 + 1e16;
            if intx << 1 > 16 * 5 {
                test = (test * p).sqrt()
            } else {
                test /= const__.powi(intx as i32)
            }
            if nbmx >= 3 {
                /* --------------------------------------------------
                Calculate P-sequence until N = NB-1
                Check for possible overflow.
                ------------------------------------------------ */
                tover = 1e308 / 1e16;
                nstart = intx + 2;
                nend = *nb as i32 - 1;
                k = nstart;
                loop {
                    if !(k as i32 <= nend) {
                        current_block_152 = 7494008139977416618;
                        break;
                    }
                    n = k;
                    en += 2.0;
                    pold = plast;
                    plast = p;
                    p = en * plast / *x + pold;
                    if p > tover {
                        /* ------------------------------------------------
                        To avoid overflow, divide P-sequence by TOVER.
                        Calculate P-sequence until ABS(P) > 1.
                        ---------------------------------------------- */
                        tover = 1e308;
                        p /= tover;
                        plast /= tover;
                        psave = p;
                        psavel = plast;
                        nstart = n + 1;
                        loop {
                            n += 1;
                            en += 2.0;
                            pold = plast;
                            plast = p;
                            p = en * plast / *x + pold;
                            if !(p <= 1.0) {
                                break;
                            }
                        }
                        bb = en / *x;
                        /* ------------------------------------------------
                        Calculate backward test, and find NCALC,
                        the highest N such that the test is passed.
                        ------------------------------------------------ */
                        test = pold * plast / 1e16;
                        test *= 0.5 - 0.5 / (bb * bb);
                        p = plast * tover;
                        n -= 1;
                        en -= 2.0;
                        nend = if *nb <= n { *nb } else { n } as i32;
                        l = nstart;
                        loop {
                            if !(l as i32 <= nend) {
                                current_block_152 = 3392087639489470149;
                                break;
                            }
                            *ncalc = l as i32;
                            pold = psavel;
                            psavel = psave;
                            psave = en * psavel / *x + pold;
                            if psave * psavel > test {
                                current_block_152 = 9422125980347120905;
                                break;
                            }
                            l += 1
                        }
                        match current_block_152 {
                            3392087639489470149 => *ncalc = nend + 1,
                            _ => {}
                        }
                        *ncalc -= 1;
                        current_block_152 = 18177119835917145477;
                        break;
                    } else {
                        k += 1
                    }
                }
                match current_block_152 {
                    18177119835917145477 => {}
                    _ => {
                        n = nend as usize;
                        en = (n + n) as f64 + twonu;
                        /*---------------------------------------------------
                        Calculate special significance test for NBMX > 2.
                        --------------------------------------------------- */
                        test = test.max((plast * 1e16).sqrt() * (p + p).sqrt());
                        current_block_152 = 1868291631715963762;
                    }
                }
            } else {
                current_block_152 = 1868291631715963762;
            }
            match current_block_152 {
                1868291631715963762 => {
                    loop
                    /* --------------------------------------------------------
                    Calculate P-sequence until significance test passed.
                    -------------------------------------------------------- */
                    {
                        n += 1;
                        en += 2.0;
                        pold = plast;
                        plast = p;
                        p = en * plast / *x + pold;
                        if !(p < test) {
                            break;
                        }
                    }
                }
                _ => {}
            }
            /* -------------------------------------------------------------------
            Initialize the backward recursion and the normalization sum.
            ------------------------------------------------------------------- */
            n += 1;
            en += 2.0;
            bb = 0.0;
            aa = 1.0 / p;
            em = n as f64 - 1.0;
            empal = em + nu;
            emp2al = em - 1.0 + twonu;
            sum = aa * empal * emp2al / em;
            nend = (n - *nb) as i32;
            if nend < 0 {
                /* -----------------------------------------------------
                N < NB, so store BI[N] and set higher orders to 0..
                ----------------------------------------------------- */
                bi[n - 1] = aa;
                nend = -nend;
                l = 1;
                while l as i32 <= nend {
                    bi[(n + l) - 1] = 0.0;
                    l += 1
                }
                current_block_152 = 3042404941205427226;
            } else {
                if nend > 0 {
                    /* -----------------------------------------------------
                    Recur backward via difference equation,
                    calculating (but not storing) BI[N], until N = NB.
                    --------------------------------------------------- */
                    l = 1;
                    while l as i32 <= nend {
                        n -= 1;
                        en -= 2.0;
                        cc = bb;
                        bb = aa;
                        /* for x ~= 1500,  sum would overflow to 'inf' here,
                         * and the final bi[] /= sum would give 0 wrongly;
                         * RE-normalize (aa, sum) here -- no need to undo */
                        if nend > 100 && aa > 1e200 {
                            /* multiply by  2^-900 = 1.18e-271 */
                            cc = cc * (-900.0_f64).exp2();
                            bb = bb * (-900.0_f64).exp2();
                            sum = sum * (-900.0_f64).exp2();
                        }
                        aa = en * bb / *x + cc;
                        em -= 1.0;
                        emp2al -= 1.0;
                        if n == 1 {
                            break;
                        }
                        if n == 2 {
                            emp2al = 1.0
                        }
                        empal -= 1.0;
                        sum = (sum + aa * empal) * emp2al / em;
                        l += 1
                    }
                }
                /* ---------------------------------------------------
                Store BI[NB]
                --------------------------------------------------- */
                bi[n - 1] = aa;
                if *nb <= 1 {
                    sum = sum + sum + aa;
                    current_block_152 = 17544876657988546940;
                } else {
                    /* -------------------------------------------------
                    Calculate and Store BI[NB-1]
                    ------------------------------------------------- */
                    n -= 1;
                    en -= 2.0;
                    bi[n - 1] = en * aa / *x + bb;
                    if n == 1 {
                        current_block_152 = 11039285166445273245;
                    } else {
                        em -= 1.0;
                        if n == 2 {
                            emp2al = 1.0
                        } else {
                            emp2al -= 1.0
                        }
                        empal -= 1.0;
                        sum = (sum + bi[n - 1] * empal) * emp2al / em;
                        current_block_152 = 3042404941205427226;
                    }
                }
            }
            match current_block_152 {
                3042404941205427226 => {
                    nend = (n - 2) as i32;
                    if nend > 0 {
                        /* --------------------------------------------
                        Calculate via difference equation
                        and store BI[N], until N = 2.
                        ------------------------------------------ */
                        l = 1;
                        while l as i32 <= nend {
                            n -= 1;
                            en -= 2.0;
                            bi[n - 1] = en * bi[(n + 1) - 1] / *x + bi[(n + 2) - 1];
                            em -= 1.0;
                            if n == 2 {
                                emp2al = 1.0
                            } else {
                                emp2al -= 1.0
                            }
                            empal -= 1.0;
                            sum = (sum + bi[n - 1] * empal) * emp2al / em;
                            l += 1
                        }
                    }
                    /* ----------------------------------------------
                    Calculate BI[1]
                    -------------------------------------------- */
                    bi[0] = 2.0 * empal * bi[1] / *x + bi[2];
                    current_block_152 = 11039285166445273245;
                }
                _ => {}
            }
            match current_block_152 {
                11039285166445273245 => sum = sum + sum + bi[0],
                _ => {}
            }
            /* ---------------------------------------------------------
            Normalize.  Divide all BI[N] by sum.
            --------------------------------------------------------- */
            if nu != 0.0 {
                sum *= gamma_cody(1.0 + nu).unwrap() * (*x * 0.5).powf(-nu)
            }
            if *ize == 1 {
                sum *= (-*x).exp()
            }
            aa = 8.9e-308;
            if sum > 1.0 {
                aa *= sum
            }
            n = 1;
            while n <= *nb {
                if bi[n - 1] < aa {
                    bi[n - 1] = 0.0
                } else {
                    bi[n - 1] /= sum
                }
                n += 1
            }
            return;
        } else {
            /* -----------------------------------------------------------
            Two-term ascending series for small X.
            -----------------------------------------------------------*/
            aa = 1.0;
            empal = 1.0 + nu;
            /* No need to check for underflow */
            halfx = 0.5 * *x;
            if nu != 0.0 {
                aa = halfx.powf(nu) / gamma_cody(empal).unwrap()
            }
            if *ize == 2 {
                aa *= (-*x).exp()
            }
            bb = halfx * halfx;
            bi[0] = aa + aa * bb / empal;
            if *x != 0.0 && bi[0] == 0.0 {
                *ncalc = 0
            }
            if *nb > 1 {
                if *x == 0.0 {
                    n = 2;
                    while n <= *nb {
                        bi[n - 1] = 0.0;
                        n += 1
                    }
                } else {
                    /* -------------------------------------------------
                    Calculate higher-order functions.
                    ------------------------------------------------- */
                    cc = halfx;
                    tover = (8.9e-308 + 8.9e-308) / *x;
                    if bb != 0.0 {
                        tover = 8.9e-308 / bb
                    }
                    n = 2;
                    while n <= *nb {
                        aa /= empal;
                        empal += 1.0;
                        aa *= cc;
                        if aa <= tover * empal {
                            aa = 0.0;
                            bi[n - 1] = aa
                        } else {
                            bi[n - 1] = aa + aa * bb / empal
                        }
                        if bi[n - 1] == 0.0 && *ncalc > n as i32 {
                            *ncalc = (n - 1) as i32
                        }
                        n += 1
                    }
                }
            }
        }
    } else {
        *ncalc = (if *nb == 0 { *nb } else { 0 }) as i32 - 1
    };
}