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)

/* Conjugate gradients, 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::error::Error;

use log::warn;
use num_traits::Float;

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

pub struct CG {
    pub calc_type: CalcType,
    pub grad_func: Option<Box<dyn Fn(Vec<f64>) -> Vec<f64>>>,
    pub optim_options: OptimOptions,
}

impl Default for CG {
    fn default() -> Self {
        Self {
            calc_type: CalcType::FletcherReeves,
            grad_func: None,
            optim_options: OptimOptions {
                parscale: None,
                fnscale: 1.0,
                abstol: f64::neg_infinity(),
                reltol: f64::epsilon().sqrt(),
                maxit: 100,
            },
        }
    }
}

impl Optim for CG {
    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 ndeps = vec![1e-3; npar];
        let bounds = None;

        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, grcount, fail) = cgmin(
            npar,
            &mut bvec,
            min_func,
            self.grad_func.as_deref(),
            &ndeps,
            &parscale,
            self.optim_options.fnscale,
            bounds,
            self.optim_options.abstol,
            self.optim_options.reltol,
            self.calc_type,
            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 RELTEST: f64 = 10.0;
const ACCTOL: f64 = 0.0001;
const STEPREDN: f64 = 0.2;

#[derive(Copy, Clone, Debug)]
pub enum CalcType {
    FletcherReeves,
    PolakRibiere,
    BealeSorenson,
}

/* Conjugate gradients, 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 */
pub fn cgmin(
    n: usize,
    Bvec: &mut [f64],
    min_func: &dyn Fn(Vec<f64>) -> f64,
    grad_func: Option<&dyn Fn(Vec<f64>) -> Vec<f64>>,
    ndeps: &[f64],
    parscale: &[f64],
    fnscale: f64,
    bounds: Option<(Vec<f64>, Vec<f64>)>,
    abstol: f64,
    intol: f64,
    calc_type: CalcType,
    maxit: usize,
) -> Result<(f64, Vec<f64>, usize, usize, bool), Box<dyn Error>> {
    let mut fail = false;
    if maxit == 0 {
        Ok((
            fminfn(n, Bvec, parscale, fnscale, min_func),
            Vec::new(),
            0,
            0,
            fail,
        ))
    } else {
        let mut Bvec = Bvec.to_owned();
        let mut x = vec![0.0; n];
        let mut c = vec![0.0; n];
        let mut g = vec![0.0; n];
        let mut t = vec![0.0; n];

        let setstep = 1.7;
        let cyclimit = n;
        let tol = intol * n as f64 * intol.sqrt();

        let mut f = fminfn(n, &Bvec, parscale, fnscale, min_func);
        let mut fmin = f;
        let mut funcount = 1;
        let mut gradcount = 0;

        let mut oldstep = 0.0;
        let mut G1 = 0.0;

        let mut count = 0;
        let mut cycle = 0;
        let mut steplength = 0.0;
        loop {
            for i in 0..n {
                t[i] = 0.0;
                c[i] = 0.0;
            }
            cycle = 0;
            oldstep = 1.0;
            count = 0;

            loop {
                cycle += 1;
                gradcount += 1;

                if gradcount > maxit {
                    warn!("Could not converge");
                    fail = true;
                    return Ok((fmin, x, funcount, gradcount, fail));
                }

                (Bvec, g) = fmingr(
                    n,
                    &Bvec,
                    ndeps,
                    parscale,
                    fnscale,
                    bounds.clone(),
                    min_func,
                    grad_func,
                );
                G1 = 0.0;
                let mut G2 = 0.0;
                for i in 0..n {
                    x[i] = Bvec[i];
                    match calc_type {
                        CalcType::FletcherReeves => {
                            G1 += g[i] * g[i];
                            G2 += c[i] * c[i];
                        }
                        CalcType::PolakRibiere => {
                            G1 += g[i] * (g[i] - c[i]);
                            G2 += c[i] * c[i];
                        }
                        CalcType::BealeSorenson => {
                            G1 += g[i] * (g[i] - c[i]);
                            G2 += t[i] * (g[i] - c[i]);
                        }
                    }
                    c[i] = g[i];
                }
                if G1 > tol {
                    let G3 = if G2 > 0.0 { G1 / G2 } else { 1.0 };
                    let mut gradproj = 0.0;
                    for i in 0..n {
                        t[i] = t[i] * G3 - g[i];
                        gradproj += t[i] * g[i];
                    }
                    steplength = oldstep;

                    let mut accpoint = false;
                    loop {
                        count = 0;
                        for i in 0..n {
                            Bvec[i] = x[i] + steplength * t[i];
                            if RELTEST + x[i] == RELTEST + Bvec[i] {
                                count += 1;
                            }
                        }
                        if count < n {
                            // point is different
                            f = fminfn(n, &Bvec, parscale, fnscale, min_func);
                            funcount += 1;
                            accpoint = f.is_finite() && f <= fmin + gradproj * steplength * ACCTOL;
                            if !accpoint {
                                steplength *= STEPREDN;
                            } else {
                                fmin = f; // we improved, so update value
                            }
                        }

                        if count == n || accpoint {
                            break;
                        }
                    }

                    if count < n {
                        let mut newstep = 2.0 * (f - fmin - gradproj * steplength);
                        if newstep > 0.0 {
                            newstep = -(gradproj * steplength * steplength / newstep);
                            for i in 0..n {
                                Bvec[i] = x[i] + newstep * t[i];
                            }
                            fmin = f;
                            f = fminfn(n, &Bvec, parscale, fnscale, min_func);
                            funcount += 1;
                            if f < fmin {
                                fmin = f;
                            } else {
                                // reset Bvec to match lowest point
                                for i in 0..n {
                                    Bvec[i] = x[i] + steplength * t[i];
                                }
                            }
                        }
                    }
                }
                oldstep = setstep * steplength;
                if oldstep > 1.0 {
                    oldstep = 1.0;
                }

                if count == n || G1 <= tol || cycle == cyclimit {
                    break;
                }
            }

            if (cycle == 1) && (count == n || G1 <= tol || fmin <= abstol) {
                break;
            }
        }

        Ok((fmin, x, funcount, gradcount, fail))
    }
}