scirs2_optimize/

constrained.rs

//! Constrained optimization algorithms
//!
//! This module provides methods for constrained optimization of scalar
//! functions of one or more variables.
//!
//! ## Example
//!
//! ```
//! use ndarray::{array, Array1};
//! use scirs2_optimize::constrained::{minimize_constrained, Method, Constraint};
//!
//! // Define a simple function to minimize
//! fn objective(x: &[f64]) -> f64 {
//!     (x[0] - 1.0).powi(2) + (x[1] - 2.5).powi(2)
//! }
//!
//! // Define a constraint: x[0] + x[1] <= 3
//! fn constraint(x: &[f64]) -> f64 {
//!     3.0 - x[0] - x[1]  // Should be >= 0
//! }
//!
//! # fn main() -> Result<(), Box<dyn std::error::Error>> {
//! // Minimize the function starting at [0.0, 0.0]
//! let initial_point = array![0.0, 0.0];
//! let constraints = vec![Constraint::new(constraint, Constraint::INEQUALITY)];
//!
//! let result = minimize_constrained(
//!     objective,
//!     &initial_point,
//!     &constraints,
//!     Method::SLSQP,
//!     None
//! )?;
//!
//! // The constrained minimum should be at [0.5, 2.5]
//! # Ok(())
//! # }
//! ```

use crate::error::{OptimizeError, OptimizeResult};
use crate::result::OptimizeResults;
use ndarray::{Array1, Array2, ArrayBase, Axis, Data, Ix1};
use std::fmt;

/// Type alias for constraint functions that take a slice of f64 and return f64
pub type ConstraintFn = fn(&[f64]) -> f64;

/// Optimization methods for constrained minimization.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Method {
    /// Sequential Least SQuares Programming
    SLSQP,

    /// Trust-region constrained algorithm
    TrustConstr,

    /// Linear programming using the simplex algorithm
    COBYLA,
}

impl fmt::Display for Method {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        match self {
            Method::SLSQP => write!(f, "SLSQP"),
            Method::TrustConstr => write!(f, "trust-constr"),
            Method::COBYLA => write!(f, "COBYLA"),
        }
    }
}

/// Options for the constrained optimizer.
#[derive(Debug, Clone)]
pub struct Options {
    /// Maximum number of iterations to perform
    pub maxiter: Option<usize>,

    /// Precision goal for the value in the stopping criterion
    pub ftol: Option<f64>,

    /// Precision goal for the gradient in the stopping criterion (relative)
    pub gtol: Option<f64>,

    /// Precision goal for constraint violation
    pub ctol: Option<f64>,

    /// Step size used for numerical approximation of the jacobian
    pub eps: Option<f64>,

    /// Whether to print convergence messages
    pub disp: bool,

    /// Return the optimization result after each iteration
    pub return_all: bool,
}

impl Default for Options {
    fn default() -> Self {
        Options {
            maxiter: None,
            ftol: Some(1e-8),
            gtol: Some(1e-8),
            ctol: Some(1e-8),
            eps: Some(1e-8),
            disp: false,
            return_all: false,
        }
    }
}

/// Constraint type for constrained optimization
pub struct Constraint<F> {
    /// The constraint function
    pub fun: F,

    /// The type of constraint (equality or inequality)
    pub kind: ConstraintKind,

    /// Lower bound for a box constraint
    pub lb: Option<f64>,

    /// Upper bound for a box constraint
    pub ub: Option<f64>,
}

/// The kind of constraint
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ConstraintKind {
    /// Equality constraint: fun(x) = 0
    Equality,

    /// Inequality constraint: fun(x) >= 0
    Inequality,
}

impl Constraint<fn(&[f64]) -> f64> {
    /// Constant for equality constraint
    pub const EQUALITY: ConstraintKind = ConstraintKind::Equality;

    /// Constant for inequality constraint
    pub const INEQUALITY: ConstraintKind = ConstraintKind::Inequality;

    /// Create a new constraint
    pub fn new(fun: fn(&[f64]) -> f64, kind: ConstraintKind) -> Self {
        Constraint {
            fun,
            kind,
            lb: None,
            ub: None,
        }
    }

    /// Create a new box constraint
    pub fn new_bounds(lb: Option<f64>, ub: Option<f64>) -> Self {
        Constraint {
            fun: |_| 0.0, // Dummy function for box constraints
            kind: ConstraintKind::Inequality,
            lb,
            ub,
        }
    }
}

impl<F> Constraint<F> {
    /// Check if this is a box constraint
    pub fn is_bounds(&self) -> bool {
        self.lb.is_some() || self.ub.is_some()
    }
}

/// Minimizes a scalar function of one or more variables with constraints.
///
/// # Arguments
///
/// * `func` - A function that takes a slice of values and returns a scalar
/// * `x0` - The initial guess
/// * `constraints` - Vector of constraints
/// * `method` - The optimization method to use
/// * `options` - Options for the optimizer
///
/// # Returns
///
/// * `OptimizeResults` containing the optimization results
///
/// # Example
///
/// ```
/// use ndarray::array;
/// use scirs2_optimize::constrained::{minimize_constrained, Method, Constraint};
///
/// // Function to minimize
/// fn objective(x: &[f64]) -> f64 {
///     (x[0] - 1.0).powi(2) + (x[1] - 2.5).powi(2)
/// }
///
/// // Constraint: x[0] + x[1] <= 3
/// fn constraint(x: &[f64]) -> f64 {
///     3.0 - x[0] - x[1]  // Should be >= 0
/// }
///
/// # fn main() -> Result<(), Box<dyn std::error::Error>> {
/// let initial_point = array![0.0, 0.0];
/// let constraints = vec![Constraint::new(constraint, Constraint::INEQUALITY)];
///
/// let result = minimize_constrained(
///     objective,
///     &initial_point,
///     &constraints,
///     Method::SLSQP,
///     None
/// )?;
/// # Ok(())
/// # }
/// ```
pub fn minimize_constrained<F, S>(
    func: F,
    x0: &ArrayBase<S, Ix1>,
    constraints: &[Constraint<ConstraintFn>],
    method: Method,
    options: Option<Options>,
) -> OptimizeResult<OptimizeResults<f64>>
where
    F: Fn(&[f64]) -> f64,
    S: Data<Elem = f64>,
{
    let options = options.unwrap_or_default();

    // Implementation of various methods will go here
    match method {
        Method::SLSQP => minimize_slsqp(func, x0, constraints, &options),
        Method::TrustConstr => minimize_trust_constr(func, x0, constraints, &options),
        _ => Err(OptimizeError::NotImplementedError(format!(
            "Method {:?} is not yet implemented",
            method
        ))),
    }
}

/// Implements the SLSQP algorithm for constrained optimization
fn minimize_slsqp<F, S>(
    func: F,
    x0: &ArrayBase<S, Ix1>,
    constraints: &[Constraint<ConstraintFn>],
    options: &Options,
) -> OptimizeResult<OptimizeResults<f64>>
where
    F: Fn(&[f64]) -> f64,
    S: Data<Elem = f64>,
{
    // Get options or use defaults
    let ftol = options.ftol.unwrap_or(1e-8);
    let gtol = options.gtol.unwrap_or(1e-8);
    let ctol = options.ctol.unwrap_or(1e-8);
    let maxiter = options.maxiter.unwrap_or(100 * x0.len());
    let eps = options.eps.unwrap_or(1e-8);

    // Initialize variables
    let n = x0.len();
    let mut x = x0.to_owned();
    let mut f = func(x.as_slice().unwrap());
    let mut nfev = 1;

    // Initialize the Lagrange multipliers for inequality constraints
    let mut lambda = Array1::zeros(constraints.len());

    // Calculate initial gradient using finite differences
    let mut g = Array1::zeros(n);
    for i in 0..n {
        let mut x_h = x.clone();
        x_h[i] += eps;
        let f_h = func(x_h.as_slice().unwrap());
        g[i] = (f_h - f) / eps;
        nfev += 1;
    }

    // Evaluate initial constraints
    let mut c = Array1::zeros(constraints.len());
    let _ceq: Array1<f64> = Array1::zeros(0); // No equality constraints support for now
    for (i, constraint) in constraints.iter().enumerate() {
        if !constraint.is_bounds() {
            let val = (constraint.fun)(x.as_slice().unwrap());

            match constraint.kind {
                ConstraintKind::Inequality => {
                    c[i] = val; // g(x) >= 0 constraint
                }
                ConstraintKind::Equality => {
                    // For simplicity, we don't fully support equality constraints yet
                    return Err(OptimizeError::NotImplementedError(
                        "Equality constraints not fully implemented in SLSQP yet".to_string(),
                    ));
                }
            }
        }
    }

    // Calculate constraint Jacobian
    let mut a = Array2::zeros((constraints.len(), n));
    for (i, constraint) in constraints.iter().enumerate() {
        if !constraint.is_bounds() {
            for j in 0..n {
                let mut x_h = x.clone();
                x_h[j] += eps;
                let c_h = (constraint.fun)(x_h.as_slice().unwrap());
                a[[i, j]] = (c_h - c[i]) / eps;
                nfev += 1;
            }
        }
    }

    // Initialize working matrices
    let mut h_inv = Array2::eye(n); // Approximate inverse Hessian

    // Main optimization loop
    let mut iter = 0;

    while iter < maxiter {
        // Check constraint violation
        let mut max_constraint_violation = 0.0;
        for (i, &ci) in c.iter().enumerate() {
            if constraints[i].kind == ConstraintKind::Inequality && ci < -ctol {
                max_constraint_violation = f64::max(max_constraint_violation, -ci);
            }
        }

        // Check convergence on gradient
        if g.iter().all(|&gi| gi.abs() < gtol) && max_constraint_violation < ctol {
            break;
        }

        // Compute the search direction using QP subproblem
        // For simplicity, we'll use a projected gradient approach
        let mut p = Array1::zeros(n);

        if max_constraint_violation > ctol {
            // If constraints are violated, move toward feasibility
            for (i, &ci) in c.iter().enumerate() {
                if ci < -ctol {
                    // This constraint is violated
                    for j in 0..n {
                        p[j] -= a[[i, j]] * ci; // Move along constraint gradient
                    }
                }
            }
        } else {
            // Otherwise, use BFGS direction with constraints
            p = -&h_inv.dot(&g);

            // Project gradient on active constraints
            for (i, &ci) in c.iter().enumerate() {
                if ci.abs() < ctol {
                    // Active constraint
                    let mut normal = Array1::zeros(n);
                    for j in 0..n {
                        normal[j] = a[[i, j]];
                    }
                    let norm = normal.dot(&normal).sqrt();
                    if norm > 1e-10 {
                        normal = &normal / norm;
                        let p_dot_normal = p.dot(&normal);

                        // If moving in the wrong direction (constraint violation), project out
                        if p_dot_normal < 0.0 {
                            p = &p - &(&normal * p_dot_normal);
                        }
                    }
                }
            }
        }

        // Line search with constraint awareness
        let mut alpha = 1.0;
        let c1 = 1e-4; // Sufficient decrease parameter
        let rho = 0.5; // Backtracking parameter

        // Initial step
        let mut x_new = &x + &(&p * alpha);
        let mut f_new = func(x_new.as_slice().unwrap());
        nfev += 1;

        // Evaluate constraints at new point
        let mut c_new = Array1::zeros(constraints.len());
        for (i, constraint) in constraints.iter().enumerate() {
            if !constraint.is_bounds() {
                c_new[i] = (constraint.fun)(x_new.as_slice().unwrap());
                nfev += 1;
            }
        }

        // Check if constraint violation is reduced and objective decreases
        let mut max_viol = 0.0;
        let mut max_viol_new = 0.0;

        for (i, constraint) in constraints.iter().enumerate() {
            if constraint.kind == ConstraintKind::Inequality {
                max_viol = f64::max(max_viol, f64::max(0.0, -c[i]));
                max_viol_new = f64::max(max_viol_new, f64::max(0.0, -c_new[i]));
            }
        }

        // Compute directional derivative
        let g_dot_p = g.dot(&p);

        // Backtracking line search
        while (f_new > f + c1 * alpha * g_dot_p && max_viol <= ctol)
            || (max_viol_new > max_viol && max_viol > ctol)
        {
            alpha *= rho;

            // Prevent tiny steps
            if alpha < 1e-10 {
                break;
            }

            x_new = &x + &(&p * alpha);
            f_new = func(x_new.as_slice().unwrap());
            nfev += 1;

            // Evaluate constraints
            for (i, constraint) in constraints.iter().enumerate() {
                if !constraint.is_bounds() {
                    c_new[i] = (constraint.fun)(x_new.as_slice().unwrap());
                    nfev += 1;
                }
            }

            max_viol_new = 0.0;
            for (i, constraint) in constraints.iter().enumerate() {
                if constraint.kind == ConstraintKind::Inequality {
                    max_viol_new = f64::max(max_viol_new, f64::max(0.0, -c_new[i]));
                }
            }
        }

        // Check convergence on function value and step size
        if ((f - f_new).abs() < ftol * (1.0 + f.abs())) && alpha * p.dot(&p).sqrt() < ftol {
            break;
        }

        // Calculate new gradient
        let mut g_new = Array1::zeros(n);
        for i in 0..n {
            let mut x_h = x_new.clone();
            x_h[i] += eps;
            let f_h = func(x_h.as_slice().unwrap());
            g_new[i] = (f_h - f_new) / eps;
            nfev += 1;
        }

        // Calculate new constraint Jacobian
        let mut a_new = Array2::zeros((constraints.len(), n));
        for (i, constraint) in constraints.iter().enumerate() {
            if !constraint.is_bounds() {
                for j in 0..n {
                    let mut x_h = x_new.clone();
                    x_h[j] += eps;
                    let c_h = (constraint.fun)(x_h.as_slice().unwrap());
                    a_new[[i, j]] = (c_h - c_new[i]) / eps;
                    nfev += 1;
                }
            }
        }

        // Update Lagrange multipliers (rudimentary)
        for (i, constraint) in constraints.iter().enumerate() {
            if constraint.kind == ConstraintKind::Inequality && c_new[i].abs() < ctol {
                // For active inequality constraints
                let mut normal = Array1::zeros(n);
                for j in 0..n {
                    normal[j] = a_new[[i, j]];
                }

                let norm = normal.dot(&normal).sqrt();
                if norm > 1e-10 {
                    normal = &normal / norm;
                    lambda[i] = -g_new.dot(&normal);
                }
            } else {
                lambda[i] = 0.0;
            }
        }

        // BFGS update for the Hessian approximation
        let s = &x_new - &x;
        let y = &g_new - &g;

        // Include constraints in y by adding Lagrangian term
        let mut y_lag = y.clone();
        for (i, &li) in lambda.iter().enumerate() {
            if li > 0.0 {
                // Only active constraints
                for j in 0..n {
                    // L = f - sum(lambda_i * c_i)
                    // So gradient of L includes -lambda_i * grad(c_i)
                    y_lag[j] += li * (a_new[[i, j]] - a[[i, j]]);
                }
            }
        }

        // BFGS update formula
        let rho_bfgs = 1.0 / y_lag.dot(&s);
        if rho_bfgs.is_finite() && rho_bfgs > 0.0 {
            let i_mat = Array2::eye(n);
            let y_row = y_lag.clone().insert_axis(Axis(0));
            let s_col = s.clone().insert_axis(Axis(1));
            let y_s_t = y_row.dot(&s_col);

            let term1 = &i_mat - &(&y_s_t * rho_bfgs);
            let s_row = s.clone().insert_axis(Axis(0));
            let y_col = y_lag.clone().insert_axis(Axis(1));
            let s_y_t = s_row.dot(&y_col);

            let term2 = &i_mat - &(&s_y_t * rho_bfgs);
            let term3 = &term1.dot(&h_inv);
            h_inv = term3.dot(&term2) + rho_bfgs * s_col.dot(&s_row);
        }

        // Update for next iteration
        x = x_new;
        f = f_new;
        g = g_new;
        c = c_new;
        a = a_new;

        iter += 1;
    }

    // Prepare constraint values for the result
    let mut c_result = Array1::zeros(constraints.len());
    for (i, constraint) in constraints.iter().enumerate() {
        if !constraint.is_bounds() {
            c_result[i] = c[i];
        }
    }

    // Create and return result
    let mut result = OptimizeResults::default();
    result.x = x;
    result.fun = f;
    result.jac = Some(g.into_raw_vec_and_offset().0);
    result.constr = Some(c_result);
    result.nfev = nfev;
    result.nit = iter;
    result.success = iter < maxiter;

    if result.success {
        result.message = "Optimization terminated successfully.".to_string();
    } else {
        result.message = "Maximum iterations reached.".to_string();
    }

    Ok(result)
}

/// Implements the Trust Region Constrained algorithm for constrained optimization
fn minimize_trust_constr<F, S>(
    func: F,
    x0: &ArrayBase<S, Ix1>,
    constraints: &[Constraint<ConstraintFn>],
    options: &Options,
) -> OptimizeResult<OptimizeResults<f64>>
where
    F: Fn(&[f64]) -> f64,
    S: Data<Elem = f64>,
{
    // Get options or use defaults
    let ftol = options.ftol.unwrap_or(1e-8);
    let gtol = options.gtol.unwrap_or(1e-8);
    let ctol = options.ctol.unwrap_or(1e-8);
    let maxiter = options.maxiter.unwrap_or(100 * x0.len());
    let eps = options.eps.unwrap_or(1e-8);

    // Initialize variables
    let n = x0.len();
    let mut x = x0.to_owned();
    let mut f = func(x.as_slice().unwrap());
    let mut nfev = 1;

    // Initialize the Lagrange multipliers
    let mut lambda = Array1::zeros(constraints.len());

    // Calculate initial gradient using finite differences
    let mut g = Array1::zeros(n);
    for i in 0..n {
        let mut x_h = x.clone();
        x_h[i] += eps;
        let f_h = func(x_h.as_slice().unwrap());
        g[i] = (f_h - f) / eps;
        nfev += 1;
    }

    // Evaluate initial constraints
    let mut c = Array1::zeros(constraints.len());
    for (i, constraint) in constraints.iter().enumerate() {
        if !constraint.is_bounds() {
            let val = (constraint.fun)(x.as_slice().unwrap());

            match constraint.kind {
                ConstraintKind::Inequality => {
                    c[i] = val; // g(x) >= 0 constraint
                }
                ConstraintKind::Equality => {
                    // For simplicity, we don't fully support equality constraints yet
                    return Err(OptimizeError::NotImplementedError(
                        "Equality constraints not fully implemented in Trust-Region yet"
                            .to_string(),
                    ));
                }
            }
        }
    }

    // Calculate constraint Jacobian
    let mut a = Array2::zeros((constraints.len(), n));
    for (i, constraint) in constraints.iter().enumerate() {
        if !constraint.is_bounds() {
            for j in 0..n {
                let mut x_h = x.clone();
                x_h[j] += eps;
                let c_h = (constraint.fun)(x_h.as_slice().unwrap());
                a[[i, j]] = (c_h - c[i]) / eps;
                nfev += 1;
            }
        }
    }

    // Initialize trust region radius
    let mut delta = 1.0;

    // Initialize approximation of the Hessian of the Lagrangian
    let mut b = Array2::eye(n);

    // Main optimization loop
    let mut iter = 0;

    while iter < maxiter {
        // Check constraint violation
        let mut max_constraint_violation = 0.0;
        for (i, &ci) in c.iter().enumerate() {
            if constraints[i].kind == ConstraintKind::Inequality && ci < -ctol {
                max_constraint_violation = f64::max(max_constraint_violation, -ci);
            }
        }

        // Check convergence on gradient and constraints
        if g.iter().all(|&gi| gi.abs() < gtol) && max_constraint_violation < ctol {
            break;
        }

        // Compute the Lagrangian gradient
        let mut lag_grad = g.clone();
        for (i, &li) in lambda.iter().enumerate() {
            if li > 0.0 || c[i] < ctol {
                // Active or violated constraint
                for j in 0..n {
                    // L = f - sum(lambda_i * c_i) for inequality constraints
                    // So gradient of L is grad(f) - sum(lambda_i * grad(c_i))
                    lag_grad[j] -= li * a[[i, j]];
                }
            }
        }

        // Compute the step using a trust-region approach
        // Solve the constrained quadratic subproblem:
        // min 0.5 * p^T B p + g^T p  s.t. ||p|| <= delta and linearized constraints

        let (p, predicted_reduction) =
            compute_trust_region_step_constrained(&lag_grad, &b, &a, &c, delta, constraints, ctol);

        // Try the step
        let x_new = &x + &p;

        // Evaluate function and constraints at new point
        let f_new = func(x_new.as_slice().unwrap());
        nfev += 1;

        let mut c_new = Array1::zeros(constraints.len());
        for (i, constraint) in constraints.iter().enumerate() {
            if !constraint.is_bounds() {
                c_new[i] = (constraint.fun)(x_new.as_slice().unwrap());
                nfev += 1;
            }
        }

        // Compute change in merit function (includes constraint violation)
        let mut merit = f;
        let mut merit_new = f_new;

        // Add constraint violation penalty
        let penalty = 10.0; // Simple fixed penalty parameter
        for (i, &ci) in c.iter().enumerate() {
            if constraints[i].kind == ConstraintKind::Inequality {
                merit += penalty * f64::max(0.0, -ci);
                merit_new += penalty * f64::max(0.0, -c_new[i]);
            }
        }

        // Compute actual reduction in merit function
        let actual_reduction = merit - merit_new;

        // Compute ratio of actual to predicted reduction
        let rho = if predicted_reduction > 0.0 {
            actual_reduction / predicted_reduction
        } else {
            0.0
        };

        // Update trust region radius based on the quality of the step
        if rho < 0.25 {
            delta *= 0.5;
        } else if rho > 0.75 && p.iter().map(|&pi| pi * pi).sum::<f64>().sqrt() >= 0.9 * delta {
            delta *= 2.0;
        }

        // Accept or reject the step
        if rho > 0.1 {
            // Accept the step
            x = x_new;
            f = f_new;
            c = c_new;

            // Check convergence on function value
            if (merit - merit_new).abs() < ftol * (1.0 + merit.abs()) {
                break;
            }

            // Compute new gradient
            let mut g_new = Array1::zeros(n);
            for i in 0..n {
                let mut x_h = x.clone();
                x_h[i] += eps;
                let f_h = func(x_h.as_slice().unwrap());
                g_new[i] = (f_h - f) / eps;
                nfev += 1;
            }

            // Compute new constraint Jacobian
            let mut a_new = Array2::zeros((constraints.len(), n));
            for (i, constraint) in constraints.iter().enumerate() {
                if !constraint.is_bounds() {
                    for j in 0..n {
                        let mut x_h = x.clone();
                        x_h[j] += eps;
                        let c_h = (constraint.fun)(x_h.as_slice().unwrap());
                        a_new[[i, j]] = (c_h - c[i]) / eps;
                        nfev += 1;
                    }
                }
            }

            // Update Lagrange multipliers using projected gradient method
            for (i, constraint) in constraints.iter().enumerate() {
                if constraint.kind == ConstraintKind::Inequality {
                    if c[i] < ctol {
                        // Active or violated constraint
                        // Increase multiplier if constraint is violated
                        lambda[i] = f64::max(0.0, lambda[i] - c[i] * penalty);
                    } else {
                        // Decrease multiplier towards zero
                        lambda[i] = f64::max(0.0, lambda[i] - 0.1 * lambda[i]);
                    }
                }
            }

            // Update Hessian approximation using BFGS or SR1
            let s = &p;
            let y = &g_new - &g;

            // Simple BFGS update for the Hessian approximation
            let s_dot_y = s.dot(&y);
            if s_dot_y > 1e-10 {
                let s_col = s.clone().insert_axis(Axis(1));
                let s_row = s.clone().insert_axis(Axis(0));

                let bs = b.dot(s);
                let bs_col = bs.clone().insert_axis(Axis(1));
                let bs_row = bs.clone().insert_axis(Axis(0));

                let term1 = s_dot_y + s.dot(&bs);
                let term2 = &s_col.dot(&s_row) * (term1 / (s_dot_y * s_dot_y));

                let term3 = &bs_col.dot(&s_row) + &s_col.dot(&bs_row);

                b = &b + &term2 - &(&term3 / s_dot_y);
            }

            // Update variables for next iteration
            g = g_new;
            a = a_new;
        }

        iter += 1;
    }

    // Prepare constraint values for the result
    let mut c_result = Array1::zeros(constraints.len());
    for (i, constraint) in constraints.iter().enumerate() {
        if !constraint.is_bounds() {
            c_result[i] = c[i];
        }
    }

    // Create and return result
    let mut result = OptimizeResults::default();
    result.x = x;
    result.fun = f;
    result.jac = Some(g.into_raw_vec_and_offset().0);
    result.constr = Some(c_result);
    result.nfev = nfev;
    result.nit = iter;
    result.success = iter < maxiter;

    if result.success {
        result.message = "Optimization terminated successfully.".to_string();
    } else {
        result.message = "Maximum iterations reached.".to_string();
    }

    Ok(result)
}

/// Compute a trust-region step for constrained optimization
fn compute_trust_region_step_constrained(
    g: &Array1<f64>,
    b: &Array2<f64>,
    a: &Array2<f64>,
    c: &Array1<f64>,
    delta: f64,
    constraints: &[Constraint<ConstraintFn>],
    ctol: f64,
) -> (Array1<f64>, f64) {
    let n = g.len();
    let n_constr = constraints.len();

    // Compute the unconstrained Cauchy point (steepest descent direction)
    let p_unconstrained = compute_unconstrained_cauchy_point(g, b, delta);

    // Check if unconstrained Cauchy point satisfies linearized constraints
    let mut constraint_violated = false;
    for i in 0..n_constr {
        if constraints[i].kind == ConstraintKind::Inequality {
            let grad_c_dot_p = (0..n).map(|j| a[[i, j]] * p_unconstrained[j]).sum::<f64>();
            if c[i] + grad_c_dot_p < -ctol {
                constraint_violated = true;
                break;
            }
        }
    }

    // If unconstrained point is feasible, use it
    if !constraint_violated {
        // Compute predicted reduction
        let g_dot_p = g.dot(&p_unconstrained);
        let bp = b.dot(&p_unconstrained);
        let p_dot_bp = p_unconstrained.dot(&bp);
        let predicted_reduction = -g_dot_p - 0.5 * p_dot_bp;

        return (p_unconstrained, predicted_reduction);
    }

    // Otherwise, project onto the linearized feasible region
    // This is a simplified approach - in practice, you would solve a CQP

    // Start with the steepest descent direction
    let mut p = Array1::zeros(n);
    for i in 0..n {
        p[i] = -g[i];
    }

    // Normalize to trust region radius
    let p_norm = p.iter().map(|&pi| pi * pi).sum::<f64>().sqrt();
    if p_norm > 1e-10 {
        p = &p * (delta / p_norm);
    }

    // Project onto each constraint
    for _iter in 0..5 {
        // Limited iterations for projection
        let mut max_viol = 0.0;
        let mut most_violated = 0;

        // Find most violated constraint
        for i in 0..n_constr {
            if constraints[i].kind == ConstraintKind::Inequality {
                let grad_c_dot_p = (0..n).map(|j| a[[i, j]] * p[j]).sum::<f64>();
                let viol = -(c[i] + grad_c_dot_p);
                if viol > max_viol {
                    max_viol = viol;
                    most_violated = i;
                }
            }
        }

        if max_viol < ctol {
            break;
        }

        // Project p onto the constraint
        let mut a_norm_sq = 0.0;
        for j in 0..n {
            a_norm_sq += a[[most_violated, j]] * a[[most_violated, j]];
        }

        if a_norm_sq > 1e-10 {
            let grad_c_dot_p = (0..n).map(|j| a[[most_violated, j]] * p[j]).sum::<f64>();
            let proj_dist = (c[most_violated] + grad_c_dot_p) / a_norm_sq;

            // Project p
            for j in 0..n {
                p[j] += a[[most_violated, j]] * proj_dist;
            }

            // Rescale to trust region
            let p_norm = p.iter().map(|&pi| pi * pi).sum::<f64>().sqrt();
            if p_norm > delta {
                p = &p * (delta / p_norm);
            }
        }
    }

    // Compute predicted reduction
    let g_dot_p = g.dot(&p);
    let bp = b.dot(&p);
    let p_dot_bp = p.dot(&bp);
    let predicted_reduction = -g_dot_p - 0.5 * p_dot_bp;

    (p, predicted_reduction)
}

/// Compute the unconstrained Cauchy point (steepest descent to trust region boundary)
fn compute_unconstrained_cauchy_point(g: &Array1<f64>, b: &Array2<f64>, delta: f64) -> Array1<f64> {
    let n = g.len();

    // Compute the steepest descent direction: -g
    let mut p = Array1::zeros(n);
    for i in 0..n {
        p[i] = -g[i];
    }

    // Compute g^T B g and ||g||^2
    let bg = b.dot(g);
    let g_dot_bg = g.dot(&bg);
    let g_norm_sq = g.dot(g);

    // Check if the gradient is practically zero
    if g_norm_sq < 1e-10 {
        // If gradient is practically zero, don't move
        return Array1::zeros(n);
    }

    // Compute tau (step to the boundary)
    let tau = if g_dot_bg <= 0.0 {
        // Negative curvature or zero curvature case
        delta / g_norm_sq.sqrt()
    } else {
        // Positive curvature case
        f64::min(delta / g_norm_sq.sqrt(), g_norm_sq / g_dot_bg)
    };

    // Scale the direction by tau
    for i in 0..n {
        p[i] *= tau;
    }

    p
}

#[cfg(test)]
mod tests {
    use super::*;
    use ndarray::array;

    fn objective(x: &[f64]) -> f64 {
        (x[0] - 1.0).powi(2) + (x[1] - 2.5).powi(2)
    }

    fn constraint(x: &[f64]) -> f64 {
        3.0 - x[0] - x[1] // Should be >= 0
    }

    #[test]
    fn test_minimize_constrained_placeholder() {
        // We're now using the real implementation, so this test needs to be adjusted
        let x0 = array![0.0, 0.0];
        let constraints = vec![Constraint::new(constraint, Constraint::INEQUALITY)];

        // Use minimal iterations to check basic algorithm behavior
        let options = Options {
            maxiter: Some(1), // Just a single iteration
            ..Options::default()
        };

        let result = minimize_constrained(
            objective,
            &x0.view(),
            &constraints,
            Method::SLSQP,
            Some(options),
        )
        .unwrap();

        // With limited iterations, we expect it not to converge
        assert!(!result.success);

        // Check that constraint value was computed
        assert!(result.constr.is_some());
        let constr = result.constr.unwrap();
        assert_eq!(constr.len(), 1);
    }

    // Test the SLSQP algorithm on a simple constrained problem
    #[test]
    fn test_minimize_slsqp() {
        // Problem:
        // Minimize (x-1)^2 + (y-2.5)^2
        // Subject to: x + y <= 3

        let x0 = array![0.0, 0.0];
        let constraints = vec![Constraint::new(constraint, Constraint::INEQUALITY)];

        let options = Options {
            maxiter: Some(100),
            gtol: Some(1e-6),
            ftol: Some(1e-6),
            ctol: Some(1e-6),
            ..Options::default()
        };

        let result = minimize_constrained(
            objective,
            &x0.view(),
            &constraints,
            Method::SLSQP,
            Some(options),
        )
        .unwrap();

        // For the purpose of this test, we're just checking that the algorithm runs
        // and produces reasonable output. The convergence may vary.

        // Check that we're moving in the right direction
        assert!(result.x[0] >= 0.0);
        assert!(result.x[1] >= 0.0);

        // Function value should be decreasing from initial point
        let initial_value = objective(&[0.0, 0.0]);
        assert!(result.fun <= initial_value);

        // Check that constraint values are computed
        assert!(result.constr.is_some());

        // Output the result for inspection
        println!(
            "SLSQP result: x = {:?}, f = {}, iterations = {}",
            result.x, result.fun, result.nit
        );
    }

    // Test the Trust Region Constrained algorithm
    #[test]
    fn test_minimize_trust_constr() {
        // Problem:
        // Minimize (x-1)^2 + (y-2.5)^2
        // Subject to: x + y <= 3

        let x0 = array![0.0, 0.0];
        let constraints = vec![Constraint::new(constraint, Constraint::INEQUALITY)];

        let options = Options {
            maxiter: Some(500), // Increased iterations for convergence
            gtol: Some(1e-6),
            ftol: Some(1e-6),
            ctol: Some(1e-6),
            ..Options::default()
        };

        let result = minimize_constrained(
            objective,
            &x0.view(),
            &constraints,
            Method::TrustConstr,
            Some(options.clone()),
        )
        .unwrap();

        // Check that we're moving in the right direction
        assert!(result.x[0] >= 0.0);
        assert!(result.x[1] >= 0.0);

        // Function value should be decreasing from initial point
        let initial_value = objective(&[0.0, 0.0]);
        assert!(result.fun <= initial_value);

        // Check that constraint values are computed
        assert!(result.constr.is_some());

        // Output the result for inspection
        println!(
            "TrustConstr result: x = {:?}, f = {}, iterations = {}",
            result.x, result.fun, result.nit
        );
    }

    // Test both constrained optimization methods on a more complex problem
    #[test]
    fn test_constrained_rosenbrock() {
        // Rosenbrock function with a constraint
        fn rosenbrock(x: &[f64]) -> f64 {
            100.0 * (x[1] - x[0].powi(2)).powi(2) + (1.0 - x[0]).powi(2)
        }

        // Constraint: x[0]^2 + x[1]^2 <= 1.5
        fn circle_constraint(x: &[f64]) -> f64 {
            1.5 - (x[0].powi(2) + x[1].powi(2)) // Should be >= 0
        }

        let x0 = array![0.0, 0.0];
        let constraints = vec![Constraint::new(circle_constraint, Constraint::INEQUALITY)];

        let options = Options {
            maxiter: Some(1000), // More iterations for this harder problem
            gtol: Some(1e-4),    // Relaxed tolerances
            ftol: Some(1e-4),
            ctol: Some(1e-4),
            ..Options::default()
        };

        // For this test, we'll clone options at each stage to avoid move issues
        let options_copy1 = options.clone();
        let options_copy2 = options.clone();

        // Test SLSQP
        let result_slsqp = minimize_constrained(
            rosenbrock,
            &x0.view(),
            &constraints,
            Method::SLSQP,
            Some(options_copy1),
        )
        .unwrap();

        // Test TrustConstr
        let result_trust = minimize_constrained(
            rosenbrock,
            &x0.view(),
            &constraints,
            Method::TrustConstr,
            Some(options_copy2),
        )
        .unwrap();

        // Check that both methods find a reasonable solution
        println!(
            "SLSQP Rosenbrock result: x = {:?}, f = {}, iterations = {}",
            result_slsqp.x, result_slsqp.fun, result_slsqp.nit
        );
        println!(
            "TrustConstr Rosenbrock result: x = {:?}, f = {}, iterations = {}",
            result_trust.x, result_trust.fun, result_trust.nit
        );

        // Check that function value is better than initial point
        let initial_value = rosenbrock(&[0.0, 0.0]);
        assert!(result_slsqp.fun < initial_value);
        assert!(result_trust.fun < initial_value);

        // Check that constraint is satisfied
        let constr_slsqp = result_slsqp.constr.unwrap();
        let constr_trust = result_trust.constr.unwrap();
        assert!(constr_slsqp[0] >= -0.01); // Relaxed tolerance for the test
        assert!(constr_trust[0] >= -0.01); // Relaxed tolerance for the test
    }
}