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)

// Based off of the python code found at https://github.com/avieira/python_lbfgsb/tree/master

use std::error::Error;

use log::{error, warn};
use nalgebra::{DMatrix, DVector};
use num_traits::Float;

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

pub struct L_BFGS_B {
    pub lmm: usize,
    pub slower: f64,
    pub supper: f64,
    pub factr: f64,
    pub pgtol: usize,
    pub grad_func: Option<Box<dyn Fn(Vec<f64>) -> Vec<f64>>>,
    pub optim_options: OptimOptions,

    alpha_linesearch: f64,
    beta_linesearch: f64,
    max_steplength: f64,
    xtol_minpack: f64,
    max_iter_linesearch: usize,
}

impl Default for L_BFGS_B {
    fn default() -> Self {
        Self {
            alpha_linesearch: 1e-4,
            beta_linesearch: 0.9,
            max_steplength: 1e8,
            // xtol_minpack: 1e-5,
            xtol_minpack: 1e-50,
            max_iter_linesearch: 30,
            lmm: 5,
            slower: f64::neg_infinity(),
            supper: f64::infinity(),
            factr: 1e+07,
            pgtol: 0,
            grad_func: None,
            optim_options: OptimOptions {
                parscale: None,
                fnscale: 1.0,
                abstol: f64::neg_infinity(),
                reltol: f64::epsilon().sqrt(),
                maxit: 500,
            },
        }
    }
}

impl Optim for L_BFGS_B {
    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 n = par.len();
        let _m = self.lmm;

        let parscale = if let Some(parscale) = &self.optim_options.parscale {
            parscale.clone()
        } else {
            vec![1.0; n]
        };

        let mut Bvec = par
            .iter()
            .zip(parscale.iter())
            .map(|(par, scale)| par / scale)
            .collect::<Vec<_>>();

        let mut par_out = par.to_vec();

        let mut lower = vec![0.0; n];
        let mut upper = vec![0.0; n];
        let mut nbd = vec![0; n];
        for i in 0..n {
            lower[i] = self.slower / parscale[i];
            upper[i] = self.supper / parscale[i];
            if !lower[i].is_finite() {
                if !upper[i].is_finite() {
                    nbd[i] = 0;
                } else {
                    nbd[i] = 3;
                }
            } else if !upper[i].is_finite() {
                nbd[i] = 1;
            } else {
                nbd[i] = 2;
            }
        }

        let ret = L_BFGS_B(
            &DVector::<f64>::from_row_slice(&Bvec),
            min_func,
            self.grad_func.as_deref(),
            &DVector::<f64>::from_row_slice(&lower),
            &DVector::<f64>::from_row_slice(&upper),
            self.lmm,
            1e-5, //self.pgtol as f64,
            self.optim_options.reltol,
            self.optim_options.maxit,
            self.alpha_linesearch,
            self.beta_linesearch,
            self.max_steplength,
            self.xtol_minpack,
            self.max_iter_linesearch,
            self.optim_options.abstol,
            &parscale,
            self.optim_options.fnscale,
        );

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

        Ok(OptimResponse {
            parameters: par_out,
            value: ret.1,
            function_count: ret.3,
            gradient_count: ret.4,
            convergence: ret.5,
        })
    }
}

/// Computes the generalized Cauchy point (GCP), defined as the first
/// local minimizer of the quadratic
///
/// .. math::
/// :nowrap:
/// \[\langle g,s\rangle + \frac{1}{2} \langle s, (\theta I + WMW^\intercal)s\rangle\]
///
/// along the projected gradient direction
/// .. math:: $P_\[l,u\](x-\theta g).$
///
///
/// :param x: starting point for the GCP computation
/// :type x: np.array
///
/// :param g: gradient of f(x). g must be a nonzero vector
/// :type g: np.array
///
/// :param l: the lower bound of x
/// :type l: np.array
///
/// :param u: the upper bound of x
/// :type u: np.array
///
/// :param W: part of limited memory BFGS Hessian approximation
/// :type W: np.array
///
/// :param M: part of limited memory BFGS Hessian approximation
/// :type M: np.array
///
/// :param theta: part of limited memory BFGS Hessian approximation
/// :type theta: float
///
/// :return: dict containing a computed value of:
/// - 'xc' the GCP
/// - 'c' = W^(T)(xc-x), used for the subspace minimization
/// - 'F' set of free variables
/// :rtype: dict
/// ..todo check F
///
/// .. seealso::
///
/// \[1\] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited
/// memory algorithm for bound constrained optimization'',
/// SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208.
///
/// \[2\] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN
/// Subroutines for Large Scale Bound Constrained Optimization''
/// Tech. Report, NAM-11, EECS Department, Northwestern University,
/// 1994.
fn compute_Cauchy_point(
    x: &DVector<f64>,
    g: &DVector<f64>,
    l: &DVector<f64>,
    u: &DVector<f64>,
    W: &DMatrix<f64>,
    M: &DMatrix<f64>,
    theta: f64,
) -> (DVector<f64>, DMatrix<f64>, Vec<usize>) {
    let eps_f_sec = 1e-30;
    let mut t = vec![0.0; x.len()];
    let mut d = DMatrix::from_element(x.len(), 1, 0.0);
    let mut x_cp = x.clone();
    for i in 0..x.len() {
        if g[i] < 0.0 {
            t[i] = (x[i] - u[i]) / g[i];
        } else if g[i] > 0.0 {
            t[i] = (x[i] - l[i]) / g[i];
        } else {
            t[i] = f64::infinity();
        }
        if t[i] == 0.0 {
            d[i] = 0.0;
        } else {
            d[i] = -g[i];
        }
    }

    let mut indices = (0..t.len()).collect::<Vec<_>>();
    indices.sort_by(|&i, &j| t[i].partial_cmp(&t[j]).unwrap());
    let mut F = indices
        .into_iter()
        .filter(|&i| t[i] > 0.0)
        .collect::<Vec<_>>();
    let mut t_old = 0.0;
    let mut F_i = 0;
    let mut b = F[0];
    let mut t_min = t[b];
    let mut Dt = t_min;

    let mut p = W.transpose() * &d;
    let mut c = DMatrix::from_element(p.nrows(), p.ncols(), 0.0);
    let mut f_prime = -d.dot(&d);
    let mut f_second = -theta * f_prime - p.dot(&(M * &p));
    let mut f_sec0 = f_second;
    let mut Dt_min = -f_prime / f_second;

    let mut W_b: DVector<f64> = DVector::<f64>::zeros(1);
    let mut g_b = 0.0;

    while Dt_min >= Dt && F_i < F.len() {
        if d[b] > 0.0 {
            x_cp[b] = u[b];
        } else if d[b] < 0.0 {
            x_cp[b] = l[b];
        }
        let x_bcp = x_cp[b];

        let zb = x_bcp - x[b];
        c += Dt * &p;
        W_b = DVector::<f64>::from_row_slice(
            W.row(b).into_iter().cloned().collect::<Vec<_>>().as_slice(),
        );
        g_b = g[b];

        f_prime += Dt * f_second + g_b * (g_b + theta * zb - W_b.dot(&(M * &c)));
        f_second -= g_b * (g_b * theta + W_b.dot(&(M * (2.0 * &p + g_b * &W_b))));
        f_second = f_second.min(eps_f_sec * f_sec0);

        Dt_min = -f_prime / f_second;

        p += g_b * W_b;
        d[b] = 0.0;
        t_old = t_min;
        F_i += 1;

        if F_i < F.len() {
            b = F[F_i];
            t_min = t[b];
            Dt = t_min - t_old;
        } else {
            t_min = f64::infinity();
        }
    }

    Dt_min = if Dt_min < 0.0 { 0.0 } else { Dt_min };
    t_old += Dt_min;

    for i in 0..x.len() {
        if t[i] >= t_min {
            x_cp[i] = x[i] + t_old * d[i];
        }
    }

    F = F.into_iter().filter(|&i| t[i] != t_min).collect::<Vec<_>>();

    c += Dt_min * p;
    (x_cp, c, F)
}

/// Computes an approximate solution of the subspace problem
/// .. math::
/// :nowrap:
///
/// \[\begin{aligned}
/// \min& &\langle r, (x-xcp)\rangle + 1/2 \langle x-xcp, B (x-xcp)\rangle\\
/// \text{s.t.}& &l<=x<=u\\
/// & & x_i=xcp_i \text{for all} i \in A(xcp)
/// \]
///
/// along the subspace unconstrained Newton direction
/// .. math:: $d = -(Z'BZ)^(-1) r.$
///
///
/// :param x: starting point for the GCP computation
/// :type x: np.array
///
/// :param xc: Cauchy point
/// :type xc: np.array
///
/// :param c: W^T(xc-x), computed with the Cauchy point
/// :type c: np.array
///
/// :param g: gradient of f(x). g must be a nonzero vector
/// :type g: np.array
///
/// :param l: the lower bound of x
/// :type l: np.array
///
/// :param u: the upper bound of x u
/// :type u: np.array
///
/// :param W: part of limited memory BFGS Hessian approximation
/// :type W: np.array
///
/// :param M: part of limited memory BFGS Hessian approximation
/// :type M: np.array
///
/// :param theta: part of limited memory BFGS Hessian approximation
/// :type theta: float
///
/// :return: dict containing a computed value of:
/// - 'xbar' the minimizer
/// :rtype: dict
///
/// ..todo Normaly, free_vars is already defined in compute_Cauchy_point in F.
///
///
/// .. seealso::
///
/// \[1\] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited
/// memory algorithm for bound constrained optimization'',
/// SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208.
///
/// \[2\] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN
/// Subroutines for Large Scale Bound Constrained Optimization''
/// Tech. Report, NAM-11, EECS Department, Northwestern University,
/// 1994.
fn minimize_model(
    x: &DVector<f64>,
    xc: &DVector<f64>,
    c: &DMatrix<f64>,
    g: &DVector<f64>,
    l: &DVector<f64>,
    u: &DVector<f64>,
    W: &DMatrix<f64>,
    M: &DMatrix<f64>,
    theta: f64,
) -> DVector<f64> {
    let invThet = 1.0 / theta;

    let mut Z = vec![];
    let mut free_vars = vec![];
    let n = xc.len();
    let mut unit = vec![0.0; n];
    for i in 0..n {
        unit[i] = 1.0;
        if (xc[i] != u[i]) && (xc[i] != l[i]) {
            free_vars.push(i);
            Z.push(unit.clone());
        }
        unit[i] = 0.0;
    }

    if free_vars.is_empty() {
        return xc.clone();
    }

    let Z = DMatrix::from_iterator(
        Z.len(),
        Z[0].len(),
        Z.into_iter().flatten().collect::<Vec<_>>(),
    )
    .transpose();
    let WTZ = W.transpose() * Z;

    let mut not_free_vars = vec![];
    for i in 0..n {
        if !free_vars.contains(&i) {
            not_free_vars.push(i);
        }
    }
    let rHat = (g + theta * (xc - x) - (W * (&(M * c)))).remove_rows_at(&not_free_vars);
    let mut v = &WTZ * &rHat;
    v = M * v;

    let mut N = invThet * (&WTZ * WTZ.transpose());
    N = DMatrix::<f64>::identity(N.nrows(), N.ncols()) - (M * N);
    v = N.qr().solve(&v).unwrap();

    let dHat = -invThet * (&rHat + invThet * WTZ.transpose() * v);

    // Find alpha
    let mut alpha_star = 1.0;
    let mut idx;
    for i in 0..free_vars.len() {
        idx = free_vars[i];
        if dHat[i] > 0.0 {
            alpha_star = alpha_star.min((u[idx] - xc[idx]) / dHat[i]);
        } else if dHat[i] < 0.0 {
            alpha_star = alpha_star.min((l[idx] - xc[idx]) / dHat[i]);
        }
    }

    let d_star = alpha_star * dHat;
    let mut xbar = xc.clone();
    for i in 0..free_vars.len() {
        idx = free_vars[i];
        xbar[idx] += d_star[i];
    }

    xbar
}

/// Computes the biggest 0<=k<=max_steplength such that:
/// l<= x+kd <= u
///
/// :param x: starting point
/// :type x: np.array
///
/// :param d: direction
/// :type d: np.array
///
/// :param l: the lower bound of x
/// :type l: np.array
///
/// :param u: the upper bound of x
/// :type u: np.array
///
/// :param max_steplength: maximum steplength allowed
/// :type max_steplength: float
///
/// :return: maximum steplength allowed
/// :rtype: float
///
///
/// .. seealso::
///
/// \[1\] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited
/// memory algorithm for bound constrained optimization'',
/// SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208.
///
/// \[2\] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN
/// Subroutines for Large Scale Bound Constrained Optimization''
/// Tech. Report, NAM-11, EECS Department, Northwestern University,
/// 1994.
fn max_allowed_steplength(
    x: &DVector<f64>,
    d: &DVector<f64>,
    l: &DVector<f64>,
    u: &DVector<f64>,
    max_steplength: f64,
) -> f64 {
    let mut max_stpl = max_steplength;
    for i in 0..x.len() {
        if d[i] > 0.0 {
            max_stpl = max_stpl.min((u[i] - x[i]) / d[i]);
        } else if d[i] < 0.0 {
            max_stpl = max_stpl.min((l[i] - x[i]) / d[i]);
        }
    }

    max_stpl
}

/// Subroutine dcstep
///
/// This subroutine computes a safeguarded step for a search
/// procedure and updates an interval that contains a step that
/// satisfies a sufficient decrease and a curvature condition.
///
/// The parameter stx contains the step with the least function
/// value. If brackt is set to .true. then a minimizer has
/// been bracketed in an interval with endpoints stx and sty.
/// The parameter stp contains the current step.
/// The subroutine assumes that if brackt is set to .true. then
///
/// min(stx,sty) < stp < max(stx,sty),
///
/// and that the derivative at stx is negative in the direction
/// of the step.
///
/// The subroutine statement is
///
/// subroutine dcstep(stx,fx,dx,sty,fy,dy,stp,fp,dp,brackt,
/// stpmin,stpmax)
///
/// where
///
/// stx is a double precision variable.
/// On entry stx is the best step obtained so far and is an
/// endpoint of the interval that contains the minimizer.
/// On exit stx is the updated best step.
///
/// fx is a double precision variable.
/// On entry fx is the function at stx.
/// On exit fx is the function at stx.
///
/// dx is a double precision variable.
/// On entry dx is the derivative of the function at
/// stx. The derivative must be negative in the direction of
/// the step, that is, dx and stp - stx must have opposite
/// signs.
/// On exit dx is the derivative of the function at stx.
///
/// sty is a double precision variable.
/// On entry sty is the second endpoint of the interval that
/// contains the minimizer.
/// On exit sty is the updated endpoint of the interval that
/// contains the minimizer.
///
/// fy is a double precision variable.
/// On entry fy is the function at sty.
/// On exit fy is the function at sty.
///
/// dy is a double precision variable.
/// On entry dy is the derivative of the function at sty.
/// On exit dy is the derivative of the function at the exit sty.
///
/// stp is a double precision variable.
/// On entry stp is the current step. If brackt is set to .true.
/// then on input stp must be between stx and sty.
/// On exit stp is a new trial step.
///
/// fp is a double precision variable.
/// On entry fp is the function at stp
/// On exit fp is unchanged.
///
/// dp is a double precision variable.
/// On entry dp is the derivative of the function at stp.
/// On exit dp is unchanged.
///
/// brackt is an logical variable.
/// On entry brackt specifies if a minimizer has been bracketed.
/// Initially brackt must be set to .false.
/// On exit brackt specifies if a minimizer has been bracketed.
/// When a minimizer is bracketed brackt is set to .true.
///
/// stpmin is a double precision variable.
/// On entry stpmin is a lower bound for the step.
/// On exit stpmin is unchanged.
///
/// stpmax is a double precision variable.
/// On entry stpmax is an upper bound for the step.
/// On exit stpmax is unchanged.
///
/// MINPACK-1 Project. June 1983
/// Argonne National Laboratory.
/// Jorge J. More' and David J. Thuente.
///
/// MINPACK-2 Project. October 1993.
/// Argonne National Laboratory and University of Minnesota.
/// Brett M. Averick and Jorge J. More'.
fn dcstep(
    stx: &mut f64,
    fx: &mut f64,
    dx: &mut f64,
    sty: &mut f64,
    fy: &mut f64,
    dy: &mut f64,
    stp: &mut f64,
    fp: f64,
    dp: f64,
    brackt: &mut bool,
    stpmin: f64,
    stpmax: f64,
) {
    /* System generated locals */
    let mut d__1;
    let mut d__2;

    /* Local variables */
    let sgnd;
    let stpc;
    let mut stpf;
    let stpq;
    let p;
    let q;
    let mut gamm;
    let r__;
    let s;
    let theta;

    sgnd = dp * (*dx / dx.abs());
    /*     First case: A higher function value. The minimum is bracketed. */
    /*     If the cubic step is closer to stx than the quadratic step, the */
    /*     cubic step is taken, otherwise the average of the cubic and */
    /*     quadratic steps is taken. */
    if fp > *fx {
        theta = (*fx - fp) * 3. / (*stp - *stx) + *dx + dp;
        /* Computing MAX */
        d__1 = theta.abs();
        d__2 = dx.abs();
        d__1 = d__1.max(d__2);
        d__2 = dp.abs();
        s = d__1.max(d__2);
        /* Computing 2nd power */
        d__1 = theta / s;
        gamm = s * (d__1 * d__1 - *dx / s * (dp / s)).sqrt();
        if stp < stx {
            gamm = -gamm;
        }
        p = gamm - *dx + theta;
        q = gamm - *dx + gamm + dp;
        r__ = p / q;
        stpc = *stx + r__ * (*stp - *stx);
        stpq = *stx + *dx / ((*fx - fp) / (*stp - *stx) + *dx) / 2.0 * (*stp - *stx);
        if (stpc - *stx).abs() < (stpq - *stx).abs() {
            stpf = stpc;
        } else {
            stpf = stpc + (stpq - stpc) / 2.0;
        }
        *brackt = true;
        /*     Second case: A lower function value and derivatives of opposite */
        /*     sign. The minimum is bracketed. If the cubic step is farther from */
        /*     stp than the secant step, the cubic step is taken, otherwise the */
        /*     secant step is taken. */
    } else if sgnd < 0.0 {
        theta = (*fx - fp) * 3.0 / (*stp - *stx) + *dx + dp;
        /* Computing MAX */
        d__1 = theta.abs();
        d__2 = dx.abs();
        d__1 = d__1.max(d__2);
        d__2 = dp.abs();
        s = d__1.max(d__2);
        /* Computing 2nd power */
        d__1 = theta / s;
        gamm = s * (d__1 * d__1 - *dx / s * (dp / s)).sqrt();
        if stp > stx {
            gamm = -gamm;
        }
        p = gamm - dp + theta;
        q = gamm - dp + gamm + *dx;
        r__ = p / q;
        stpc = *stp + r__ * (*stx - *stp);
        stpq = *stp + dp / (dp - *dx) * (*stx - *stp);
        if (stpc - *stp).abs() > (stpq - *stp).abs() {
            stpf = stpc;
        } else {
            stpf = stpq;
        }
        *brackt = true;
        /*     Third case: A lower function value, derivatives of the same sign, */
        /*     and the magnitude of the derivative decreases. */
    } else if dp.abs() < dx.abs() {
        /*	  The cubic step is computed only if the cubic tends to ML_POSINF */
        /*	  in the direction of the step or if the minimum of the cubic */
        /*	  is beyond stp. Otherwise the cubic step is defined to be the */
        /*	  secant step. */
        theta = (*fx - fp) * 3.0 / (*stp - *stx) + *dx + dp;
        /* Computing MAX */
        d__1 = theta.abs();
        d__2 = dx.abs();
        d__1 = d__1.max(d__2);
        d__2 = dp.abs();
        s = d__1.max(d__2);
        /*	  The case gamm = 0 only arises if the cubic does not tend */
        /*	  to ML_POSINF in the direction of the step. */
        /* Computing MAX */
        /* Computing 2nd power */
        d__1 = theta / s;
        d__1 = d__1 * d__1 - *dx / s * (dp / s);
        gamm = if d__1 < 0.0 { 0.0 } else { s * d__1.sqrt() };
        if *stp > *stx {
            gamm = -gamm;
        }
        p = gamm - dp + theta;
        q = gamm + (*dx - dp) + gamm;
        r__ = p / q;
        if r__ < 0.0 && gamm != 0.0 {
            stpc = *stp + r__ * (*stx - *stp);
        } else if *stp > *stx {
            stpc = stpmax;
        } else {
            stpc = stpmin;
        }
        stpq = *stp + dp / (dp - *dx) * (*stx - *stp);
        if *brackt {
            /*	     A minimizer has been bracketed. If the cubic step is */
            /*	     closer to stp than the secant step, the cubic step is */
            /*	     taken, otherwise the secant step is taken. */
            if (stpc - *stp).abs() < (stpq - *stp).abs() {
                stpf = stpc;
            } else {
                stpf = stpq;
            }
            d__1 = *stp + (*sty - *stp) * 0.66;
            if stp > stx {
                stpf = d__1.min(stpf);
            } else {
                stpf = d__1.max(stpf);
            }
        } else {
            /*	     A minimizer has not been bracketed. If the cubic step is */
            /*	     farther from stp than the secant step, the cubic step is */
            /*	     taken, otherwise the secant step is taken. */
            if (stpc - *stp).abs() > (stpq - *stp).abs() {
                stpf = stpc;
            } else {
                stpf = stpq;
            }
            stpf = stpmax.min(stpf);
            stpf = stpmin.max(stpf);
        }
        /*     Fourth case: A lower function value, derivatives of the */
        /*     same sign, and the magnitude of the derivative does not */
        /*     decrease. If the minimum is not bracketed, the step is either */
        /*     stpmin or stpmax, otherwise the cubic step is taken. */
    } else {
        if *brackt {
            theta = (fp - *fy) * 3. / (*sty - *stp) + *dy + dp;
            /* Computing MAX */
            d__1 = theta.abs();
            d__2 = dy.abs();
            d__1 = d__1.max(d__2);
            d__2 = dp.abs();
            s = d__1.max(d__2);
            /* Computing 2nd power */
            d__1 = theta / s;
            gamm = s * (d__1 * d__1 - *dy / s * (dp / s)).sqrt();
            if *stp > *sty {
                gamm = -gamm;
            }
            p = gamm - dp + theta;
            q = gamm - dp + gamm + *dy;
            r__ = p / q;
            stpc = *stp + r__ * (*sty - *stp);
            stpf = stpc;
        } else if *stp > *stx {
            stpf = stpmax;
        } else {
            stpf = stpmin;
        }
    }
    /*     Update the interval which contains a minimizer. */
    if fp > *fx {
        *sty = *stp;
        *fy = fp;
        *dy = dp;
    } else {
        if sgnd < 0.0 {
            *sty = *stx;
            *fy = *fx;
            *dy = *dx;
        }
        *stx = *stp;
        *fx = fp;
        *dx = dp;
    }
    /*     Compute the new step. */
    *stp = stpf;
}

#[derive(Clone, Debug, Default)]
struct DcsrchData {
    brackt: bool,
    stage: usize,
    finit: f64,
    ginit: f64,
    gtest: f64,
    width: f64,
    width1: f64,
    /*	  The variables stx, fx, gx contain the values of the step, */
    /*	  function, and derivative at the best step. */
    /*	  The variables sty, fy, gy contain the value of the step, */
    /*	  function, and derivative at sty. */
    /*	  The variables stp, f, g contain the values of the step, */
    /*	  function, and derivative at stp. */
    stx: f64,
    fx: f64,
    gx: f64,
    sty: f64,
    fy: f64,
    gy: f64,
    stmin: f64,
    stmax: f64,
    task: String,
}

/// Subroutine dcsrch
///
/// This subroutine finds a step that satisfies a sufficient
/// decrease condition and a curvature condition.
///
/// Each call of the subroutine updates an interval with
/// endpoints stx and sty. The interval is initially chosen
/// so that it contains a minimizer of the modified function
///
/// psi(stp) = f(stp) - f(0) - ftol*stp*f'(0).
///
/// If psi(stp) <= 0 and f'(stp) >= 0 for some step, then the
/// interval is chosen so that it contains a minimizer of f.
///
/// The algorithm is designed to find a step that satisfies
/// the sufficient decrease condition
///
/// f(stp) <= f(0) + ftol*stp*f'(0),
///
/// and the curvature condition
///
/// abs(f'(stp)) <= gtol*abs(f'(0)).
///
/// If ftol is less than gtol and if, for example, the function
/// is bounded below, then there is always a step which satisfies
/// both conditions.
///
/// If no step can be found that satisfies both conditions, then
/// the algorithm stops with a warning. In this case stp only
/// satisfies the sufficient decrease condition.
///
/// A typical invocation of dcsrch has the following outline:
///
/// task = 'START'
/// 10 continue
/// call dcsrch( ... )
/// if (task .eq. 'FG') then
/// Evaluate the function and the gradient at stp
/// goto 10
/// end if
///
/// NOTE: The user must no alter work arrays between calls.
///
/// The subroutine statement is
///
/// subroutine dcsrch(f,g,stp,ftol,gtol,xtol,stpmin,stpmax task)
/// where
///
/// f is a double precision variable.
/// On initial entry f is the value of the function at 0.
/// On subsequent entries f is the value of the
/// function at stp.
/// On exit f is the value of the function at stp.
///
/// g is a double precision variable.
/// On initial entry g is the derivative of the function at 0.
/// On subsequent entries g is the derivative of the
/// function at stp.
/// On exit g is the derivative of the function at stp.
///
/// stp is a double precision variable.
/// On entry stp is the current estimate of a satisfactory
/// step. On initial entry, a positive initial estimate
/// must be provided.
/// On exit stp is the current estimate of a satisfactory step
/// if task = 'FG'. If task = 'CONV' then stp satisfies
/// the sufficient decrease and curvature condition.
///
/// ftol is a double precision variable.
/// On entry ftol specifies a nonnegative tolerance for the
/// sufficient decrease condition.
/// On exit ftol is unchanged.
///
/// gtol is a double precision variable.
/// On entry gtol specifies a nonnegative tolerance for the
/// curvature condition.
/// On exit gtol is unchanged.
///
/// xtol is a double precision variable.
/// On entry xtol specifies a nonnegative relative tolerance
/// for an acceptable step. The subroutine exits with a
/// warning if the relative difference between sty and stx
/// is less than xtol.
/// On exit xtol is unchanged.
///
/// stpmin is a double precision variable.
/// On entry stpmin is a nonnegative lower bound for the step.
/// On exit stpmin is unchanged.
///
/// stpmax is a double precision variable.
/// On entry stpmax is a nonnegative upper bound for the step.
/// On exit stpmax is unchanged.
///
/// task is a character variable of length at least 60.
/// On initial entry task must be set to 'START'.
/// On exit task indicates the required action:
///
/// If task(1:2) = 'FG' then evaluate the function and
/// derivative at stp and call dcsrch again.
///
/// If task(1:4) = 'CONV' then the search is successful.
///
/// If task(1:4) = 'WARN' then the subroutine is not able
/// to satisfy the convergence conditions. The exit value of
/// stp contains the best point found during the search.
///
/// If task(1:5) = 'ERROR' then there is an error in the
/// input arguments.
///
/// On exit with convergence, a warning or an error, the
/// variable task contains additional information.
///
/// Subprograms called
///
/// MINPACK-2 ... dcstep
///
///
/// MINPACK-1 Project. June 1983.
/// Argonne National Laboratory.
/// Jorge J. More' and David J. Thuente.
///
/// MINPACK-2 Project. October 1993.
/// Argonne National Laboratory and University of Minnesota.
/// Brett M. Averick, Richard G. Carter, and Jorge J. More'.
fn dcsrch(
    f: f64,
    g: f64,
    stp: &mut f64,
    /*Chgd: the next five are no longer pointers:*/
    ftol: f64,
    gtol: f64,
    xtol: f64,
    stpmin: f64,
    stpmax: f64,
    task: &mut String,
    dcsrch_data: &mut DcsrchData,
) {
    /* Local variables */
    let ftest;
    let fm;
    let gm;
    let mut fxm;
    let mut fym;
    let mut gxm;
    let mut gym;

    /* Function Body */

    /*     Initialization block. */
    if task.starts_with("START") {
        /*	  Check the input arguments for errors. */
        if *stp < stpmin {
            *task = "ERROR: STP .LT. STPMIN".to_string();
        }
        if *stp > stpmax {
            *task = "ERROR: STP .GT. STPMAX".to_string();
        }
        if g >= 0.0 {
            *task = "ERROR: INITIAL G .GE. ZERO".to_string();
        }
        if ftol < 0.0 {
            *task = "ERROR: FTOL .LT. ZERO".to_string();
        }
        if gtol < 0.0 {
            *task = "ERROR: GTOL .LT. ZERO".to_string();
        }
        if xtol < 0.0 {
            *task = "ERROR: XTOL .LT. ZERO".to_string();
        }
        if stpmin < 0.0 {
            *task = "ERROR: STPMIN .LT. ZERO".to_string();
        }
        if stpmax < stpmin {
            *task = "ERROR: STPMAX .LT. STPMIN".to_string();
        }

        /*	  Exit if there are errors on input. */
        if task.starts_with("ERROR") {
            return;
        }
        dcsrch_data.brackt = false;
        dcsrch_data.stage = 1;
        dcsrch_data.finit = f;
        dcsrch_data.ginit = g;
        dcsrch_data.gtest = ftol * dcsrch_data.ginit;
        dcsrch_data.width = stpmax - stpmin;
        dcsrch_data.width1 = dcsrch_data.width / 0.5;
        /*	  The variables stx, fx, gx contain the values of the step, */
        /*	  function, and derivative at the best step. */
        /*	  The variables sty, fy, gy contain the value of the step, */
        /*	  function, and derivative at sty. */
        /*	  The variables stp, f, g contain the values of the step, */
        /*	  function, and derivative at stp. */
        dcsrch_data.stx = 0.0;
        dcsrch_data.fx = dcsrch_data.finit;
        dcsrch_data.gx = dcsrch_data.ginit;
        dcsrch_data.sty = 0.0;
        dcsrch_data.fy = dcsrch_data.finit;
        dcsrch_data.gy = dcsrch_data.ginit;
        dcsrch_data.stmin = 0.0;
        dcsrch_data.stmax = *stp + *stp * 4.0;
        *task = "FG".to_string();
        return;
    }
    /*      If psi(stp) <= 0 and f'(stp) >= 0 for some step, then the */
    /*      algorithm enters the second stage. */
    ftest = dcsrch_data.finit + *stp * dcsrch_data.gtest;
    if dcsrch_data.stage == 1 && f <= ftest && g >= 0.0 {
        dcsrch_data.stage = 2;
    }
    /*	Test for warnings. */
    if dcsrch_data.brackt && (*stp < dcsrch_data.stmin || *stp > dcsrch_data.stmax) {
        *task = "WARNING: ROUNDING ERRORS PREVENT PROGRESS".to_string();
    }
    if dcsrch_data.brackt && dcsrch_data.stmax - dcsrch_data.stmin <= xtol * dcsrch_data.stmax {
        *task = "WARNING: XTOL TEST SATISFIED".to_string();
    }
    if *stp == stpmax && f <= ftest && g <= dcsrch_data.gtest {
        *task = "WARNING: STP = STPMAX".to_string();
    }
    if *stp == stpmin && (f > ftest || g >= dcsrch_data.gtest) {
        *task = "WARNING: STP = STPMIN".to_string();
    }
    /*	Test for convergence. */
    if f <= ftest && g.abs() <= gtol * (-dcsrch_data.ginit) {
        *task = "CONVERGENCE".to_string();
    }
    /*	Test for termination. */
    if task.starts_with("WARN") || task.starts_with("CONV") {
        return;
    }

    /*     A modified function is used to predict the step during the */
    /*     first stage if a lower function value has been obtained but */
    /*     the decrease is not sufficient. */
    if dcsrch_data.stage == 1 && f <= dcsrch_data.fx && f > ftest {
        /*	  Define the modified function and derivative values. */
        fm = f - *stp * dcsrch_data.gtest;
        fxm = dcsrch_data.fx - dcsrch_data.stx * dcsrch_data.gtest;
        fym = dcsrch_data.fy - dcsrch_data.sty * dcsrch_data.gtest;
        gm = g - dcsrch_data.gtest;
        gxm = dcsrch_data.gx - dcsrch_data.gtest;
        gym = dcsrch_data.gy - dcsrch_data.gtest;
        /*	  Call dcstep to update stx, sty, and to compute the new step. */
        dcstep(
            &mut dcsrch_data.stx,
            &mut fxm,
            &mut gxm,
            &mut dcsrch_data.sty,
            &mut fym,
            &mut gym,
            stp,
            fm,
            gm,
            &mut dcsrch_data.brackt,
            dcsrch_data.stmin,
            dcsrch_data.stmax,
        );
        /*	  Reset the function and derivative values for f. */
        dcsrch_data.fx = fxm + dcsrch_data.stx * dcsrch_data.gtest;
        dcsrch_data.fy = fym + dcsrch_data.sty * dcsrch_data.gtest;
        dcsrch_data.gx = gxm + dcsrch_data.gtest;
        dcsrch_data.gy = gym + dcsrch_data.gtest;
    } else {
        /*	 Call dcstep to update stx, sty, and to compute the new step. */
        dcstep(
            &mut dcsrch_data.stx,
            &mut dcsrch_data.fx,
            &mut dcsrch_data.gx,
            &mut dcsrch_data.sty,
            &mut dcsrch_data.fy,
            &mut dcsrch_data.gy,
            stp,
            f,
            g,
            &mut dcsrch_data.brackt,
            dcsrch_data.stmin,
            dcsrch_data.stmax,
        );
    }
    /*     Decide if a bisection step is needed. */
    if dcsrch_data.brackt {
        if (dcsrch_data.sty - dcsrch_data.stx).abs() >= dcsrch_data.width1 * 0.66 {
            *stp = dcsrch_data.stx + (dcsrch_data.sty - dcsrch_data.stx) * 0.5;
        }
        dcsrch_data.width1 = dcsrch_data.width;
        dcsrch_data.width = (dcsrch_data.sty - dcsrch_data.stx).abs();
    }
    /*     Set the minimum and maximum steps allowed for stp. */
    if dcsrch_data.brackt {
        dcsrch_data.stmin = dcsrch_data.stx.min(dcsrch_data.sty);
        dcsrch_data.stmax = dcsrch_data.stx.max(dcsrch_data.sty);
    } else {
        dcsrch_data.stmin = *stp + (*stp - dcsrch_data.stx) * 1.1;
        dcsrch_data.stmax = *stp + (*stp - dcsrch_data.stx) * 4.0;
    }
    /*     Force the step to be within the bounds stpmax and stpmin. */
    if *stp < stpmin {
        *stp = stpmin;
    }
    if *stp > stpmax {
        *stp = stpmax;
    }

    /*     If further progress is not possible, let stp be the best */
    /*     point obtained during the search. */
    if dcsrch_data.brackt
        && ((*stp <= dcsrch_data.stmin || *stp >= dcsrch_data.stmax)
            || (dcsrch_data.stmax - dcsrch_data.stmin <= xtol * dcsrch_data.stmax))
    {
        *stp = dcsrch_data.stx;
    }
    /*     Obtain another function and derivative. */
    *task = "FG".to_string();
}

/// Finds a step that satisfies a sufficient decrease condition and a curvature condition.
///
/// The algorithm is designed to find a step that satisfies the sufficient decrease condition
///
/// f(x0+stp*d) <= f(x0) + alpha*stp*\langle f'(x0),d\rangle,
///
/// and the curvature condition
///
/// abs(f'(x0+stp*d)) <= beta*abs(\langle f'(x0),d\rangle).
///
/// If alpha is less than beta and if, for example, the functionis bounded below, then
/// there is always a step which satisfies both conditions.
///
/// :param x0: starting point
/// :type x0: np.array
///
/// :param f0: f(x0)
/// :type f0: float
///
/// :param g0: f'(x0), gradient
/// :type g0: np.array
///
/// :param d: search direction
/// :type d: np.array
///
/// :param above_iter: current iteration in optimization process
/// :type above_iter: integer
///
/// :param max_steplength: maximum steplength allowed
/// :type max_steplength: float
///
/// :param fct_f: callable, function f(x)
/// :type fct_f: function returning float
///
/// :param fct_grad: callable, function f'(x)
/// :type fct_grad: function returning np.array
///
/// :param alpha, beta: parameters of the decrease and curvature conditions
/// :type alpha, beta: floats
///
/// :param xtol_minpack: tolerance used in minpack2.dcsrch
/// :type xtol_minpack: float
///
/// :param max_iter: number of iteration allowed for finding a steplength
/// :type max_iter: integer
///
/// :return: optimal steplength meeting both decrease and curvature condition
/// :rtype: float
///
///
///
///
/// .. seealso::
///
/// \[minpack\] scipy.optimize.minpack2.dcsrch
///
/// \[1\] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited
/// memory algorithm for bound constrained optimization'',
/// SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208.
///
/// \[2\] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN
/// Subroutines for Large Scale Bound Constrained Optimization''
/// Tech. Report, NAM-11, EECS Department, Northwestern University,
/// 1994.
fn line_search(
    x0: &DVector<f64>,
    f0: f64,
    g0: &DVector<f64>,
    d: &DVector<f64>,
    above_iter: usize,
    mut max_steplength: f64,
    fct_f: &dyn Fn(Vec<f64>) -> f64,
    fct_grad: Option<&dyn Fn(Vec<f64>) -> Vec<f64>>,
    alpha: f64,        // = 1e-4,
    beta: f64,         // = 0.9,
    xtol_minpack: f64, // = 1e-5,
    max_iter: usize,   // = 30
    ndeps: &[f64],
    parscale: &[f64],
    fnscale: f64,
    bounds: Option<(Vec<f64>, Vec<f64>)>,
    g_count: &mut usize,
) -> f64 {
    let mut steplength_0 = if max_steplength > 1.0 {
        1.0
    } else {
        0.5 * max_steplength
    };
    let mut f_m1 = f0;
    let dphi = g0.dot(d);
    let mut dphi_m1 = dphi;
    let mut i = 0;

    if above_iter == 0 {
        max_steplength = 1.0;
        steplength_0 = (1.0 / d.dot(d).sqrt()).min(1.0);
    }

    let mut task = "START".to_string();

    let mut steplength = f64::nan();

    let mut dcsrch_data = DcsrchData::default();

    while i < max_iter {
        steplength = steplength_0;
        // f0 should be set off of this too, but it's not
        dcsrch(
            f_m1,
            dphi_m1,
            &mut steplength,
            alpha,
            beta,
            xtol_minpack,
            0.0,
            max_steplength,
            &mut task,
            &mut dcsrch_data,
        );
        if task.starts_with("FG") {
            steplength_0 = steplength;
            f_m1 = fct_f((x0 + steplength * d).as_slice().to_vec());
            dphi_m1 = DVector::<f64>::from_vec(
                fmingr(
                    x0.len(),
                    (x0 + steplength * d).as_slice(),
                    &ndeps,
                    parscale,
                    fnscale,
                    bounds.clone(),
                    fct_f,
                    fct_grad,
                )
                .1,
            )
            .dot(d);
            *g_count += 1;
        } else {
            break;
        }
        i += 1;
    }

    // println!("norm: {}", d.dot(d).sqrt() * steplength);

    if i >= max_iter {
        // max_iter reached, the line search did not converge
        println!("Max iter reached");
        steplength = f64::nan()
    }

    if (task.starts_with("ERROR") || task.starts_with("WARN")) && task != "WARNING: STP = STPMAX" {
        println!("{}", task);
        steplength = f64::nan(); // failed
    }

    steplength
}

/// Update lists S and Y, and form the L-BFGS Hessian approximation thet, W and M.
///
/// :param sk: correction in x = new_x - old_x
/// :type sk: np.array
///
/// :param yk: correction in gradient = f'(new_x) - f'(old_x)
/// :type yk: np.array
///
/// :param S, Y: lists defining the L-BFGS matrices, updated during process (IN/OUT)
/// :type S, Y: list
///
/// :param m: Maximum size of lists S and Y: keep in memory only m previous iterations
/// :type m: integer
///
/// :param W, M: L-BFGS matrices
/// :type W, M: np.array
///
/// :param thet: L-BFGS float parameter
/// :type thet: float
///
/// :param eps: Positive stability parameter for accepting current step for updating matrices.
/// :type eps: float >0
///
/// :return: updated \[W, M, thet\]
/// :rtype: tuple
///
/// .. seealso::
///
/// \[1\] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited
/// memory algorithm for bound constrained optimization'',
/// SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208.
///
/// \[2\] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN
/// Subroutines for Large Scale Bound Constrained Optimization''
/// Tech. Report, NAM-11, EECS Department, Northwestern University,
/// 1994.
fn update_SY(
    sk: &DVector<f64>,
    yk: &DVector<f64>,
    S: &mut Vec<DVector<f64>>,
    Y: &mut Vec<DVector<f64>>,
    m: usize,
    eps: f64, // = 2.2e-16
) -> Option<(DMatrix<f64>, DMatrix<f64>, f64)> {
    let sTy = sk.dot(yk);
    let yTy = yk.dot(yk);
    if sTy > eps * yTy {
        S.push(sk.clone());
        Y.push(yk.clone());
        if S.len() > m {
            S.remove(0);
            Y.remove(0);
        }

        let mut SArray = DMatrix::<f64>::zeros(0, S[0].nrows());
        let mut YArray = DMatrix::<f64>::zeros(0, S[0].nrows());
        for i in 0..S.len() {
            SArray = SArray.clone().insert_row(0, 0.0);
            SArray
                .row_mut(i)
                .iter_mut()
                .zip(sk.iter())
                .for_each(|(s, &sk)| *s = sk);
            YArray = YArray.clone().insert_row(0, 0.0);
            YArray
                .row_mut(i)
                .iter_mut()
                .zip(yk.iter())
                .for_each(|(y, &yk)| *y = yk);
        }
        SArray = SArray.transpose();
        YArray = YArray.transpose();

        let STS = &SArray.transpose() * &SArray;
        let mut L = &SArray.transpose() * &YArray;
        let mut D = DMatrix::<f64>::zeros(L.nrows(), L.ncols());
        D.set_diagonal(&-L.diagonal());
        L.fill_diagonal(0.0);
        L = L.lower_triangle();

        let hstack = |X: &DMatrix<f64>, Y: &DMatrix<f64>| -> DMatrix<f64> {
            let mut Z = X
                .clone()
                .resize(X.nrows(), X.ncols() + Y.ncols(), f64::nan());
            for i in 0..X.nrows() {
                for j in 0..Y.ncols() {
                    *Z.get_mut((i, X.ncols() + j)).unwrap() = Y[(i, j)];
                }
            }
            Z
        };
        let vstack = |X: &DMatrix<f64>, Y: &DMatrix<f64>| -> DMatrix<f64> {
            let mut Z = X
                .clone()
                .resize(X.nrows() + Y.nrows(), X.ncols(), f64::nan());
            for i in 0..Y.nrows() {
                for j in 0..X.ncols() {
                    *Z.get_mut((X.nrows() + i, j)).unwrap() = Y[(i, j)];
                }
            }
            Z
        };

        let thet = yTy / sTy;
        let W = hstack(&YArray, &(thet * SArray));
        let M = hstack(&vstack(&D, &L), &vstack(&L.transpose(), &(thet * STS)))
            .pseudo_inverse(0.0)
            .unwrap();
        Some((W, M, thet))
    } else {
        None
    }
}

/// Solves bound constrained optimization problems by using the compact formula
/// of the limited memory BFGS updates.
///
/// :param x0: initial guess
/// :type sk: np.array
///
/// :param f: cost function to optimize f(x)
/// :type f: function returning float
///
/// :param df: gradient of cost function to optimize f'(x)
/// :type df: function returning np.array
///
/// :param l: the lower bound of x
/// :type l: np.array
///
/// :param u: the upper bound of x
/// :type u: np.array
///
///:param m: Maximum size of lists for L-BFGS Hessian approximation
/// :type m: integer
///
/// :param epsg: Tolerance on projected gradient: programs converges when
/// P(x-g, l, u)<epsg.
/// :type epsg: float
///
/// :param epsf: Tolerance on function change: programs ends when (f_k-f_{k+1})/max(|f_k|,|f_{k+1}|,1) < epsf * epsmch, where
/// epsmch is the machine precision.
/// :type epsf: float
///
/// :param alpha_linesearch, beta_linesearch: Parameters for linesearch.
/// See ``alpha`` and ``beta`` in :func:`line_search`
/// :type alpha_linesearch, beta_linesearch: float
///
/// :param max_steplength: Maximum steplength allowed. See ``max_steplength`` in :func:`max_allowed_steplength`
/// :type max_steplength: float
///
/// :param xtol_minpack: Tolerence used by minpack2. See ``xtol_minpack`` in :func:`line_search`
/// :type xtol_minpack: float
///
/// :param max_iter_linesearch: Maximum number of trials for linesearch.
/// See ``max_iter_linesearch`` in :func:`line_search`
/// :type max_iter_linesearch: integer
///
/// :param eps_SY: Parameter used for updating the L-BFGS matrices. See ``eps`` in :func:`update_SY`
/// :type eps_SY: float
///
/// :return: dict containing:
/// - 'x': optimal point
/// - 'f': optimal value at x
/// - 'df': gradient f'(x)
/// :rtype: dict
///
///
/// ..todo Check matrices update and different safeguards may be missing
///
/// .. seealso::
/// Function tested on Rosenbrock and Beale function with different starting points. All tests passed.
///
/// \[1\] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited
/// memory algorithm for bound constrained optimization'',
/// SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208.
///
/// \[2\] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN
/// Subroutines for Large Scale Bound Constrained Optimization''
/// Tech. Report, NAM-11, EECS Department, Northwestern University,
/// 1994.
pub fn L_BFGS_B(
    x0: &DVector<f64>,
    f: &dyn Fn(Vec<f64>) -> f64,
    df: Option<&dyn Fn(Vec<f64>) -> Vec<f64>>,
    l: &DVector<f64>,
    u: &DVector<f64>,
    m: usize,
    epsg: f64,
    epsf: f64,
    max_iter: usize,
    alpha_linesearch: f64,
    beta_linesearch: f64,
    max_steplength: f64,
    xtol_minpack: f64,
    max_iter_linesearch: usize,
    eps_SY: f64,
    parscale: &[f64],
    fnscale: f64,
) -> (DVector<f64>, f64, DVector<f64>, usize, usize, bool) {
    let clip = |x: &mut [f64], l: &[f64], u: &[f64]| {
        for ((x_i, &l_i), &u_i) in x.iter_mut().zip(l.iter()).zip(u.iter()) {
            if *x_i < l_i {
                *x_i = l_i;
            } else if *x_i > u_i {
                *x_i = u_i;
            }
        }
    };

    let n = x0.len();
    let ndeps = vec![1e-3; n];

    let mut x = x0.clone();
    clip(x.as_mut_slice(), l.as_slice(), u.as_slice());
    let mut S = vec![];
    let mut Y = vec![];
    let mut W = DMatrix::<f64>::zeros(n, 1);
    let mut M = DMatrix::<f64>::zeros(1, 1);
    let mut theta = 1.0;
    let epsmch = f64::epsilon();
    let bounds = Some((l.as_slice().to_vec(), u.as_slice().to_vec()));

    let mut f0 = fminfn(n, x.as_slice(), parscale, fnscale, f);
    let mut f_count = 1;
    let mut g = DVector::<f64>::from_vec(
        fmingr(
            n,
            x.as_slice(),
            &ndeps,
            parscale,
            fnscale,
            bounds.clone(),
            f,
            df,
        )
        .1,
    );
    let mut g_count = 1;
    let mut i = 0;

    let get_x_max = |x: &[f64], g: &DVector<f64>, l: &DVector<f64>, u: &DVector<f64>| {
        x.iter()
            .zip(g.iter())
            .zip(l.iter())
            .zip(u.iter())
            .map(|(((x_i, g_i), &l_i), &u_i)| {
                let mut x_temp = x_i - g_i;
                if x_temp < l_i {
                    x_temp = l_i;
                } else if x_temp > u_i {
                    x_temp = u_i;
                }
                (x_temp - x_i).abs()
            })
            .max_by(|x_1, x_2| x_1.partial_cmp(x_2).unwrap())
            .unwrap()
    };

    // let mut ii = 0;
    while get_x_max(x.as_slice(), &g, l, u) > epsg && i < max_iter {
        // if ii > 15 {
        //     break;
        // }
        // println!("Iteration {ii}");
        // ii += 1;
        // println!("x: {:?}", x.as_slice());
        // println!("f: {f0}");
        // println!("g: {:?}", g.as_slice());
        let oldf0 = f0;
        let oldx = x.clone();
        let oldg = g.clone();
        let dictCP = compute_Cauchy_point(&x, &g, l, u, &W, &M, theta);
        let dictMinMod = minimize_model(&x, &dictCP.0, &dictCP.1, &g, l, u, &W, &M, theta);

        let d = dictMinMod - &x;
        let max_stpl = max_allowed_steplength(&x, &d, &l, &u, max_steplength);
        let steplength = line_search(
            &x,
            f0,
            &g,
            &d,
            i,
            max_stpl,
            f,
            df,
            alpha_linesearch,
            beta_linesearch,
            xtol_minpack,
            max_iter_linesearch,
            &ndeps,
            parscale,
            fnscale,
            bounds.clone(),
            &mut g_count,
        );

        if steplength.is_nan() {
            if S.is_empty() {
                // Hessian already rebooted: abort.
                error!("Error: can not compute new steplength : abort");
                return (
                    x.clone(),
                    f(x.as_slice().to_vec()),
                    DVector::<f64>::from_vec(
                        fmingr(
                            n,
                            x.as_slice(),
                            &ndeps,
                            parscale,
                            fnscale,
                            bounds.clone(),
                            f,
                            df,
                        )
                        .1,
                    ),
                    f_count,
                    g_count,
                    false,
                );
            } else {
                // Reboot BFGS-Hessian:
                S = vec![];
                Y = vec![];
                W = DMatrix::<f64>::zeros(n, 1);
                M = DMatrix::<f64>::zeros(1, 1);
                theta = 1.0;
            }
        } else {
            x += steplength * d;
            f0 = fminfn(n, x.as_slice(), parscale, fnscale, f);
            f_count += 1;
            g = DVector::<f64>::from_vec(
                fmingr(
                    n,
                    x.as_slice(),
                    &ndeps,
                    parscale,
                    fnscale,
                    bounds.clone(),
                    f,
                    df,
                )
                .1,
            );
            g_count += 1;

            if let Some((W_t, M_t, theta_t)) =
                update_SY(&(&x - oldx), &(&g - oldg), &mut S, &mut Y, m, eps_SY)
            {
                W = W_t;
                M = M_t;
                theta = theta_t;
            }

            if (oldf0 - f0) / vec![oldf0.abs(), f0.abs(), 1.0].max() < epsmch * epsf {
                error!("Relative reduction of f below tolerence: abort.");
                break;
            }
            i += 1;
        }
    }

    if i == max_iter {
        warn!("Maximum iteration reached.");
    }

    (x, f0, g, f_count, g_count, i < max_iter)
}