lbfgsbrs 0.1.1

use super::miniblas::{ddot, dcopy};

use log::info;

pub const START: core::ffi::c_int = 1 as core::ffi::c_int;
pub const NEW_X: core::ffi::c_int = 2 as core::ffi::c_int;
pub const FG: core::ffi::c_int = 10 as core::ffi::c_int;
pub const FG_LN: core::ffi::c_int = 11 as core::ffi::c_int;
pub const FG_LNSRCH: core::ffi::c_int = 11 as core::ffi::c_int;
pub const CONVERGENCE: core::ffi::c_int = 20 as core::ffi::c_int;
pub const CONVERGENCE_END: core::ffi::c_int = 25 as core::ffi::c_int;
pub const WARNING: core::ffi::c_int = 100 as core::ffi::c_int;
pub const WARNING_END: core::ffi::c_int = 110 as core::ffi::c_int;
pub const WARNING_ROUND: core::ffi::c_int = 101 as core::ffi::c_int;
pub const WARNING_XTOL: core::ffi::c_int = 102 as core::ffi::c_int;
pub const WARNING_STPMAX: core::ffi::c_int = 103 as core::ffi::c_int;
pub const WARNING_STPMIN: core::ffi::c_int = 104 as core::ffi::c_int;
pub const ERROR: core::ffi::c_int = 200 as core::ffi::c_int;
pub const ERROR_END: core::ffi::c_int = 240 as core::ffi::c_int;
pub const ERROR_SMALLSTP: core::ffi::c_int = 201 as core::ffi::c_int;
pub const ERROR_LARGESTP: core::ffi::c_int = 202 as core::ffi::c_int;
pub const ERROR_INITIAL: core::ffi::c_int = 203 as core::ffi::c_int;
pub const ERROR_FTOL: core::ffi::c_int = 204 as core::ffi::c_int;
pub const ERROR_GTOL: core::ffi::c_int = 205 as core::ffi::c_int;
pub const ERROR_XTOL: core::ffi::c_int = 206 as core::ffi::c_int;
pub const ERROR_STP0: core::ffi::c_int = 207 as core::ffi::c_int;
pub const ERROR_STP1: core::ffi::c_int = 208 as core::ffi::c_int;
pub const FTOL: f64 = 0.001f64;
pub const GTOL: f64 = 0.9f64;
pub const XTOL: f64 = 0.1f64;
pub const STEPMIN: f64 = 0.0f64;
static mut c__1: i32 = 1;

/// Safe implementation of the line search routine for L-BFGS-B algorithm
/// 
/// # Arguments
/// * `n` - Number of variables
/// * `l` - Lower bounds for variables
/// * `u` - Upper bounds for variables
/// * `nbd` - Types of bounds (0: unbounded, 1: lower bound only, 2: both bounds, 3: upper bound only)
/// * `x` - Current point
/// * `f` - Function value at current point
/// * `fold` - Previous function value
/// * `gd` - Directional derivative
/// * `gdold` - Previous directional derivative
/// * `g` - Gradient at current point
/// * `d` - Search direction
/// * `r`, `t`, `z` - Working arrays
/// * `stp` - Step length
/// * `dnorm` - Norm of search direction
/// * `dtd` - Inner product of d with itself
/// * `xstep` - Actual step taken
/// * `stpmx` - Maximum step length
/// * `iter` - Current iteration number
/// * `ifun`, `iback`, `nfgv` - Counters for function evaluations
/// * `info` - Status indicator
/// * `task` - Task indicator
/// * `boxed`, `cnstnd` - Flags for bound constraints
/// * `csave` - Character working array
/// * `isave` - i32 working array
/// * `dsave` - Double precision working array
pub fn lnsrlb(
    n: i32,
    l: &[f64],
    u: &[f64],
    nbd: &[i32],
    x: &mut [f64],
    f: &mut f64,
    fold: &mut f64,
    gd: &mut f64,
    gdold: &mut f64,
    g: &[f64],
    d: &[f64],
    r: &mut [f64],
    t: &mut [f64],
    z: &[f64],
    stp: &mut f64,
    dnorm: &mut f64,
    dtd: &mut f64,
    xstep: &mut f64,
    stpmx: &mut f64,
    iter: &mut i32,
    ifun: &mut i32,
    iback: &mut i32,
    nfgv: &mut i32,
    info: &mut i32,
    task: &mut i32,
    boxed: bool,
    cnstnd: bool,
    csave: &mut i32,
    isave: &mut [i32],  // Slice for isave array
    dsave: &mut [f64],      // Slice for dsave array
) -> i32 {

    if *task != FG_LN as i32 {
        // Calculate dtd and dnorm
        *dtd = ddot(n, d, 1, d, 1);
        *dnorm = (*dtd).sqrt();
        *stpmx = 1e10f64;
        
        if cnstnd == true {
            if *iter == 0 {
                *stpmx = 1.0f64;
            } else {
                for i in 0..n as usize {
                    let a1 = d[i];
                    if nbd[i] != 0 {
                        if a1 < 0.0f64 && nbd[i] <= 2 {
                            let a2 = l[i] - x[i];
                            if a2 >= 0.0f64 {
                                *stpmx = 0.0f64;
                            } else if a1 * *stpmx < a2 {
                                *stpmx = a2 / a1;
                            }
                        } else if a1 > 0.0f64 && nbd[i] >= 2 {
                            let a2 = u[i] - x[i];
                            if a2 <= 0.0f64 {
                                *stpmx = 0.0f64;
                            } else if a1 * *stpmx > a2 {
                                *stpmx = a2 / a1;
                            }
                        }
                    }
                }
            }
        }
        
        if *iter == 0 && boxed == false {
            let d__1 = 1.0f64 / *dnorm;
            *stp = f64::min(d__1, *stpmx);
        } else {
            *stp = 1.0f64;
        }
        
        // Copy x to t and g to r
        dcopy(n, x, 1, t, 1);
        dcopy(n, g, 1, r, 1);
        
        *fold = *f;
        *ifun = 0;
        *iback = 0;
        *csave = START as i32;
    }
    
    // Calculate gd
    *gd = ddot(n, g, 1, d, 1);
    
    if *ifun == 0 {
        *gdold = *gd;
        if *gd >= 0.0f64 {
            info!("ascend direction in projection gd = {}", *gd);
            *info = -(4) as i32;
            return 0;
        }
    }
    
    // Call dcsrch
    dcsrch(
        f,
        gd,
        stp,
        &mut FTOL,
        &mut GTOL,
        &mut XTOL,
        &mut STEPMIN,
        stpmx,
        csave,
        isave,
        dsave,
    );
    
    *xstep = *stp * *dnorm;
    
    // Check csave status
    let is_warning = *csave >= WARNING as i32 && *csave <= WARNING_END as i32;
    let is_convergence = *csave >= CONVERGENCE as i32 && *csave <= CONVERGENCE_END as i32;
    
    if !is_warning && !is_convergence {
        *task = FG_LNSRCH as i32;
        *ifun += 1;
        *nfgv += 1;
        *iback = *ifun - 1;
        
        if *stp == 1.0f64 {
            // Copy z to x
            for i in 0..n as usize {
                x[i] = z[i];
            }
        } else {
            // Calculate x = t + stp * d
            for i in 0..n as usize {
                x[i] = *stp * d[i] + t[i];
            }
        }
    } else {
        *task = NEW_X as i32;
    }

    0
}

/// Safe implementation of the More-Thuente line search algorithm
/// 
/// This routine finds a step that satisfies a sufficient decrease condition
/// and a curvature condition.
/// 
/// # Arguments
/// * `f` - Current function value
/// * `g` - Current gradient value
/// * `stp` - Step length
/// * `ftol` - Tolerance for sufficient decrease condition
/// * `gtol` - Tolerance for curvature condition
/// * `xtol` - Relative width tolerance
/// * `stpmin` - Minimum step length
/// * `stpmax` - Maximum step length
/// * `task` - Task indicator
/// * `isave` - i32 working array (length 2)
/// * `dsave` - Double precision working array (length 13)
fn dcsrch(
    f: &mut f64,
    g: &mut f64,
    stp: &mut f64,
    ftol: &mut f64,
    gtol: &mut f64,
    xtol: &mut f64,
    stpmin: &mut f64,
    stpmax: &mut f64,
    task: &mut i32,
    isave: &mut [i32], // Slice for isave array (needs 2 elements)
    dsave: &mut [f64],     // Slice for dsave array (needs 13 elements)
) -> core::ffi::c_int {
    let mut brackt: bool;
    let mut stage: i32;
    let mut finit: f64;
    let mut ginit: f64;
    let mut gtest: f64;
    let mut width: f64;
    let mut width1: f64;
    let mut stx: f64;
    let mut fx: f64;
    let mut gx: f64;
    let mut sty: f64;
    let mut fy: f64;
    let mut gy: f64;
    let mut stmin: f64;
    let mut stmax: f64;
    let mut fm: f64;
    let mut gm: f64;
    let mut fxm: f64;
    let mut fym: f64;
    let mut gxm: f64;
    let mut gym: f64;
    let mut ftest: f64;

    if *task == START as i32 {
        // Initial checks
        if *stp < *stpmin {
            *task = ERROR_SMALLSTP as i32;
        }
        if *stp > *stpmax {
            *task = ERROR_LARGESTP as i32;
        }
        if *g >= 0.0f64 {
            *task = ERROR_INITIAL as i32;
        }
        if *ftol < 0.0f64 {
            *task = ERROR_FTOL as i32;
        }
        if *gtol < 0.0f64 {
            *task = ERROR_GTOL as i32;
        }
        if *xtol < 0.0f64 {
            *task = ERROR_XTOL as i32;
        }
        if *stpmin < 0.0f64 {
            *task = ERROR_STP0 as i32;
        }
        if *stpmax < *stpmin {
            *task = ERROR_STP1 as i32;
        }

        if (*task >= ERROR as i32) && (*task <= ERROR_END as i32) {
            return 0;
        }

        // Initialize local variables
        brackt = false;
        stage = 1;
        finit = *f;
        ginit = *g;
        gtest = *ftol * ginit;
        width = *stpmax - *stpmin;
        width1 = width / 0.5f64;
        
        // Initialize the variables
        stx = 0.0f64;
        fx = finit;
        gx = ginit;
        sty = 0.0f64;
        fy = finit;
        gy = ginit;
        stmin = 0.0f64;
        stmax = *stp + *stp * 4.0f64;
        
        *task = FG as i32;
    } else {
        // Restore variables from saved state
        brackt = if isave[21] == 1 { true } else { false };
        stage = isave[22];
        ginit = dsave[17];
        gtest = dsave[18];
        gx = dsave[19];
        gy = dsave[20];
        finit = dsave[21];
        fx = dsave[22];
        fy = dsave[23];
        stx = dsave[24];
        sty = dsave[25];
        stmin = dsave[26];
        stmax = dsave[27];
        width = dsave[28];
        width1 = dsave[29];

        ftest = finit + *stp * gtest;

        if stage == 1 && *f <= ftest && *g >= 0.0f64 {
            stage = 2;
        }

        // Check for warnings and convergence
        if brackt == true && (*stp <= stmin || *stp >= stmax) {
            *task = WARNING_ROUND as i32;
        }
        if brackt == true && stmax - stmin <= *xtol * stmax {
            *task = WARNING_XTOL as i32;
        }
        if *stp == *stpmax && *f <= ftest && *g <= gtest {
            *task = WARNING_STPMAX as i32;
        }
        if *stp == *stpmin && (*f > ftest || *g >= gtest) {
            *task = WARNING_STPMIN as i32;
        }
        if *f <= ftest && (*g).abs() <= *gtol * -ginit {
            *task = CONVERGENCE as i32;
        }

        // Continue if no warnings or convergence
        if !(*task >= WARNING as i32 && *task <= WARNING_END as i32) 
            && !(*task >= CONVERGENCE as i32 && *task <= CONVERGENCE_END as i32) {
            
            if stage == 1 && *f <= fx && *f > ftest {
                // Calculate the modified function values
                fm = *f - *stp * gtest;
                fxm = fx - stx * gtest;
                fym = fy - sty * gtest;
                gm = *g - gtest;
                gxm = gx - gtest;
                gym = gy - gtest;

                // Call dcstep with modified values
                dcstep(
                    &mut stx, &mut fxm, &mut gxm,
                    &mut sty, &mut fym, &mut gym,
                    stp, &mut fm, &mut gm,
                    &mut brackt, &mut stmin, &mut stmax,
                );

                // Restore function values
                fx = fxm + stx * gtest;
                fy = fym + sty * gtest;
                gx = gxm + gtest;
                gy = gym + gtest;
            } else {
                // Call dcstep with original values
                dcstep(
                    &mut stx, &mut fx, &mut gx,
                    &mut sty, &mut fy, &mut gy,
                    stp, f, g,
                    &mut brackt, &mut stmin, &mut stmax,
                );
            }

            // Update the interval of uncertainty
            if brackt == true {
                if (sty - stx).abs() >= width1 * 0.66f64 {
                    *stp = stx + (sty - stx) * 0.5f64;
                }
                width1 = width;
                width = (sty - stx).abs();
            }

            // Update interval bounds
            if brackt == true {
                stmin = f64::min(stx, sty);
                stmax = f64::max(stx, sty);
            } else {
                stmin = *stp + (*stp - stx) * 1.1f64;
                stmax = *stp + (*stp - stx) * 4.0f64;
            }

            // Force the step to be within bounds
            *stp = f64::max(*stp, *stpmin);
            *stp = f64::min(*stp, *stpmax);

            // Force the step to be within the interval of uncertainty
            if brackt == true && (*stp <= stmin || *stp >= stmax) 
                || (brackt == true && (stmax - stmin <= *xtol * stmax)) {
                *stp = stx;
            }

            *task = FG as i32;
        }
    }

    // Save state
    isave[21] = if brackt == true { 1 } else { 0 };
    isave[22] = stage;
    dsave[17] = ginit;
    dsave[18] = gtest;
    dsave[19] = gx;
    dsave[20] = gy;
    dsave[21] = finit;
    dsave[22] = fx;
    dsave[23] = fy;
    dsave[24] = stx;
    dsave[25] = sty;
    dsave[26] = stmin;
    dsave[27] = stmax;
    dsave[28] = width;
    dsave[29] = width1;

    0
}

/// Safe implementation of the step computation in the line search
/// 
/// This routine computes a safeguarded step for a linesearch and updates
/// an interval of uncertainty for a minimizer of the function.
/// 
/// # Arguments
/// * `stx`, `fx`, `dx` - Best step, function value, and derivative
/// * `sty`, `fy`, `dy` - Alternative step, function value, and derivative
/// * `stp`, `fp`, `dp` - Current step, function value, and derivative
/// * `brackt` - Flag indicating if a minimizer has been bracketed
/// * `stpmin` - Lower bound for step
/// * `stpmax` - Upper bound for step
fn dcstep(
    stx: &mut f64,
    fx: &mut f64,
    dx: &mut f64,
    sty: &mut f64,
    fy: &mut f64,
    dy: &mut f64,
    stp: &mut f64,
    fp: &mut f64,
    dp: &mut f64,
    brackt: &mut bool,
    stpmin: &mut f64,
    stpmax: &mut f64,
) -> core::ffi::c_int {
    let sgnd = *dp * (*dx / dx.abs());
    
    let mut stpf;
    if *fp > *fx {
        let theta = (*fx - *fp) * 3.0f64 / (*stp - *stx) + *dx + *dp;
        let s = f64::max(f64::max(theta.abs(), dx.abs()), dp.abs());
        let gamma = s * ((theta / s).powi(2) - *dx / s * (*dp / s)).sqrt();
        let gamma = if *stp < *stx { -gamma } else { gamma };
        
        let p = gamma - *dx + theta;
        let q = gamma - *dx + gamma + *dp;
        let r = p / q;
        let stpc = *stx + r * (*stp - *stx);
        let stpq = *stx + *dx / ((*fx - *fp) / (*stp - *stx) + *dx) / 2.0f64 * (*stp - *stx);
        
        stpf = if (stpc - *stx).abs() < (stpq - *stx).abs() {
            stpc
        } else {
            stpc + (stpq - stpc) / 2.0f64
        };
        *brackt = true;
    } else if sgnd < 0.0f64 {
        let theta = (*fx - *fp) * 3.0f64 / (*stp - *stx) + *dx + *dp;
        let s = f64::max(f64::max(theta.abs(), dx.abs()), dp.abs());
        let gamma = s * ((theta / s).powi(2) - *dx / s * (*dp / s)).sqrt();
        let gamma = if *stp > *stx { -gamma } else { gamma };
        
        let p = gamma - *dp + theta;
        let q = gamma - *dp + gamma + *dx;
        let r = p / q;
        let stpc = *stp + r * (*stx - *stp);
        let stpq = *stp + *dp / (*dp - *dx) * (*stx - *stp);
        
        stpf = if (stpc - *stp).abs() > (stpq - *stp).abs() {
            stpc
        } else {
            stpq
        };
        *brackt = true;
    } else if dp.abs() < dx.abs() {
        let theta = (*fx - *fp) * 3.0f64 / (*stp - *stx) + *dx + *dp;
        let s = f64::max(f64::max(theta.abs(), dx.abs()), dp.abs());
        let gamma = s * f64::max(0.0f64, (theta / s).powi(2) - *dx / s * (*dp / s)).sqrt();
        let gamma = if *stp > *stx { -gamma } else { gamma };
        
        let p = gamma - *dp + theta;
        let q = gamma + (*dx - *dp) + gamma;
        let r = p / q;
        let stpc = if r < 0.0f64 && gamma != 0.0f64 {
            *stp + r * (*stx - *stp)
        } else if *stp > *stx {
            *stpmax
        } else {
            *stpmin
        };
        let stpq = *stp + *dp / (*dp - *dx) * (*stx - *stp);
        
        if *brackt == true {
            stpf = if (stpc - *stp).abs() < (stpq - *stp).abs() {
                stpc
            } else {
                stpq
            };
            if *stp > *stx {
                stpf = f64::min(*stp + (*sty - *stp) * 0.66f64, stpf);
            } else {
                stpf = f64::max(*stp + (*sty - *stp) * 0.66f64, stpf);
            }
        } else {
            stpf = if (stpc - *stp).abs() > (stpq - *stp).abs() {
                stpc
            } else {
                stpq
            };
            stpf = f64::min(*stpmax, stpf);
            stpf = f64::max(*stpmin, stpf);
        }
    } else if *brackt == true {
        let theta = (*fp - *fy) * 3.0f64 / (*sty - *stp) + *dy + *dp;
        let s = f64::max(f64::max(theta.abs(), dy.abs()), dp.abs());
        let gamma = s * ((theta / s).powi(2) - *dy / s * (*dp / s)).sqrt();
        let gamma = if *stp > *sty { -gamma } else { gamma };
        
        let p = gamma - *dp + theta;
        let q = gamma - *dp + gamma + *dy;
        let r = p / q;
        stpf = *stp + r * (*sty - *stp);
    } else {
        stpf = if *stp > *stx { *stpmax } else { *stpmin };
    }

    if *fp > *fx {
        *sty = *stp;
        *fy = *fp;
        *dy = *dp;
    } else {
        if sgnd < 0.0f64 {
            *sty = *stx;
            *fy = *fx;
            *dy = *dx;
        }
        *stx = *stp;
        *fx = *fp;
        *dx = *dp;
    }
    
    *stp = stpf;
    0
}