lbfgsbrs 0.1.2

use super::miniblas::{ddot, dscal, dcopy};
use super::linpack::{dtrsl, dpofa, LinpackError};
use super::debug::{print_fvector};

use log::{info, debug, warn, trace};


const C_1: i32 = 1;
const C_11: i32 = 11;

// Safe implementation of the active function
pub fn active(
    n: usize,
    l: &[f64],
    u: &[f64],
    nbd: &[i32],
    x: &mut [f64],
    iwhere: &mut [i32],
    prjctd: &mut bool,
    cnstnd: &mut bool,
    boxed: &mut bool,
) {
    let mut nbdd = 0;
    *prjctd = false;
    *cnstnd = false;
    *boxed = true;
    
    // First loop: Check if x is within bounds and project if necessary
    for i in 0..n {
        if nbd[i] > 0 {
            if nbd[i] <= 2 && x[i] <= l[i] {
                if x[i] < l[i] {
                    *prjctd = true;
                    x[i] = l[i];
                }
                nbdd += 1;
            } else if nbd[i] >= 2 && x[i] >= u[i] {
                if x[i] > u[i] {
                    *prjctd = true;
                    x[i] = u[i];
                }
                nbdd += 1;
            }
        }
    }
    
    // Second loop: Set iwhere and check if problem is boxed
    for i in 0..n {
        if nbd[i] != 2 {
            *boxed = false;
        }
        
        if nbd[i] == 0 {
            iwhere[i] = -1;
        } else {
            *cnstnd = true;
            if nbd[i] == 2 && u[i] - l[i] <= 0.0 {
                iwhere[i] = 3;
            } else {
                iwhere[i] = 0;
            }
        }
    }
    
    // Print information if requested
    if *prjctd {
        info!("The initial X is infeasible. Restart with its projection");
    }
    if !*cnstnd {
        info!("This problem is unconstrained");
    }

    info!("At X0, {} variables are exactly at the bounds", nbdd);
}

// Safe implementation of the bmv function
pub fn bmv(
    m: usize,
    sy: &[f64],
    wt: &[f64],
    col: usize,
    v: &[f64],
    p: &mut [f64],
) -> Result<(), i32> {
    if col == 0 {
        return Ok(());
    }
    
    // First part: compute p(col+i) = v(col+i) + sum_{k=1}^{i-1} sy(i,k)*v(k)/sy(k,k)
    p[col] = v[col];
    
    for i in 2..=col {
        let i2 = col + i;
        let mut sum = 0.0;
        
        for k in 1..i {
            // Note: sy is stored in column-major format
            sum += sy[(i-1) + (k-1) * m] * v[k-1] / sy[(k-1) + (k-1) * m];
        }
        
        p[i2-1] = v[i2-1] + sum;
    }
    
    // Second part: solve the triangular system
    // We need to use the safe dtrsl function from linpack.rs
    let mut p_col_plus = vec![0.0; col];
    for i in 0..col {
        p_col_plus[i] = p[col + i];
    }

    // Create a mutable copy of wn for dtrsl
    let mut wt_copy = vec![0.0; wt.len()];
    wt_copy.copy_from_slice(wt);
    
    match dtrsl(
        &mut wt_copy,
        m,
        col,
        &mut p_col_plus,
        C_11 // job parameter for lower triangular, no transpose
    ) {
        Ok(_) => {
            // Copy back the results
            for i in 0..col {
                p[col + i] = p_col_plus[i];
            }
        },
        Err(_) => return Err(1),
    }
    
    // Third part: compute p(i) = v(i)/sqrt(sy(i,i))
    for i in 1..=col {
        p[i-1] = v[i-1] / sy[(i-1) + (i-1) * m].sqrt();
    }
    
    // Fourth part: solve another triangular system
    let mut p_col_plus = vec![0.0; col];
    for i in 0..col {
        p_col_plus[i] = p[col + i];
    }
    
    match dtrsl(
        &mut wt_copy,
        m,
        col,
        &mut p_col_plus,
        C_1 // job parameter for lower triangular, transpose
    ) {
        Ok(_) => {
            // Copy back the results
            for i in 0..col {
                p[col + i] = p_col_plus[i];
            }
        },
        Err(_) => return Err(1),
    }
    
    // Fifth part: negate and scale p(i)
    for i in 1..=col {
        p[i-1] = -p[i-1] / sy[(i-1) + (i-1) * m].sqrt();
    }
    
    // Sixth part: compute the final values of p(i)
    for i in 1..=col {
        let mut sum = 0.0;
        
        for k in (i+1)..=col {
            sum += sy[(k-1) + (i-1) * m] * p[col + k - 1] / sy[(i-1) + (i-1) * m];
        }
        
        p[i-1] += sum;
    }
    
    Ok(())
}

// Helper function to fix index in 0-based indexing
fn idx(idx0: usize) -> usize {
    if idx0 > 0 {
        idx0 - 1
    }
    else {
        0
    }
}

fn initialize_cauchy(
    n: usize,
    x: &[f64],
    l: &[f64],
    u: &[f64],
    nbd: &[i32],
    g: &[f64],
    iorder: &mut [i32],
    iwhere: &mut [i32],
    t: &mut [f64],
    d: &mut [f64],
    m: usize,
    wy: &[f64],
    ws: &[f64],
    col: usize,
    head: usize,
    p: &mut [f64]
) -> (bool, usize, usize, usize, f64, f64) {
    // Initialize variables
    let mut bnded = true;
    let mut nfree = n + 1;
    let mut nbreak = 0;
    let mut ibkmin = 0;
    let mut bkmin = 0.0;
    let col2 = col * 2;
    let mut f1 = 0.0;
    let mut tl: f64 = 0.0;
    let mut tu: f64 = 0.0;

    // Initialize p to zero
    for i in 0..col2 {
        p[i] = 0.0;
    }

    // Compute the breakpoints and initialize d and xcp
    for i in 0..n {
        let neggi = -g[i];
        
        // Skip if variable is fixed
        if iwhere[i] != 3 && iwhere[i] != -1 {
            // Compute the breakpoints
            tl = 0.0;
            tu = 0.0;
            
            if nbd[i] <= 2 {
                tl = x[i] - l[i];
            }
            if nbd[i] >= 2 {
                tu = u[i] - x[i];
            }
            
            let xlower = nbd[i] <= 2 && tl <= 0.0;
            let xupper = nbd[i] >= 2 && tu <= 0.0;
            
            // Initialize iwhere
            iwhere[i] = 0;
            
            if xlower {
                if neggi <= 0.0 {
                    iwhere[i] = 1;
                }
            } else if xupper {
                if neggi >= 0.0 {
                    iwhere[i] = 2;
                }
            } else if neggi.abs() <= 0.0 {
                iwhere[i] = -3;
            }
        } else {
            trace!("  Variable {} is fixed (iwhere[{}] = {})", i, i, iwhere[i]);
        }
        
        let mut pointr = head;
        
        // Initialize d and compute f1
        if iwhere[i] != 0 && iwhere[i] != -1 {
            d[i] = 0.0;
        } else {
            d[i] = neggi;
            f1 -= neggi * neggi;
            
            // Compute p = W^T d
            for j in 0..col {                
                p[j] += wy[i + idx(pointr) * n] * neggi;
                p[col + j] += ws[i + idx(pointr) * n] * neggi;

                pointr = pointr % m + 1;
            }
            
            // Check if a breakpoint needs to be added
            if nbd[i] <= 2 && nbd[i] != 0 && neggi < 0.0 {
                // Add a breakpoint for the lower bound
                nbreak += 1;
                iorder[idx(nbreak)] = i as i32 + 1; // +1 for 1-based indexing
                t[idx(nbreak)] = tl / -neggi;
                
                // Update the minimum breakpoint
                if nbreak == 1 || t[idx(nbreak)] < bkmin {
                    bkmin = t[idx(nbreak)];
                    ibkmin = nbreak;
                }
            } else if nbd[i] >= 2 && neggi > 0.0 {
                // Add a breakpoint for the upper bound
                nbreak += 1;
                iorder[idx(nbreak)] = i as i32 + 1; // +1 for 1-based indexing
                t[idx(nbreak)] = tu / neggi;
                
                // Update the minimum breakpoint
                if nbreak == 1 || t[idx(nbreak)] < bkmin {
                    bkmin = t[idx(nbreak)];
                    ibkmin = nbreak;
                }
            } else {
                // Variable is free
                nfree -= 1;
                iorder[idx(nfree)] = i as i32 + 1; // +1 for 1-based indexing
                
                if neggi.abs() > 0.0 {
                    bnded = false;
                }
            }
        }
    }

    // Return the computed values
    (bnded, nfree, nbreak, ibkmin, bkmin, f1)
}

/// Safe implementation of the hpsolb_0 function
///
/// This function performs heap sort operations for the L-BFGS-B algorithm.
/// It can either initialize a heap (when iheap=0) or adjust the heap after removing the minimum element.
///
/// # Parameters
/// * `t` - Working array for the breakpoints
/// * `iorder` - Working array for sorting
/// * `n` - Number of elements to sort
/// * `iheap` - Flag indicating whether to initialize the heap (0) or adjust it (1)
fn heap_sort(t: &mut [f64], iorder: &mut [i32], n: usize, iheap: i32) {
    
    // If iheap is 0, initialize the heap
    if iheap == 0 {
        
        // Start from the second element (index 1 in 0-based indexing)
        for k in 1..=n {
            let ddum = t[idx(k)];
            let indxin = iorder[idx(k)];
            
            // Move up the heap
            let mut i = k;
            while i > 1 {
                let j = i / 2;
                if !(ddum < t[idx(j)]) {
                    break;
                }
                
                t[idx(i)] = t[idx(j)];
                iorder[idx(i)] = iorder[idx(j)];
                i = j;
            }
            
            t[idx(i)] = ddum;
            iorder[idx(i)] = indxin;
        }
    }
    
    // If n > 1, adjust the heap after removing the minimum element
    if n > 0 {
        
        // Save the root (minimum) element
        let out = t[0];
        let indxou = iorder[0];
        
        // Move the last element to the root
        let ddum = t[idx(n)];
        let indxin = iorder[idx(n)];
        
        // Sift down to maintain heap property
        let mut i = 1;
        loop {
            let mut j = i + i; // Left child
            
            // If no children, break
            if j > n - 1 {
                break;
            }
            
            // Choose the smaller child
            if j < n - 1 && t[idx(j + 1)] < t[idx(j)] {
                j += 1;
            }
            
            // If the element is in the right place, break
            if !(t[idx(j)] < ddum) {
                break;
            }
            
            // Move the smaller child up
            t[idx(i)] = t[idx(j)];
            iorder[idx(i)] = iorder[idx(j)];
            i = j;
        }
        
        // Place the last element in its correct position
        t[idx(i)] = ddum;
        iorder[idx(i)] = indxin;
        
        // Move the minimum element to the end
        t[idx(n)] = out;
        iorder[idx(n)] = indxou;
    }
}

/// Instrumented version of the cauchy_safe function with extensive debug logging
///
/// This function computes the generalized Cauchy point (GCP) for the L-BFGS-B algorithm
/// with added instrumentation to log variable values and execution steps.
///
/// # Parameters
/// * `n` - Number of variables
/// * `x` - Current point
/// * `l` - Lower bounds
/// * `u` - Upper bounds
/// * `nbd` - Type of bounds for each variable (0: unbounded, 1: lower bound only, 2: both bounds, 3: upper bound only)
/// * `g` - Gradient at the current point
/// * `iorder` - Working array for sorting
/// * `iwhere` - Working array indicating the status of each variable
/// * `t` - Working array for the breakpoints
/// * `d` - Direction of search (projected gradient)
/// * `xcp` - The generalized Cauchy point (output)
/// * `m` - Maximum number of variable metric corrections
/// * `wy` - Matrix of Y vectors (size n×m)
/// * `ws` - Matrix of S vectors (size n×m)
/// * `sy` - Matrix of S'Y values (size m×m)
/// * `wt` - Working matrix for the BFGS update (size m×m)
/// * `theta` - Scaling factor for the BFGS update
/// * `col` - Current column in the limited memory matrix
/// * `head` - Pointer to the first element in the circular queue
/// * `p` - Working array
/// * `c` - Working array
/// * `wbp` - Working array
/// * `v` - Working array
/// * `nseg` - Number of segments explored (output)
/// * `sbgnrm` - Norm of the projected gradient
/// * `epsmch` - Machine precision
///
/// # Returns
/// * `Result<(), i32>` - Ok(()) on success, Err(code) on failure
pub fn cauchy(
    n: usize,
    x: &[f64],
    l: &[f64],
    u: &[f64],
    nbd: &[i32],
    g: &[f64],
    iorder: &mut [i32],
    iwhere: &mut [i32],
    t: &mut [f64],
    d: &mut [f64],
    xcp: &mut [f64],
    m: usize,
    wy: &[f64],
    ws: &[f64],
    sy: &[f64],
    wt: &[f64],
    theta: f64,
    col: usize,
    head: usize,
    p: &mut [f64],
    c: &mut [f64],
    wbp: &mut [f64],
    v: &mut [f64],
    nseg: &mut i32,
    sbgnrm: f64,
    epsmch: f64
) -> Result<(), i32> {
    
    let col2 = 2 * col;

    // Early return if the norm of the projected gradient is zero
    if sbgnrm <= 0.0 {
        info!("Subnorm = 0. GCP = X.");
        xcp.copy_from_slice(x);
        return Ok(());
    }

    // Call the initialize_cauchy function and get the return values
    let (bnded, nfree, nbreak, ibkmin, bkmin, mut f1) = initialize_cauchy(
        n, x, l, u, nbd, g, iorder, iwhere, t, d, m, wy, ws, col, head, p
    );
    
    // Scale p if theta != 1.0
    if theta != 1.0 {
        dscal(col as i32, theta, &mut p[col..], 1);
    }

    // Initialize xcp = x
    xcp.copy_from_slice(x);

    // If no breakpoints, return
    if nbreak == 0 && nfree == n + 1 {
        info!("No breakpoints and all variables are fixed. Returning with xcp = x.");
        return Ok(());
    }

    // Initialize c to zero
    for j in 0..col2 {
        c[j] = 0.0;
    }

    // Compute f2 = -theta * f1
    let mut f2 = -theta * f1;
    let f2_org = f2;

    // Compute v = W^T B W p
    if col > 0 {  
        // Call bmv_safe to compute v = W^T B W p
        match bmv(m, sy, wt, col, p, v) {
            Ok(_) => {
                log::trace!("bmv successful");
            },
            Err(e) => {
                log::warn!("Error during bmv: {:?}", e);
            },
        }

        // Compute f2 -= p^T v
        f2 -= ddot(col2 as i32, v, 1, p, 1);
    }

    // Compute the generalized Cauchy point
    let mut dtm = -f1 / f2; // Initial step to the minimum
    let mut tsum = 0.0;     // Total step length
    *nseg = 1;              // Number of segments

    // If no breakpoints, go directly to the minimum
    if nbreak == 0 {
        // GCP found in this segment
        info!("No breakpoints. GCP found in first segment.");
        debug!("Piece {} --f1={}, f2={} at start point", *nseg, f1, f2);
        debug!("Distance to the stationary point = {}", dtm);
        
        // Ensure dtm is non-negative
        if dtm <= 0.0 {
            dtm = 0.0;
        }
        
        // Update tsum and xcp        
        for i in 0..n {
            xcp[i] += tsum * d[i];
        }
    } 
    else {
        // Process breakpoints
        let mut nleft = nbreak;
        let mut iter = 1;
        let mut tj = 0.0;
        
        loop {
            let tj0 = tj;
            let mut ibp = 0;
            
            // Determine the next breakpoint
            if iter == 1 {
                // First iteration, use the minimum breakpoint
                tj = bkmin;
                ibp = iorder[idx(ibkmin)] as usize - 1; // -1 for 0-based indexing
            } 
            else {
                // Subsequent iterations
                if iter == 2 && ibkmin != nbreak {
                    // Move the last breakpoint to the position of ibkmin
                    t[idx(ibkmin)] = t[idx(nbreak)];
                    iorder[idx(ibkmin)] = iorder[idx(nbreak)];
                }
                
                // Use heap sort to find the next breakpoint
                heap_sort(t, iorder, nleft, iter - 2);

                tj = t[idx(nleft)];
                ibp = iorder[idx(nleft)] as usize - 1; // -1 for 0-based indexing
            }
            
            // Compute the step to the breakpoint
            let dt = tj - tj0;
            
            if dt != 0.0 {
                debug!("Piece {} --f1={}, f2={} at start point", *nseg, f1, f2);
                debug!("Distance to the next break point = {}", dt);
                debug!("Distance to the stationary point = {}", dtm);
            }
            
            // Check if the minimum is before the next breakpoint
            if dtm < dt {
                // GCP found in this segment
                debug!("\nGCP found in this segment. Piece {} --f1, f2 at start point {} {}", 
                       nseg, f1, f2);
                debug!("Distance to the stationary point = {}", dtm);
                
                // Ensure dtm is non-negative
                if dtm <= 0.0 {
                    dtm = 0.0;
                }
                
                // Update tsum and xcp
                tsum += dtm;
                
                for i in 0..n {
                    xcp[i] += tsum * d[i];
                }

                break;
            }
            
            // Move to the breakpoint
            tsum += dt;
            nleft -= 1;
            iter += 1;
            
            // Fix the variable at the breakpoint
            let dibp = d[ibp];
            d[ibp] = 0.0;
            
            let mut zibp = 0.0;
            if dibp > 0.0 {
                zibp = u[ibp] - x[ibp];
                xcp[ibp] = u[ibp];
                iwhere[ibp] = 2;
            } 
            else {
                zibp = l[ibp] - x[ibp];
                xcp[ibp] = l[ibp];
                iwhere[ibp] = 1;
            }
            
            // Check if all variables are fixed
            if nleft == 0 && nbreak == n {
                // All variables are fixed, exit the loop
                dtm = dt;
                tsum += dtm;
                
                for i in 0..n {
                    xcp[i] += tsum * d[i];
                }
                break;
            }
            
            // Update the quadratic model
            *nseg += 1;
            let dibp2 = dibp * dibp;

            f1 = f1 + dt * f2 + dibp2 - theta * dibp * zibp;
            f2 -= theta * dibp2;
            
            if col > 0 {                
                // Update c = c + dt * p
                for i in 0..col2 {
                    c[i] += dt * p[i];
                }
                
                // Compute wbp = W^T e_ibp
                let mut j_pointr = head;
                for j in 0..col {
                    wbp[j] = wy[ibp + idx(j_pointr) * n];
                    wbp[col + j] = theta * ws[ibp + idx(j_pointr) * n];
                    j_pointr = j_pointr % m + 1;
                }
                
                // Compute v = W^T B W wbp
                let result = bmv(m, sy, wt, col, wbp, v);
                
                if let Err(_) = result {
                    return Ok(());
                }
                
                // Compute dot products
                let wmc = ddot(col2 as i32, c, 1, v, 1);
                let wmp = ddot(col2 as i32, p, 1, v, 1);
                let wmw = ddot(col2 as i32, wbp, 1, v, 1);
                
                // Update p = p - dibp * wbp
                for i in 0..col2 {
                    p[i] -= dibp * wbp[i];
                }
                
                // Update f1 and f2
                f1 += dibp * wmc;
                f2 = f2 + dibp * 2.0 * wmp - dibp2 * wmw;
            }
            
            // Ensure f2 is positive
            f2 = f64::max(epsmch * f2_org, f2);
            
            // Compute the new step to the minimum
            if nleft > 0 {
                dtm = -f1 / f2;
            } 
            else {
                // All breakpoints processed
                if bnded {
                    f1 = 0.0;
                    f2 = 0.0;
                    dtm = 0.0;
                } else {
                    dtm = -f1 / f2;
                }
                
                // GCP found in this segment
                info!("GCP found in final segment ({})", *nseg);
                debug!("Piece {} --f1={}, f2={} at start point", *nseg, f1, f2);
                debug!("Distance to the stationary point = {}", dtm);
                
                // Ensure dtm is non-negative
                if dtm <= 0.0 {
                    dtm = 0.0;
                }
                
                // Update tsum and xcp
                tsum += dtm;
                
                for i in 0..n {
                    xcp[i] += tsum * d[i];
                }
                
                // Exit the loop
                break;
            }
        }
    }

    // Update c if col > 0
    if col > 0 {
        for i in 0..col2 {
            c[i] += dtm * p[i];
        }
    }

    // Log the Cauchy point
    print_fvector(xcp, n, "Cauchy X = ");

    Ok(())
}

// Safe implementation of the cmprlb function
pub fn cmprlb(
    n: usize,
    m: usize,
    x: &[f64],
    g: &[f64],
    ws: &[f64],
    wy: &[f64],
    sy: &[f64],
    wt: &[f64],
    z: &[f64],
    r: &mut [f64],
    wa: &mut [f64],
    index: &[i32],
    theta: f64,
    col: usize,
    head: usize,
    nfree: usize,
    cnstnd: bool,
) -> Result<(), i32> {

    // If the problem is unconstrained, simply set r = -g
    if !cnstnd && col > 0 {
        for i in 0..n {
            r[i] = -g[i];
        }
        return Ok(());
    }
    
    // For constrained problems, compute r = -theta*(z-x) - g
    for i in 0..nfree {
        let k = index[i] as usize - 1; // Adjust for 0-based indexing
        r[i] = -theta * (z[k] - x[k]) - g[k];
    }
    
    // If there are no corrections, return
    if col == 0 {
        return Ok(());
    }
    
    // Compute wa = (sy)^T * (wt)^-1 * v
    // First, set up the input vector for bmv
    let mut v = vec![0.0; 2 * col];
    for i in 0..2*col {
        v[i] = wa[2 * m + i];
    }
    
    // Call bmv_safe to compute the matrix-vector product
    let mut p = vec![0.0; 2 * col];
    bmv(m, sy, wt, col, &v, &mut p)?;
    
    // Copy the result to wa
    for i in 0..2*col {
        wa[i] = p[i];
    }
    
    // Update r using the result from bmv
    let mut pointr = head;
    for j in 1..=col {
        let a1 = wa[j-1];
        let a2 = theta * wa[col+j-1];
        
        for i in 0..nfree {
            let k = index[i] as usize - 1; // Adjust for 0-based indexing
            r[i] += wy[k + (pointr-1) * n] * a1 + ws[k + (pointr-1) * n] * a2;
        }
        
        pointr = pointr % m + 1;
    }
    
    Ok(())
}

/// Safe implementation of the formk function from L-BFGS-B algorithm.
///
/// This function forms the upper half of the BFGS matrix in the compact form.
/// It updates the matrices WN and WN1 which are used to compute the search direction.
///
/// # Parameters
/// * `n` - Number of variables
/// * `nsub` - Number of free variables
/// * `ind` - Indices of free variables
/// * `nenter` - Number of variables entering the free set
/// * `ileave` - Number of variables leaving the free set
/// * `indx2` - Array of indices for variables entering/leaving the free set
/// * `iupdat` - Counter for BFGS updates
/// * `updatd` - Boolean flag indicating if the BFGS matrix was updated
/// * `wn` - Working matrix for the BFGS update (size 2m×2m)
/// * `wn1` - Working matrix for the BFGS update (size 2m×2m)
/// * `m` - Maximum number of variable metric corrections
/// * `ws` - Matrix of s vectors (size n×m)
/// * `wy` - Matrix of y vectors (size n×m)
/// * `sy` - Matrix of s'y values (size m×m)
/// * `theta` - Scaling factor for the BFGS update
/// * `col` - Current column in the limited memory matrix
/// * `head` - Pointer to the first element in the circular queue
///
/// # Returns
/// * `Result<(), i32>` - Ok(()) on success, Err(-1) on failure
pub fn formk(
    n: usize,
    nsub: usize,
    ind: &[i32],
    nenter: usize,
    ileave: usize,
    indx2: &[i32],
    iupdat: usize,
    updatd: bool,
    wn: &mut [f64],
    wn1: &mut [f64],
    m: usize,
    ws: &[f64],
    wy: &[f64],
    sy: &[f64],
    theta: f64,
    col: usize,
    head: usize,
) -> Result<(), i32> {

    let wn1_dim: usize = 2 * m;
    let wn_dim: usize = 2 * m;

    // If the BFGS matrix has been updated, we need to update the WN1 matrix
    if updatd {
        // If we have more updates than the memory size, we need to shift the old information
        if iupdat > m {            
            // Shift the old information in the WN1 matrix
            let i__1 = m - 1;
            
            // Use 0-based indexing
            for jy in 0..i__1 {
                let js = m + jy;
                
                // Copy the lower triangular part of WN1
                let i__2 = m - jy - 1; // Adjusted for 0-based indexing
                // Create slices for the source and destination
                let src_offset = ((jy + 1) + (jy + 1) * wn1_dim) as usize; // Equivalent to 1-based jy + 1
                let dst_offset = (jy + jy * wn1_dim) as usize; // Equivalent to 1-based jy
                let slice_len = i__2 as usize;
                let src_slice_upper = wn1[src_offset.. src_offset + slice_len].to_vec();
                dcopy(i__2 as i32, &src_slice_upper, 1, &mut wn1[dst_offset..dst_offset + slice_len], 1);
                
                // Copy the upper triangular part of WN1
                let i__2 = m - jy - 1; // Adjusted for 0-based indexing
                let src_offset = ((js + 1) + (js + 1) * wn1_dim) as usize; // Equivalent to 1-based js + 1
                let dst_offset = (js + js * wn1_dim) as usize; // Equivalent to 1-based js
                let slice_len = i__2 as usize;
                let src_slice_lower = wn1[src_offset.. src_offset + slice_len].to_vec();
                dcopy(i__2 as i32, &src_slice_lower, 1, &mut wn1[dst_offset..dst_offset + slice_len], 1);
                
                // Copy the rectangular part of WN1
                let i__2 = m - 1;
                let src_offset = (m + 1 + (jy + 1) * wn1_dim) as usize; // Equivalent to 1-based m + 2 + (jy + 1) * wn1_dim
                let dst_offset = (m + jy * wn1_dim) as usize; // Equivalent to 1-based m + 1 + jy * wn1_dim
                let slice_len = i__2 as usize;
                let src_slice_rect = wn1[src_offset.. src_offset + slice_len].to_vec();
                dcopy(i__2 as i32, &src_slice_rect, 1, &mut wn1[dst_offset..dst_offset + slice_len], 1);
            }
        }
   
        // Define indices for the free variables
        let pbegin = 0;
        let pend = nsub - 1;
        let dbegin = nsub;
        let dend = n - 1;
        
        // Set up pointers for the circular queue
        let iy = col - 1;
        let is = m + col - 1;
        let mut ipntr = head + col - 2;
        if ipntr >= m {
            ipntr -= m;
        }
        let mut jpntr = head - 1;

        // Compute the new elements of WN1 for the current column
        for jy in 0..col {
            let js = m + jy;
            let mut temp1 = 0.0;  // For wy'wy
            let mut temp2 = 0.0;  // For ws'ws
            let mut temp3 = 0.0;  // For ws'wy

            // Compute dot products for the free variables
            for k in pbegin..=pend {
                let k1 = ind[k] as usize - 1;
                let wy_ipntr_k1 = wy[k1 + ipntr * n];
                let wy_jpntr_k1 = wy[k1 + jpntr * n];
                temp1 += wy_ipntr_k1 * wy_jpntr_k1;
            }

            // Compute dot products for the fixed variables
            for k in dbegin..=dend {
                let k1 = ind[k] as usize - 1;
                let ws_ipntr_k1 = ws[k1 + ipntr * n];
                let ws_jpntr_k1 = ws[k1 + jpntr * n];
                let wy_jpntr_k1 = wy[k1 + jpntr * n];
                
                temp2 += ws_ipntr_k1 * ws_jpntr_k1;
                temp3 += ws_ipntr_k1 * wy_jpntr_k1;
            }

            // Store the computed values in WN1
            wn1[iy + jy * wn1_dim] = temp1;
            wn1[is + js * wn1_dim] = temp2;
            wn1[is + jy * wn1_dim] = temp3;
            
            // Move to the next column in the circular queue
            jpntr = (jpntr + 1) % m;
        }

        // Compute the remaining elements of WN1
        let jy = col - 1;
        let mut jpntr = head + col - 2;
        if jpntr >= m {
            jpntr -= m;
        }
        let mut ipntr = head - 1;

        for i in 0..col {
            let is = m + i;
            let mut temp3 = 0.0;  // For ws'wy

            // Compute dot products for the free variables
            for k in pbegin..=pend {
                let k1 = ind[k] as usize - 1;
                let ws_ipntr_k1 = ws[k1 + ipntr * n];
                let wy_jpntr_k1 = wy[k1 + jpntr * n];
                temp3 += ws_ipntr_k1 * wy_jpntr_k1;
            }

            // Move to the next row in the circular queue
            ipntr = (ipntr + 1) % m;
            
            // Store the computed value in WN1
            wn1[is + jy * wn1_dim] = temp3;
        }
    }

    // Determine how many columns to update
    let upcl = if updatd { col - 1 } else { col };
    let mut ipntr = head - 1;

    // Update WN1 for variables entering and leaving the free set
    for iy in 0..upcl {
        let is = m + iy;
        let mut jpntr = head - 1;

        for jy in 0..=iy {
            let js = m + jy;
            let mut temp1 = 0.0;  // For wy'wy (entering)
            let mut temp2 = 0.0;  // For ws'ws (entering)
            let mut temp3 = 0.0;  // For wy'wy (leaving)
            let mut temp4 = 0.0;  // For ws'ws (leaving)

            // Compute dot products for variables entering the free set
            for k in 0..nenter {
                let k1 = indx2[k] as usize - 1;
                let wy_ipntr_k1 = wy[k1 + ipntr * n];
                let wy_jpntr_k1 = wy[k1 + jpntr * n];
                let ws_ipntr_k1 = ws[k1 + ipntr * n];
                let ws_jpntr_k1 = ws[k1 + jpntr * n];
                
                temp1 += wy_ipntr_k1 * wy_jpntr_k1;
                temp2 += ws_ipntr_k1 * ws_jpntr_k1;
            }

            // Compute dot products for variables leaving the free set
            for k in (ileave - 1)..n {
                let k1 = indx2[k] as usize - 1;
                let wy_ipntr_k1 = wy[k1 + ipntr * n];
                let wy_jpntr_k1 = wy[k1 + jpntr * n];
                let ws_ipntr_k1 = ws[k1 + ipntr * n];
                let ws_jpntr_k1 = ws[k1 + jpntr * n];
                
                temp3 += wy_ipntr_k1 * wy_jpntr_k1;
                temp4 += ws_ipntr_k1 * ws_jpntr_k1;
            }

            // Update WN1 with the computed values
            wn1[iy + jy * wn1_dim] += temp1 - temp3;
            wn1[is + js * wn1_dim] += -temp2 + temp4;
            
            // Move to the next column in the circular queue
            jpntr = (jpntr + 1) % m;
        }
        // Move to the next row in the circular queue
        ipntr = (ipntr + 1) % m;
    }

    // Update the cross-terms in WN1
    let mut ipntr = head - 1;

    for is in (m)..(m + upcl) {
        let mut jpntr = head - 1;

        for jy in 0..upcl {
            let mut temp1 = 0.0;  // For ws'wy (entering)
            let mut temp3 = 0.0;  // For ws'wy (leaving)

            // Compute dot products for variables entering the free set
            for k in 0..nenter {
                let k1 = indx2[k] as usize - 1;
                let ws_ipntr_k1 = ws[k1 + ipntr * n];
                let wy_jpntr_k1 = wy[k1 + jpntr * n];
                
                temp1 += ws_ipntr_k1 * wy_jpntr_k1;
            }

            // Compute dot products for variables leaving the free set
            for k in (ileave - 1)..n {
                let k1 = indx2[k] as usize - 1;
                let ws_ipntr_k1 = ws[k1 + ipntr * n];
                let wy_jpntr_k1 = wy[k1 + jpntr * n];
                
                temp3 += ws_ipntr_k1 * wy_jpntr_k1;
            }

            // Update WN1 with the computed values, handling upper/lower triangular parts
            if is <= jy + m {
                wn1[is + jy * wn1_dim] += temp1 - temp3;
            } else {
                wn1[is + jy * wn1_dim] += -temp1 + temp3;
            }
            
            // Move to the next column in the circular queue
            jpntr = (jpntr + 1) % m;
        }
        // Move to the next row in the circular queue
        ipntr = (ipntr + 1) % m;
    }

    // Form the upper half of the BFGS matrix in the compact form    
    for iy in 0..col {
        let is = col + iy;
        let is1 = m + iy;

        for jy in 0..=iy {
            let js = col + jy;
            let js1 = m + jy;
            
            // Scale the matrices by theta
            wn[jy + iy * wn_dim] = wn1[iy + jy * wn_dim] / theta;
            wn[js + is * wn_dim] = wn1[is1 + js1 * wn_dim] * theta;
        }

        // Copy the cross-terms
        for jy in 0..(iy) {
            wn[jy + is * wn_dim] = -wn1[is1 + jy * wn_dim];
        }

        for jy in iy..col {
            wn[jy + is * wn_dim] = wn1[is1 + jy * wn_dim];
        }

        // Add the s'y values to the diagonal
        wn[iy + iy * wn_dim] += sy[iy + iy * m];
    }
 
    // Perform Cholesky factorization on the upper left block
    match dpofa(wn, 2 * m, col) {
        Ok(_) => {
            trace!("Upper left block Cholesky factorization successful");
        },
        Err(e) => {
            warn!("Upper left block Cholesky factorization failed with error: {:?}", e);
            return Err(-1);  // Return error if factorization fails
        },
    }

    let col2 = col << 1;  // col2 = 2*col

    // Solve the triangular system for the upper right block
    for js in (col)..col2 {        
        // Create a mutable copy of wn for dtrsl
        let mut wn_copy = vec![0.0; wn.len()];
        wn_copy.copy_from_slice(wn);
        
        // Solve the triangular system
        match dtrsl(
            &mut wn_copy,
            2 * m,
            col,
            &mut wn[js * wn_dim..],
            11, // job parameter for lower triangular, no transpose
        ) {
            Ok(_) => {
                trace!("dtrsl successful for column {}", js);
            },
            Err(e) => {
                warn!("dtrsl failed for column {} with error: {:?}", js, e);
                return Err(-1);  // Return error if triangular solve fails
            },
        }
    }

    // Form the lower right block of the BFGS matrix
    for is in (col)..col2 {
        for js in is..col2 {
            let mut sum = 0.0;
            
            // Compute the dot product
            for k in 0..col {
                sum += wn[is * wn_dim + k] * wn[js * wn_dim + k];
            }
            // TODO: check if it's possible to remove the previous loop
            let mut d = ddot(col as i32, &wn[(is * wn_dim) as usize..], 1, &wn[js * wn_dim..], 1);
            
            // Update the lower right block
            let idx = is + js * wn_dim;
            wn[idx] += sum;
        }
    }

    // Perform Cholesky factorization on the lower right block
    let upper_right_block_start = (col) + (col) * 2 * m;
    let upper_right_block = &mut wn[upper_right_block_start..];

    match dpofa(upper_right_block, 2 * m, col) {
        Ok(_) => {
            trace!("Lower right block Cholesky factorization successful");
        },
        Err(e) => {
            warn!("Lower right block Cholesky factorization failed with error: {:?}", e);
            return Err(-1);  // Return error if factorization fails
        },
    }

    debug!("formk completed successfully");
    Ok(())
}

pub fn formt(
    m: usize,
    wt: &mut [f64],
    sy: &[f64],
    ss: &[f64],
    col: usize,
    theta: f64,
) -> Result<(), LinpackError> {
    // Initialize the wt matrix
    for j in 0..col {
        wt[j * m] = theta * ss[j * m];
    }

    for i in 1..col {
        for j in i..col {
            let k1 = (i.min(j) - 1) as usize;
            let mut ddum = 0.0;
            for k in 0..=k1 {
                ddum += sy[i + k * m] * sy[j + k * m] / sy[k + k * m];
            }
            wt[i + j * m] = ddum + theta * ss[i + j * m];
        }
    }

    // Call the safe dpofa function
    dpofa(wt, m, col)?;

    Ok(())
}

pub fn freev(
    n: usize,
    nfree: &mut usize,
    index: &mut [i32],
    nenter: &mut i32,
    ileave: &mut i32,
    indx2: &mut [i32],
    iwhere: &[i32],
    wrk: &mut bool,
    updatd: bool,
    cnstnd: bool,
    iter: i32,
) {
    // Initialize counters
    *nenter = 0;
    *ileave = n as i32 + 1;
    
    // If this is not the first iteration and the problem is constrained
    if iter > 0 && cnstnd {
        // Check which variables leave the free set
        for i in 1..=*nfree {
            let k = index[i-1];
            if iwhere[k as usize - 1] > 0 {
                *ileave -= 1;
                indx2[*ileave as usize - 1] = k;
            }
        }
        
        // Check which variables enter the free set
        for i in (*nfree + 1)..=n {
            let k = index[i-1];
            if iwhere[k as usize - 1] <= 0 {
                *nenter += 1;
                indx2[*nenter as usize - 1] = k;
            }
        }
    }
    
    // Set the work flag if there are changes in the free set or if the solution was updated
    *wrk = *ileave < (n as i32 + 1) || *nenter > 0 || updatd;
    
    // Rearrange index set so that free variables come first
    *nfree = 0;
    let mut iact = n;
    
    for i in 1..=n {
        let k = i;
        if iwhere[k-1] <= 0 {
            *nfree += 1;
            index[*nfree-1] = k as i32;
        } else {
            iact -= 1;
            index[iact] = k as i32;
        }
    }
    
    debug!("{} variables are free at GCP iter {}", *nfree, iter + 1);
}

/// Updates the L-BFGS matrices after a step is taken
/// 
/// This function updates the limited memory BFGS matrices (WS, WY, SY, SS) that are used
/// to approximate the Hessian. It's called after each successful step in the optimization.
///
/// # Parameters:
/// - n: problem dimension
/// - m: maximum number of variable metric corrections
/// - ws, wy: workspace matrices for storing S and Y
/// - sy, ss: matrices for the BFGS update
/// - d: search direction
/// - r: gradient difference
/// - itail: tail of the circular queue
/// - iupdat: number of BFGS updates made so far
/// - col: number of variable metric corrections stored
/// - head: head of the circular queue
/// - theta: scaling factor for the BFGS update
/// - rr, dr: dot products used in the BFGS update
/// - stp: step length
/// - dtd: dot product of d with itself
pub fn matupd(
    n: usize,
    m: usize,
    ws: &mut [f64],
    wy: &mut [f64],
    sy: &mut [f64],
    ss: &mut [f64],
    d: &[f64],
    r: &[f64],
    itail: &mut usize,
    iupdat: &mut usize,
    col: &mut usize,
    head: &mut usize,
    theta: &mut f64,
    rr: f64,
    dr: f64,
    stp: f64,
    dtd: f64,
) -> i32 {
    // Matrix dimensions and offsets for 1-based indexing
    let wy_dim1 = n;
    let sy_dim1 = m;
    let ss_dim1 = m;

    // Update column count and tail pointer
    if *iupdat <= m {
        // We haven't filled the memory yet
        *col = *iupdat;
        *itail = (*head + *iupdat - 2) % m + 1;
    } else {
        // Memory is full, update in circular fashion
        *itail = *itail % m + 1;
        *head = *head % m + 1;
    }

    let col_zero = *col - 1;
    let itail_zero = *itail - 1;
    
    // Copy d to ws(:, itail)
    for i in 0..n {
        ws[(i + itail_zero * n) as usize] = d[i as usize];
    }
    
    // Copy r to wy(:, itail)
    for i in 0..n {
        wy[(i + itail_zero * n) as usize] = r[i as usize];
    }

    // Compute the scaling factor theta = rr/dr
    *theta = rr / dr;

    // If memory is full, shift the old information
    if *iupdat > m {
        // Shift old information in SS and SY matrices
        for j in 1..(*col as usize) {
            // Copy SS(j+1,2:j+1) to SS(j,1:j)
            for k in 0..j {
                ss[k + (j-1) * (m as usize)] = ss[(k+1) + j * (m as usize)];
            }
            
            // Copy SY(j+1:col,j+1) to SY(j:col-1,j)
            for k in j..(*col as usize) {
                sy[k-1 + (j-1) * (m as usize)] = sy[k + j * (m as usize)];
            }
        }
    }

    // Compute new elements of SY and SS for the current update
    let mut pointr = *head as usize;
    for j in 1..(*col as usize) {
        let j_idx = j - 1;
        
        // Compute SY(col,j) = d'*wy_j
        sy[(*col as usize - 1) + j_idx * (m as usize)] = ddot(n as i32, d, 1, &wy[((pointr-1) * (wy_dim1 as usize)) as usize..], 1);
        ss[j_idx + (*col as usize - 1) * (m as usize)] = ddot(n as i32, &ws[((pointr-1) * (wy_dim1 as usize)) as usize..], 1, d, 1);
        
        // Move to next column in circular queue
        pointr = pointr % (m as usize) + 1;
    }

    // Compute the diagonal element of SS
    if stp == 1.0 {
        ss[(col_zero + col_zero * ss_dim1) as usize] = dtd;
    } else {
        ss[(col_zero + col_zero * ss_dim1) as usize] = stp * stp * dtd;
    }

    // Store the gradient difference dot product
    sy[(col_zero + col_zero * sy_dim1) as usize] = dr;

    0 // Success return code
}

pub fn projgr(
    n: usize,
    l: &[f64],
    u: &[f64],
    nbd: &[i32],
    x: &[f64],
    g: &[f64],
) -> f64 {
    let mut sbgnrm = 0.0f64;
    
    for i in 0..n {
        let mut gi = g[i];
        
        if nbd[i] != 0 {
            if gi < 0.0 {
                if nbd[i] >= 2 {
                    gi = f64::max(x[i] - u[i], gi);
                }
            } else if nbd[i] <= 2 {
                gi = f64::min(x[i] - l[i], gi);
            }
        }
        
        sbgnrm = f64::max(sbgnrm, gi.abs());
    }
    
    sbgnrm
}

// Computes the Newton direction in the subspace
pub fn compute_newton_direction(
    nsub: usize,
    ind: &[i32],
    d: &mut [f64],
    ws: &[f64],
    wy: &[f64],
    wv: &mut [f64],
    wn: &[f64],
    theta: f64,
    col: usize,
    head: usize,
    m: usize,
    n: usize,
) -> Result<(), i32> {
    let m2 = 2 * m;
    let col2 = 2 * col;
    let mut pointr = head;
    
    // Compute wv = W^T d
    for i in 1..=col {
        let mut temp1 = 0.0f64;
        let mut temp2 = 0.0f64;
        
        for j in 1..=nsub {
            let k = ind[j-1] as usize;
            temp1 += wy[(k-1) + (pointr-1) * n] * d[j-1];
            temp2 += ws[(k-1) + (pointr-1) * n] * d[j-1];
        }
        
        wv[i-1] = temp1;
        wv[(col + i)-1] = theta * temp2;
        pointr = pointr % m + 1;
    }
    
    // Create a mutable copy of wn for dtrsl
    let mut wn_copy = vec![0.0; wn.len()];
    wn_copy.copy_from_slice(wn);
    
    // Use the safe dtrsl function from linpack.rs
    // First solve: wn * wv = -wv
    match dtrsl(
        &mut wn_copy,
        m2,
        col2,
        &mut wv[0..col2],
        C_11 // job parameter for lower triangular, no transpose
    ) {
        Ok(_) => {},
        Err(_) => return Err(-1),
    }
    
    // Negate the solution
    for i in 0..col {
        wv[i] = -wv[i];
    }
    
    // Second solve
    match dtrsl(
        &mut wn_copy,
        m2,
        col2,
        &mut wv[0..col2],
        C_1 // job parameter for lower triangular, transpose
    ) {
        Ok(_) => {},
        Err(_) => return Err(-1),
    }
    
    // Update the direction d
    pointr = head;
    for jy in 1..=col {
        let js = col + jy;
        for i in 1..=nsub {
            let k = ind[i-1] as usize;
            d[i-1] = d[i-1] + 
                wy[(k-1) + (pointr-1) * n] * wv[jy-1] / theta +
                ws[(k-1) + (pointr-1) * n] * wv[js-1];
        }
        pointr = pointr % m + 1;
    }
    
    // Scale the direction
    let scale = 1.0 / theta;
    for i in 0..nsub {
        d[i] *= scale;
    }
    
    Ok(())
}

// Projects the solution onto the feasible region defined by bounds
fn project_onto_feasible_region(
    nsub: usize,
    ind: &[i32],
    d: &[f64],
    x: &mut [f64],
    xp: &mut [f64],
    l: &[f64],
    u: &[f64],
    nbd: &[i32],
    n: usize,
) -> bool {
    // Copy current x to xp (backup)
    xp[0..n].copy_from_slice(&x[0..n]);
    
    let mut iword = 0;
    
    // Project each component
    for i in 1..=nsub {
        let k = ind[i-1] as usize;
        let dk = d[i-1];
        let xk = x[k-1];
        
        if nbd[k-1] != 0 {
            if nbd[k-1] == 1 {
                // Lower bound only
                x[k-1] = f64::max(l[k-1], xk + dk);
                if x[k-1] == l[k-1] {
                    iword = 1;
                }
            } else if nbd[k-1] == 2 {
                // Both bounds
                let xk_new = f64::max(l[k-1], xk + dk);
                x[k-1] = f64::min(u[k-1], xk_new);
                if x[k-1] == l[k-1] || x[k-1] == u[k-1] {
                    iword = 1;
                }
            } else if nbd[k-1] == 3 {
                // Upper bound only
                x[k-1] = f64::min(u[k-1], xk + dk);
                if x[k-1] == u[k-1] {
                    iword = 1;
                }
            }
        } else {
            // No bounds
            x[k-1] = xk + dk;
        }
    }
    
    iword != 0
}

// Performs backtracking if the projected point is not acceptable
fn backtrack_if_needed(
    nsub: usize,
    ind: &[i32],
    d: &mut [f64],
    x: &mut [f64],
    xp: &[f64],
    xx: &[f64],
    gg: &[f64],
    l: &[f64],
    u: &[f64],
    nbd: &[i32],
    n: usize,
) {
    // Check if we need to backtrack
    let mut dd_p = 0.0f64;
    for i in 0..n {
        dd_p += (x[i] - xx[i]) * gg[i];
    }
    
    if dd_p > 0.0f64 {
        // Restore x from backup
        x[0..n].copy_from_slice(&xp[0..n]);
        
        // Compute the maximum step size
        let mut alpha = 1.0f64;
        let mut ibd = 0;
        
        for i in 1..=nsub {
            let k = ind[i-1] as usize;
            let dk = d[i-1];
            
            if nbd[k-1] != 0 {
                if dk < 0.0f64 && nbd[k-1] <= 2 {
                    // Moving toward lower bound
                    let temp2 = l[k-1] - x[k-1];
                    if temp2 >= 0.0f64 {
                        alpha = 0.0f64;
                    } else if dk * alpha < temp2 {
                        alpha = temp2 / dk;
                        ibd = i;
                    }
                } else if dk > 0.0f64 && nbd[k-1] >= 2 {
                    // Moving toward upper bound
                    let temp2 = u[k-1] - x[k-1];
                    if temp2 <= 0.0f64 {
                        alpha = 0.0f64;
                    } else if dk * alpha > temp2 {
                        alpha = temp2 / dk;
                        ibd = i;
                    }
                }
            }
        }
        
        // Apply the step with the computed alpha
        if alpha < 1.0f64 && ibd > 0 {
            let dk = d[ibd-1];
            let k = ind[ibd-1] as usize;
            
            if dk > 0.0f64 {
                x[k-1] = u[k-1];
                d[ibd-1] = 0.0f64;
            } else if dk < 0.0f64 {
                x[k-1] = l[k-1];
                d[ibd-1] = 0.0f64;
            }
        }
        
        // Update x with the scaled direction
        for i in 1..=nsub {
            let k = ind[i-1] as usize;
            x[k-1] += alpha * d[i-1];
        }
    }
}

pub fn subsm(
    n: usize,
    m: usize,
    nsub: usize,
    ind: &[i32],
    l: &[f64],
    u: &[f64],
    nbd: &[i32],
    d: &mut [f64],
    x: &mut [f64],
    xp: &mut [f64],
    xx: &[f64],
    gg: &[f64],
    ws: &[f64],
    wy: &[f64],
    theta: f64,
    col: usize,
    head: usize,
    wv: &mut [f64],
    wn: &[f64],
) -> i32 {
    debug!("----------------- enter SUBSM --------------");

    let mut iword = 0;

    match compute_newton_direction(
        nsub,
        ind,
        d,
        ws,
        wy,
        wv,
        wn,
        theta,
        col,
        head,
        m,
        n,
    ) {
        Ok(_) => {},
        Err(err_code) => {
            debug!("compute_newton_direction failed with error code: {}", err_code);
            debug!("----------------- exit SUBSM (early) --------------");
            return 0;
        }
    }

    // Project onto feasible region
    let hit_boundary = project_onto_feasible_region(
        nsub,
        ind,
        d,
        x,
        xp,
        l,
        u,
        nbd,
        n,
    );

    if hit_boundary {
        iword = 1;
        // Backtrack if needed
        backtrack_if_needed(
            nsub,
            ind,
            d,
            x,
            xp,
            xx,
            gg,
            l,
            u,
            nbd,
            n,
        );
    }

    debug!("----------------- exit SUBSM --------------");

    return iword;
}