r2rs-stats 0.1.1

// "Whatever you do, work at it with all your heart, as working for the Lord,
// not for human masters, since you know that you will receive an inheritance
// from the Lord as a reward. It is the Lord Christ you are serving."
// (Col 3:23-24)

/* Nelder-Mead, based on Pascal code
in J.C. Nash, `Compact Numerical Methods for Computers', 2nd edition,
converted by p2c then re-crafted by B.D. Ripley */
use std::{convert::TryFrom, error::Error};

use nalgebra::DMatrix;
use num_traits::Float;

use crate::funcs::opt::{fminfn::fminfn, Optim, OptimOptions, OptimResponse};

pub struct NelderMead {
    pub alpha: f64,
    pub beta: f64,
    pub gamma: f64,
    pub optim_options: OptimOptions,
}

impl Default for NelderMead {
    fn default() -> Self {
        Self {
            alpha: 1.0,
            beta: 0.5,
            gamma: 2.0,
            optim_options: OptimOptions {
                parscale: None,
                fnscale: 1.0,
                abstol: f64::neg_infinity(),
                reltol: f64::epsilon().sqrt(),
                maxit: 500,
            },
        }
    }
}

impl Optim for NelderMead {
    type ParameterType = Vec<f64>;
    fn optim(
        &mut self,
        par: Self::ParameterType,
        min_func: &dyn Fn(Self::ParameterType) -> f64,
    ) -> Result<OptimResponse<Self::ParameterType>, Box<dyn Error>> {
        let npar = par.len();
        let parscale = if let Some(parscale) = &self.optim_options.parscale {
            parscale.clone()
        } else {
            vec![1.0; npar]
        };
        let mut bvec = par
            .iter()
            .zip(parscale.iter())
            .map(|(par, scale)| par / scale)
            .collect::<Vec<_>>();

        let mut grcount = 0;
        let mut fncount = 0;
        let mut min = f64::nan();
        let mut par_out = Vec::new();
        let mut fail = true;
        (min, par_out, fncount, fail) = nmmin(
            npar,
            &mut bvec,
            min_func,
            &parscale,
            self.optim_options.fnscale,
            self.optim_options.abstol,
            self.optim_options.reltol,
            self.alpha,
            self.beta,
            self.gamma,
            self.optim_options.maxit,
        )?;

        for i in 0..npar {
            par_out[i] *= parscale[i];
        }

        Ok(OptimResponse {
            parameters: par_out,
            value: min,
            function_count: fncount,
            gradient_count: grcount,
            convergence: !fail,
        })
    }
}

const BIG: f64 = 1.0e35;

pub fn nmmin(
    n: usize,
    Bvec: &mut [f64],
    min_func: &dyn Fn(Vec<f64>) -> f64,
    parscale: &[f64],
    fnscale: f64,
    abstol: f64,
    intol: f64,
    alpha: f64,
    beta: f64,
    gamma: f64,
    maxit: usize,
) -> Result<(f64, Vec<f64>, usize, bool), Box<dyn Error>> {
    let mut fail = false;
    if maxit == 0 {
        Ok((
            fminfn(n, Bvec, parscale, fnscale, min_func),
            Vec::new(),
            0,
            fail,
        ))
    } else {
        let mut P = DMatrix::<f64>::zeros(n + 1, n + 2);
        let mut f: f64 = fminfn(n, Bvec, parscale, fnscale, min_func);

        if !f.is_finite() {
            Err(Box::try_from("Could not be evaluated at initial parameters").unwrap())
        } else {
            let mut funcount = 1;
            let convtol = intol * (f.abs() + intol);

            let n1 = n + 1;
            let C = n + 2;
            P[(n1 - 1, 0)] = f;

            for i in 0..n {
                P[(i, 0)] = Bvec[i];
            }

            let mut L = 1;
            let mut size = 0.0;

            let mut step = 0.0;
            for Bvec_i in &Bvec[0..n] {
                if 0.1 * Bvec_i.abs() > step {
                    step = 0.1 * Bvec_i.abs();
                }
            }
            if step == 0.0 {
                step = 0.1;
            }

            // Build
            for j in 2..=n1 {
                for i in 0..n {
                    P[(i, j - 1)] = Bvec[i];
                }

                let mut trystep = step;
                while P[(j - 2, j - 1)] == Bvec[j - 2] {
                    P[(j - 2, j - 1)] = Bvec[j - 2] + trystep;
                    trystep *= 10.0;
                }
                size += trystep;
            }

            let mut oldsize = size;
            let mut calcvert = true;

            loop {
                if calcvert {
                    for j in 0..n1 {
                        if j + 1 != L {
                            for i in 0..n {
                                Bvec[i] = P[(i, j)];
                            }
                            f = fminfn(n, Bvec, parscale, fnscale, min_func);
                            if !f.is_finite() {
                                f = BIG;
                            }
                            funcount += 1;
                            P[(n1 - 1, j)] = f;
                        }
                    }
                    calcvert = false;
                }

                let mut VL = P[(n1 - 1, L - 1)];
                let mut VH = VL;
                let mut H = L;

                for j in 1..=n1 {
                    if j != L {
                        f = P[(n1 - 1, j - 1)];
                        if f < VL {
                            L = j;
                            VL = f;
                        }
                        if f > VH {
                            H = j;
                            VH = f;
                        }
                    }
                }

                if VH <= VL + convtol || VL <= abstol {
                    break;
                }

                for i in 0..n {
                    let mut temp = -P[(i, H - 1)];
                    for j in 0..n1 {
                        temp += P[(i, j)];
                    }
                    P[(i, C - 1)] = temp / n as f64;
                }
                for i in 0..n {
                    Bvec[i] = (1.0 + alpha) * P[(i, C - 1)] - alpha * P[(i, H - 1)];
                }
                f = fminfn(n, Bvec, parscale, fnscale, min_func);
                if !f.is_finite() {
                    f = BIG;
                }
                funcount += 1;

                // Reflection
                let VR = f;
                if VR < VL {
                    P[(n1 - 1, C - 1)] = f;
                    for i in 0..n {
                        f = gamma * Bvec[i] + (1.0 - gamma) * P[(i, C - 1)];
                        P[(i, C - 1)] = Bvec[i];
                        Bvec[i] = f;
                    }
                    f = fminfn(n, Bvec, parscale, fnscale, min_func);
                    if !f.is_finite() {
                        f = BIG;
                    }
                    funcount += 1;

                    if f < VR {
                        // Extension
                        for i in 0..n {
                            P[(i, H - 1)] = Bvec[i];
                        }
                        P[(n1 - 1, H - 1)] = f;
                    } else {
                        for i in 0..n {
                            P[(i, H - 1)] = P[(i, C - 1)];
                        }
                        P[(n1 - 1, H - 1)] = VR;
                    }
                } else {
                    // Hi-Reduction
                    if VR < VH {
                        for i in 0..n {
                            P[(i, H - 1)] = Bvec[i];
                        }
                        P[(n1 - 1, H - 1)] = VR;
                        // Lo-Reduction
                    }

                    for i in 0..n {
                        Bvec[i] = (1.0 - beta) * P[(i, H - 1)] + beta * P[(i, C - 1)];
                    }
                    f = fminfn(n, Bvec, parscale, fnscale, min_func);
                    if !f.is_finite() {
                        f = BIG;
                    }
                    funcount += 1;

                    if f < P[(n1 - 1, H - 1)] {
                        for i in 0..n {
                            P[(i, H - 1)] = Bvec[i];
                        }
                        P[(n1 - 1, H - 1)] = f;
                    } else if VR >= VH {
                        // Shrink
                        calcvert = true;
                        size = 0.0;
                        for j in 0..n1 {
                            if j + 1 != L {
                                for i in 0..n {
                                    P[(i, j)] = beta * (P[(i, j)] - P[(i, L - 1)]) + P[(i, L - 1)];
                                }
                            }
                        }
                        if size < oldsize {
                            oldsize = size;
                        } else {
                            return Err(Box::try_from(
                                "Polytope size measure not decreased in shrink",
                            )
                            .unwrap());
                        }
                    }
                }

                if funcount > maxit {
                    break;
                }
            }

            let fmin = P[(n1 - 1, L - 1)];
            let mut x = Vec::new();
            for i in 0..n {
                x.push(P[(i, L - 1)]);
            }
            if funcount > maxit {
                fail = true;
            }
            Ok((fmin, x, funcount, fail))
        }
    }
}