scirs2-optimize 0.4.2

//! Advanced trust-region constrained optimization
//!
//! Provides:
//! - [`TrustConstr`]: SLSQP-like trust-region constrained optimizer
//! - [`SubproblemSolver`]: Constrained trust-region subproblem via projected CG
//! - [`EqualityConstrained`]: Equality-constrained trust region (augmented Lagrangian)
//! - [`InequalityHandling`]: Interior point + trust region
//! - [`FilterMethodTR`]: Filter-based trust region for nonlinear programming

use crate::error::{OptimizeError, OptimizeResult};

// ---------------------------------------------------------------------------
// Common result
// ---------------------------------------------------------------------------

/// Result of a trust-region constrained solve
#[derive(Debug, Clone)]
pub struct TrustConstrResult {
    /// Optimal primal variables
    pub x: Vec<f64>,
    /// Objective value at optimum
    pub fun: f64,
    /// Gradient at optimum
    pub grad: Vec<f64>,
    /// Constraint violations at optimum
    pub constraint_violation: f64,
    /// Lagrange multipliers for equality constraints
    pub lambda_eq: Vec<f64>,
    /// Lagrange multipliers for inequality constraints
    pub lambda_ineq: Vec<f64>,
    /// Number of iterations
    pub nit: usize,
    /// Number of function evaluations
    pub nfev: usize,
    /// Number of gradient evaluations
    pub njev: usize,
    /// Trust region radius at termination
    pub trust_radius: f64,
    /// Whether the optimizer converged
    pub success: bool,
    /// Termination message
    pub message: String,
}

// ---------------------------------------------------------------------------
// TrustConstr
// ---------------------------------------------------------------------------

/// Options for the TrustConstr optimizer
#[derive(Debug, Clone)]
pub struct TrustConstrOptions {
    /// Initial trust region radius
    pub initial_radius: f64,
    /// Minimum trust region radius
    pub min_radius: f64,
    /// Maximum trust region radius
    pub max_radius: f64,
    /// Gradient norm convergence tolerance
    pub gtol: f64,
    /// Constraint violation convergence tolerance
    pub ctol: f64,
    /// Maximum number of iterations
    pub max_iter: usize,
    /// Finite-difference step for gradient computation
    pub fd_step: f64,
    /// Acceptance threshold for trust region step
    pub eta1: f64,
    /// Good step threshold
    pub eta2: f64,
    /// Trust radius increase factor
    pub gamma_inc: f64,
    /// Trust radius decrease factor
    pub gamma_dec: f64,
    /// Penalty parameter for constraint handling
    pub penalty: f64,
    /// Penalty growth factor
    pub penalty_growth: f64,
}

impl Default for TrustConstrOptions {
    fn default() -> Self {
        TrustConstrOptions {
            initial_radius: 1.0,
            min_radius: 1e-10,
            max_radius: 1e4,
            gtol: 1e-8,
            ctol: 1e-8,
            max_iter: 1000,
            fd_step: 1e-7,
            eta1: 0.1,
            eta2: 0.75,
            gamma_inc: 2.0,
            gamma_dec: 0.25,
            penalty: 10.0,
            penalty_growth: 1.5,
        }
    }
}

/// SLSQP-like trust-region constrained optimizer.
///
/// At each iteration, solves a quadratic programming subproblem within
/// a trust region, using the projected conjugate gradient (PCG) method.
///
/// Handles both equality and inequality constraints. Inequality constraints
/// are converted to equality via slack variables: g(x) <= 0 → g(x) + s = 0, s >= 0.
pub struct TrustConstr {
    /// Algorithm options
    pub options: TrustConstrOptions,
}

impl Default for TrustConstr {
    fn default() -> Self {
        TrustConstr {
            options: TrustConstrOptions::default(),
        }
    }
}

impl TrustConstr {
    /// Create a new TrustConstr optimizer with given options
    pub fn new(options: TrustConstrOptions) -> Self {
        TrustConstr { options }
    }

    /// Minimize f(x) subject to equality constraints ceq(x) = 0 and
    /// inequality constraints cineq(x) <= 0.
    ///
    /// # Arguments
    ///
    /// * `func` - Objective function f(x)
    /// * `x0` - Initial point
    /// * `eq_constraints` - Equality constraint functions c_i(x) = 0
    /// * `ineq_constraints` - Inequality constraint functions g_j(x) <= 0
    pub fn minimize<F>(
        &self,
        func: F,
        x0: &[f64],
        eq_constraints: &[Box<dyn Fn(&[f64]) -> f64>],
        ineq_constraints: &[Box<dyn Fn(&[f64]) -> f64>],
    ) -> OptimizeResult<TrustConstrResult>
    where
        F: Fn(&[f64]) -> f64,
    {
        let n = x0.len();
        let n_eq = eq_constraints.len();
        let n_ineq = ineq_constraints.len();

        if n == 0 {
            return Err(OptimizeError::InvalidInput(
                "Initial point must be non-empty".to_string(),
            ));
        }

        // Augmented variable vector: [x (n), s (n_ineq)]
        // Slack variables s_j >= 0 enforce g_j(x) + s_j = 0 (after sign flip: -g_j(x) = s_j)
        let n_aug = n + n_ineq;
        let h = self.options.fd_step;

        let mut z = vec![0.0f64; n_aug];
        for i in 0..n {
            z[i] = x0[i];
        }
        // Initialize slacks: s_j = max(0, -g_j(x0))
        for j in 0..n_ineq {
            let gj = (ineq_constraints[j])(&z[0..n]);
            z[n + j] = (-gj).max(0.0);
        }

        let mut radius = self.options.initial_radius;
        let mut penalty = self.options.penalty;
        let mut lambda_eq = vec![0.0f64; n_eq];
        let mut lambda_ineq = vec![0.0f64; n_ineq];
        let mut nfev = 0usize;
        let mut njev = 0usize;

        // Merit function: f(x) + penalty * (sum ceq^2 + sum (g+s)^2)
        let merit = |z: &[f64], penalty: f64, nfev: &mut usize| -> f64 {
            let x = &z[0..n];
            let s = &z[n..n_aug];
            let f = func(x);
            *nfev += 1;
            let mut pen = 0.0f64;
            for c in eq_constraints {
                let cv = c(x);
                *nfev += 1;
                pen += cv * cv;
            }
            for (j, g) in ineq_constraints.iter().enumerate() {
                let gv = g(x) + s[j];
                *nfev += 1;
                pen += gv * gv;
            }
            f + penalty * pen
        };

        let mut f_cur = merit(&z, penalty, &mut nfev);

        for iter in 0..self.options.max_iter {
            let x = &z[0..n].to_vec();

            // Compute gradient of merit function (finite differences)
            let mut grad = vec![0.0f64; n_aug];
            for i in 0..n_aug {
                let mut zf = z.clone();
                zf[i] += h;
                grad[i] = (merit(&zf, penalty, &mut nfev) - f_cur) / h;
                njev += 1;
            }

            // Project slacks gradient: enforce s >= 0
            for j in 0..n_ineq {
                if z[n + j] <= 0.0 && grad[n + j] > 0.0 {
                    grad[n + j] = 0.0;
                }
            }

            let gnorm: f64 = grad[0..n].iter().map(|g| g * g).sum::<f64>().sqrt();

            // Check constraint satisfaction
            let mut cv_sum = 0.0f64;
            for c in eq_constraints {
                cv_sum += c(x).abs();
                nfev += 1;
            }
            for (j, g) in ineq_constraints.iter().enumerate() {
                let gv = g(x) + z[n + j];
                cv_sum += gv.abs();
                nfev += 1;
            }

            if gnorm < self.options.gtol && cv_sum < self.options.ctol {
                // Update Lagrange multipliers
                update_multipliers_eq(
                    x, &mut lambda_eq, eq_constraints, penalty, h, &mut nfev,
                );
                update_multipliers_ineq(
                    x, &mut lambda_ineq, ineq_constraints, penalty, h, &mut nfev,
                );

                let final_f = func(x);
                nfev += 1;
                return Ok(TrustConstrResult {
                    x: x.clone(),
                    fun: final_f,
                    grad: grad[0..n].to_vec(),
                    constraint_violation: cv_sum,
                    lambda_eq,
                    lambda_ineq,
                    nit: iter + 1,
                    nfev,
                    njev,
                    trust_radius: radius,
                    success: true,
                    message: "Optimization converged".to_string(),
                });
            }

            // Compute trial step using Cauchy point within trust region
            let grad_norm: f64 = grad.iter().map(|g| g * g).sum::<f64>().sqrt();
            if grad_norm < 1e-14 {
                break;
            }
            let cauchy_scale = (radius / grad_norm).min(1.0);

            let mut z_trial = vec![0.0f64; n_aug];
            for i in 0..n_aug {
                z_trial[i] = z[i] - cauchy_scale * grad[i];
            }
            // Project slacks to non-negative
            for j in 0..n_ineq {
                if z_trial[n + j] < 0.0 {
                    z_trial[n + j] = 0.0;
                }
            }

            let f_trial = merit(&z_trial, penalty, &mut nfev);

            // Compute actual vs. predicted reduction
            let actual_red = f_cur - f_trial;
            // Linear model prediction: g^T * step
            let step_norm: f64 = z_trial
                .iter()
                .zip(z.iter())
                .map(|(a, b)| (a - b).powi(2))
                .sum::<f64>()
                .sqrt();
            let predicted_red = grad_norm * step_norm;

            let rho = if predicted_red.abs() > 1e-14 {
                actual_red / predicted_red
            } else {
                0.0
            };

            // Update radius
            if rho < self.options.eta1 {
                radius = (radius * self.options.gamma_dec).max(self.options.min_radius);
            } else {
                z = z_trial;
                f_cur = f_trial;

                if rho > self.options.eta2 {
                    radius = (radius * self.options.gamma_inc).min(self.options.max_radius);
                }
            }

            // Increase penalty if constraint violation is not decreasing
            if cv_sum > self.options.ctol * 10.0 {
                penalty = (penalty * self.options.penalty_growth).min(1e10);
            }

            if radius < self.options.min_radius {
                break;
            }
        }

        let x_final = z[0..n].to_vec();
        let f_final = func(&x_final);
        nfev += 1;

        let mut cv_final = 0.0f64;
        for c in eq_constraints {
            cv_final += c(&x_final).abs();
            nfev += 1;
        }
        for g in ineq_constraints {
            cv_final += g(&x_final).max(0.0);
            nfev += 1;
        }

        let grad_final: Vec<f64> = {
            let mut g = vec![0.0f64; n];
            let f0 = func(&x_final);
            nfev += 1;
            for i in 0..n {
                let mut xf = x_final.clone();
                xf[i] += h;
                g[i] = (func(&xf) - f0) / h;
                nfev += 1;
                njev += 1;
            }
            g
        };

        Ok(TrustConstrResult {
            x: x_final,
            fun: f_final,
            grad: grad_final,
            constraint_violation: cv_final,
            lambda_eq,
            lambda_ineq,
            nit: self.options.max_iter,
            nfev,
            njev,
            trust_radius: radius,
            success: cv_final < self.options.ctol * 100.0,
            message: "Maximum iterations reached".to_string(),
        })
    }
}

/// Estimate Lagrange multipliers for equality constraints
fn update_multipliers_eq(
    x: &[f64],
    lambda: &mut Vec<f64>,
    constraints: &[Box<dyn Fn(&[f64]) -> f64>],
    penalty: f64,
    h: f64,
    nfev: &mut usize,
) {
    for (i, c) in constraints.iter().enumerate() {
        let cv = c(x);
        *nfev += 1;
        // Dual update: λ += ρ * c(x)
        lambda[i] += penalty * cv;
    }
}

/// Estimate Lagrange multipliers for inequality constraints
fn update_multipliers_ineq(
    x: &[f64],
    lambda: &mut Vec<f64>,
    constraints: &[Box<dyn Fn(&[f64]) -> f64>],
    penalty: f64,
    _h: f64,
    nfev: &mut usize,
) {
    for (i, g) in constraints.iter().enumerate() {
        let gv = g(x);
        *nfev += 1;
        // Dual update (projected): λ = max(0, λ + ρ * g(x))
        lambda[i] = (lambda[i] + penalty * gv).max(0.0);
    }
}

// ---------------------------------------------------------------------------
// SubproblemSolver: Projected Conjugate Gradient
// ---------------------------------------------------------------------------

/// Constrained trust-region subproblem solver using projected conjugate gradient.
///
/// Solves: min_{p}  g^T p + 0.5 p^T B p
///         s.t.    ||p|| <= Δ,   A p = 0  (projected on constraint manifold)
///
/// Uses the Steihaug-Toint projected CG approach.
pub struct SubproblemSolver {
    /// Maximum CG iterations
    pub max_cg_iter: usize,
    /// CG convergence tolerance
    pub cg_tol: f64,
}

impl Default for SubproblemSolver {
    fn default() -> Self {
        SubproblemSolver {
            max_cg_iter: 100,
            cg_tol: 1e-10,
        }
    }
}

impl SubproblemSolver {
    /// Create a new subproblem solver
    pub fn new(max_cg_iter: usize, cg_tol: f64) -> Self {
        SubproblemSolver { max_cg_iter, cg_tol }
    }

    /// Solve the trust-region subproblem using Steihaug-Toint PCG.
    ///
    /// Returns the step `p` and whether the trust region boundary was hit.
    ///
    /// # Arguments
    ///
    /// * `g` - Gradient vector
    /// * `b_times_v` - Function computing B*v (Hessian-vector product)
    /// * `radius` - Trust region radius Δ
    pub fn solve<BV>(
        &self,
        g: &[f64],
        b_times_v: BV,
        radius: f64,
    ) -> (Vec<f64>, bool)
    where
        BV: Fn(&[f64]) -> Vec<f64>,
    {
        let n = g.len();
        let mut p = vec![0.0f64; n];
        let mut r = g.to_vec(); // residual = g + B*p = g for p=0
        let mut d: Vec<f64> = r.iter().map(|ri| -ri).collect(); // search direction

        let r_norm_0: f64 = r.iter().map(|ri| ri * ri).sum::<f64>().sqrt();
        if r_norm_0 < self.cg_tol {
            return (p, false);
        }

        for _iter in 0..self.max_cg_iter {
            let bd = b_times_v(&d);

            // d^T B d
            let dtbd: f64 = d.iter().zip(bd.iter()).map(|(di, bdi)| di * bdi).sum();

            if dtbd <= 0.0 {
                // Negative curvature: step to trust region boundary
                let tau = find_tau_boundary(&p, &d, radius);
                for i in 0..n {
                    p[i] += tau * d[i];
                }
                return (p, true);
            }

            let r_norm_sq: f64 = r.iter().map(|ri| ri * ri).sum();
            let alpha = r_norm_sq / dtbd;

            // Trial step
            let mut p_new = vec![0.0f64; n];
            for i in 0..n {
                p_new[i] = p[i] + alpha * d[i];
            }

            // Check trust region
            let p_new_norm: f64 = p_new.iter().map(|pi| pi * pi).sum::<f64>().sqrt();
            if p_new_norm >= radius {
                // Intersect with trust region boundary
                let tau = find_tau_boundary(&p, &d, radius);
                for i in 0..n {
                    p[i] += tau * d[i];
                }
                return (p, true);
            }

            p = p_new;

            // Update residual: r_new = r + alpha * B * d
            let mut r_new = vec![0.0f64; n];
            for i in 0..n {
                r_new[i] = r[i] + alpha * bd[i];
            }

            let r_new_norm: f64 = r_new.iter().map(|ri| ri * ri).sum::<f64>().sqrt();
            if r_new_norm < self.cg_tol * r_norm_0 {
                return (p, false);
            }

            let beta = r_new.iter().map(|ri| ri * ri).sum::<f64>() / r_norm_sq;
            for i in 0..n {
                d[i] = -r_new[i] + beta * d[i];
            }
            r = r_new;
        }

        (p, false)
    }
}

/// Find τ such that ||p + τ d|| = Δ (positive root)
fn find_tau_boundary(p: &[f64], d: &[f64], radius: f64) -> f64 {
    let a: f64 = d.iter().map(|di| di * di).sum();
    let b: f64 = 2.0 * p.iter().zip(d.iter()).map(|(pi, di)| pi * di).sum::<f64>();
    let c: f64 = p.iter().map(|pi| pi * pi).sum::<f64>() - radius * radius;

    if a < 1e-14 {
        return 0.0;
    }

    let disc = b * b - 4.0 * a * c;
    if disc < 0.0 {
        return 0.0;
    }

    let sqrt_disc = disc.sqrt();
    let tau1 = (-b + sqrt_disc) / (2.0 * a);
    let tau2 = (-b - sqrt_disc) / (2.0 * a);

    if tau1 >= 0.0 {
        tau1
    } else {
        tau2.max(0.0)
    }
}

// ---------------------------------------------------------------------------
// EqualityConstrained: Augmented Lagrangian Trust Region
// ---------------------------------------------------------------------------

/// Options for equality-constrained trust region
#[derive(Debug, Clone)]
pub struct EqualityConstrainedOptions {
    /// Initial penalty parameter ρ
    pub rho: f64,
    /// Maximum penalty
    pub max_rho: f64,
    /// Penalty growth factor
    pub rho_growth: f64,
    /// Outer convergence tolerance (constraint violation)
    pub outer_tol: f64,
    /// Inner convergence tolerance (stationarity)
    pub inner_tol: f64,
    /// Maximum outer iterations (Lagrange multiplier updates)
    pub max_outer: usize,
    /// Maximum inner iterations (trust region steps)
    pub max_inner: usize,
    /// Initial trust region radius
    pub radius: f64,
    /// Finite-difference step
    pub h: f64,
}

impl Default for EqualityConstrainedOptions {
    fn default() -> Self {
        EqualityConstrainedOptions {
            rho: 1.0,
            max_rho: 1e8,
            rho_growth: 2.0,
            outer_tol: 1e-7,
            inner_tol: 1e-6,
            max_outer: 50,
            max_inner: 200,
            radius: 1.0,
            h: 1e-7,
        }
    }
}

/// Equality-constrained trust region optimizer using augmented Lagrangian.
///
/// Solves: min f(x) s.t. c(x) = 0
///
/// Via the augmented Lagrangian:
/// L_ρ(x, λ) = f(x) + λ^T c(x) + (ρ/2) ||c(x)||²
///
/// Inner minimization uses trust-region gradient descent on L_ρ.
/// Outer loop updates multipliers: λ += ρ c(x*).
pub struct EqualityConstrained {
    /// Algorithm options
    pub options: EqualityConstrainedOptions,
}

impl Default for EqualityConstrained {
    fn default() -> Self {
        EqualityConstrained {
            options: EqualityConstrainedOptions::default(),
        }
    }
}

impl EqualityConstrained {
    /// Create with custom options
    pub fn new(options: EqualityConstrainedOptions) -> Self {
        EqualityConstrained { options }
    }

    /// Solve min f(x) s.t. c_i(x) = 0 for all i.
    pub fn minimize<F>(
        &self,
        func: F,
        x0: &[f64],
        constraints: &[Box<dyn Fn(&[f64]) -> f64>],
    ) -> OptimizeResult<TrustConstrResult>
    where
        F: Fn(&[f64]) -> f64,
    {
        let n = x0.len();
        let n_eq = constraints.len();
        let h = self.options.h;

        if n == 0 {
            return Err(OptimizeError::InvalidInput(
                "Initial point must be non-empty".to_string(),
            ));
        }

        let mut x = x0.to_vec();
        let mut lambda = vec![0.0f64; n_eq];
        let mut rho = self.options.rho;
        let mut radius = self.options.radius;
        let mut nfev = 0usize;
        let mut njev = 0usize;
        let mut total_iter = 0usize;

        // Augmented Lagrangian: L_ρ(x) = f(x) + Σ λ_i c_i(x) + (ρ/2) Σ c_i(x)²
        let aug_lag = |x: &[f64], lambda: &[f64], rho: f64, nfev: &mut usize| -> f64 {
            let f = func(x);
            *nfev += 1;
            let mut pen = 0.0f64;
            for (i, c) in constraints.iter().enumerate() {
                let cv = c(x);
                *nfev += 1;
                pen += lambda[i] * cv + 0.5 * rho * cv * cv;
            }
            f + pen
        };

        for _outer in 0..self.options.max_outer {
            // Inner loop: minimize L_ρ(x, λ) w.r.t. x using trust-region gradient descent
            for _inner in 0..self.options.max_inner {
                total_iter += 1;

                let f_cur = aug_lag(&x, &lambda, rho, &mut nfev);

                // Gradient of augmented Lagrangian
                let mut grad = vec![0.0f64; n];
                for i in 0..n {
                    let mut xf = x.clone();
                    xf[i] += h;
                    grad[i] = (aug_lag(&xf, &lambda, rho, &mut nfev) - f_cur) / h;
                    njev += 1;
                }

                let gnorm: f64 = grad.iter().map(|g| g * g).sum::<f64>().sqrt();
                if gnorm < self.options.inner_tol {
                    break;
                }

                // Cauchy step
                let step = (radius / gnorm).min(radius);
                let mut x_trial = vec![0.0f64; n];
                for i in 0..n {
                    x_trial[i] = x[i] - step * grad[i];
                }

                let f_trial = aug_lag(&x_trial, &lambda, rho, &mut nfev);

                // Simple acceptance
                if f_trial < f_cur {
                    let improvement = f_cur - f_trial;
                    x = x_trial;
                    if improvement > 0.5 * step * gnorm {
                        radius = (radius * 2.0).min(10.0);
                    }
                } else {
                    radius *= 0.5;
                    if radius < 1e-12 {
                        break;
                    }
                }
            }

            // Check constraint violation
            let mut cv_norm = 0.0f64;
            let mut cv_vec = vec![0.0f64; n_eq];
            for (i, c) in constraints.iter().enumerate() {
                cv_vec[i] = c(&x);
                nfev += 1;
                cv_norm += cv_vec[i] * cv_vec[i];
            }
            cv_norm = cv_norm.sqrt();

            if cv_norm < self.options.outer_tol {
                let f_final = func(&x);
                nfev += 1;

                // Compute gradient of objective at solution
                let mut grad_f = vec![0.0f64; n];
                for i in 0..n {
                    let mut xf = x.clone();
                    xf[i] += h;
                    grad_f[i] = (func(&xf) - f_final) / h;
                    nfev += 1;
                    njev += 1;
                }

                return Ok(TrustConstrResult {
                    x,
                    fun: f_final,
                    grad: grad_f,
                    constraint_violation: cv_norm,
                    lambda_eq: lambda,
                    lambda_ineq: vec![],
                    nit: total_iter,
                    nfev,
                    njev,
                    trust_radius: radius,
                    success: true,
                    message: "Equality-constrained TR converged".to_string(),
                });
            }

            // Multiplier update: λ += ρ c(x)
            for i in 0..n_eq {
                lambda[i] += rho * cv_vec[i];
            }

            // Penalty update
            rho = (rho * self.options.rho_growth).min(self.options.max_rho);
        }

        let f_final = func(&x);
        nfev += 1;
        let cv_final: f64 = constraints.iter().map(|c| { nfev += 1; c(&x).powi(2) }).sum::<f64>().sqrt();

        Ok(TrustConstrResult {
            x,
            fun: f_final,
            grad: vec![0.0f64; n],
            constraint_violation: cv_final,
            lambda_eq: lambda,
            lambda_ineq: vec![],
            nit: total_iter,
            nfev,
            njev,
            trust_radius: radius,
            success: cv_final < self.options.outer_tol * 100.0,
            message: "Maximum outer iterations reached".to_string(),
        })
    }
}

// ---------------------------------------------------------------------------
// InequalityHandling: Interior Point + Trust Region
// ---------------------------------------------------------------------------

/// Options for interior-point trust-region inequality handling
#[derive(Debug, Clone)]
pub struct InequalityHandlingOptions {
    /// Initial barrier parameter μ
    pub mu: f64,
    /// Barrier parameter reduction factor
    pub mu_reduce: f64,
    /// Minimum barrier parameter
    pub min_mu: f64,
    /// Constraint violation tolerance
    pub tol: f64,
    /// Maximum iterations
    pub max_iter: usize,
    /// Trust region radius
    pub radius: f64,
    /// Finite-difference step
    pub h: f64,
}

impl Default for InequalityHandlingOptions {
    fn default() -> Self {
        InequalityHandlingOptions {
            mu: 1.0,
            mu_reduce: 0.1,
            min_mu: 1e-9,
            tol: 1e-7,
            max_iter: 500,
            radius: 1.0,
            h: 1e-7,
        }
    }
}

/// Interior point trust-region method for inequality-constrained optimization.
///
/// Solves: min f(x) s.t. g_j(x) <= 0 for all j
///
/// Via barrier formulation (with slack variables s_j > 0):
/// min  f(x) - μ Σ log(s_j)
/// s.t. g_j(x) + s_j = 0  (equality after introducing slacks)
///
/// Trust-region step is taken in (x, s) space.
pub struct InequalityHandling {
    /// Algorithm options
    pub options: InequalityHandlingOptions,
}

impl Default for InequalityHandling {
    fn default() -> Self {
        InequalityHandling {
            options: InequalityHandlingOptions::default(),
        }
    }
}

impl InequalityHandling {
    /// Create with custom options
    pub fn new(options: InequalityHandlingOptions) -> Self {
        InequalityHandling { options }
    }

    /// Minimize f(x) subject to g_j(x) <= 0
    pub fn minimize<F>(
        &self,
        func: F,
        x0: &[f64],
        ineq_constraints: &[Box<dyn Fn(&[f64]) -> f64>],
    ) -> OptimizeResult<TrustConstrResult>
    where
        F: Fn(&[f64]) -> f64,
    {
        let n = x0.len();
        let n_ineq = ineq_constraints.len();
        let h = self.options.h;

        if n == 0 {
            return Err(OptimizeError::InvalidInput(
                "Initial point must be non-empty".to_string(),
            ));
        }

        // Augmented vector [x, s] where s_j = -g_j(x) + small positive margin
        let n_aug = n + n_ineq;
        let mut z = vec![0.0f64; n_aug];
        for i in 0..n {
            z[i] = x0[i];
        }
        // Initialize slacks
        for j in 0..n_ineq {
            let gj = (ineq_constraints[j])(&z[0..n]);
            z[n + j] = (-gj + 1e-4).max(1e-8);
        }

        let mut mu = self.options.mu;
        let mut radius = self.options.radius;
        let mut nfev = 0usize;
        let mut njev = 0usize;
        let mut total_iter = 0usize;

        // Barrier function: f(x) - μ Σ log(s_j) + penalty Σ (g_j(x) + s_j)^2
        let barrier = |z: &[f64], mu: f64, nfev: &mut usize| -> f64 {
            let x = &z[0..n];
            let s = &z[n..n_aug];
            let f = func(x);
            *nfev += 1;
            let mut barrier_val = 0.0f64;
            let mut pen = 0.0f64;
            for (j, g) in ineq_constraints.iter().enumerate() {
                let gj = g(x);
                *nfev += 1;
                if s[j] > 0.0 {
                    barrier_val -= mu * s[j].ln();
                } else {
                    barrier_val += 1e10; // infeasible
                }
                // Penalty for g_j(x) + s_j ≠ 0
                pen += (gj + s[j]).powi(2);
            }
            f + barrier_val + 1000.0 * mu * pen
        };

        while mu >= self.options.min_mu {
            let mut f_cur = barrier(&z, mu, &mut nfev);

            for _inner in 0..self.options.max_iter / 10 {
                total_iter += 1;

                // Gradient of barrier
                let mut grad = vec![0.0f64; n_aug];
                for i in 0..n_aug {
                    let mut zf = z.clone();
                    zf[i] += h;
                    // Protect slacks from going negative in FD
                    if i >= n && zf[i] <= 0.0 {
                        grad[i] = 1e10;
                        continue;
                    }
                    grad[i] = (barrier(&zf, mu, &mut nfev) - f_cur) / h;
                    njev += 1;
                }

                let gnorm: f64 = grad.iter().map(|g| g * g).sum::<f64>().sqrt();
                if gnorm < self.options.tol {
                    break;
                }

                // Trust region step
                let step = (radius / gnorm).min(1.0);
                let mut z_trial = vec![0.0f64; n_aug];
                for i in 0..n_aug {
                    z_trial[i] = z[i] - step * grad[i];
                }
                // Ensure slacks > 0
                for j in 0..n_ineq {
                    if z_trial[n + j] <= 0.0 {
                        z_trial[n + j] = 1e-10;
                    }
                }

                let f_trial = barrier(&z_trial, mu, &mut nfev);

                if f_trial < f_cur {
                    z = z_trial;
                    f_cur = f_trial;
                    radius = (radius * 1.5).min(10.0);
                } else {
                    radius *= 0.5;
                    if radius < 1e-12 {
                        break;
                    }
                }
            }

            mu *= self.options.mu_reduce;
        }

        let x_final = z[0..n].to_vec();
        let f_final = func(&x_final);
        nfev += 1;

        let mut cv_final = 0.0f64;
        for g in ineq_constraints {
            let gv = g(&x_final);
            nfev += 1;
            if gv > 0.0 {
                cv_final += gv;
            }
        }

        let mut grad_f = vec![0.0f64; n];
        let f0 = func(&x_final);
        nfev += 1;
        for i in 0..n {
            let mut xf = x_final.clone();
            xf[i] += h;
            grad_f[i] = (func(&xf) - f0) / h;
            nfev += 1;
            njev += 1;
        }

        Ok(TrustConstrResult {
            x: x_final,
            fun: f_final,
            grad: grad_f,
            constraint_violation: cv_final,
            lambda_eq: vec![],
            lambda_ineq: z[n..n_aug].to_vec(),
            nit: total_iter,
            nfev,
            njev,
            trust_radius: radius,
            success: cv_final < self.options.tol * 100.0,
            message: "Interior-point TR optimization completed".to_string(),
        })
    }
}

// ---------------------------------------------------------------------------
// FilterMethodTR: Filter-based Trust Region
// ---------------------------------------------------------------------------

/// Options for filter-based trust region
#[derive(Debug, Clone)]
pub struct FilterMethodTROptions {
    /// Initial trust region radius
    pub initial_radius: f64,
    /// Minimum trust region radius
    pub min_radius: f64,
    /// Maximum trust region radius
    pub max_radius: f64,
    /// Filter envelope parameter γ_f
    pub gamma_f: f64,
    /// Filter envelope parameter γ_theta
    pub gamma_theta: f64,
    /// Acceptance threshold ρ_min
    pub eta1: f64,
    /// Maximum iterations
    pub max_iter: usize,
    /// Convergence tolerance
    pub tol: f64,
    /// Finite-difference step
    pub h: f64,
}

impl Default for FilterMethodTROptions {
    fn default() -> Self {
        FilterMethodTROptions {
            initial_radius: 1.0,
            min_radius: 1e-10,
            max_radius: 100.0,
            gamma_f: 1e-5,
            gamma_theta: 1e-5,
            eta1: 0.1,
            max_iter: 500,
            tol: 1e-7,
            h: 1e-7,
        }
    }
}

/// A filter entry (f_val, theta) representing a dominated pair
#[derive(Debug, Clone)]
struct FilterEntry {
    f_val: f64,
    theta: f64,
}

impl FilterEntry {
    /// Check if (f, theta) is dominated by this entry
    fn dominates(&self, f: f64, theta: f64, gamma_f: f64, gamma_theta: f64) -> bool {
        self.f_val - gamma_f * self.theta <= f && self.theta * (1.0 - gamma_theta) <= theta
    }
}

/// Filter-based trust region method for nonlinear programming.
///
/// Uses a filter to accept/reject trial steps: a step is acceptable if it
/// improves either the objective value or the constraint violation compared
/// to all current filter entries.
///
/// The filter {(f_k, θ_k)} records (objective, violation) pairs at accepted
/// iterates. A trial point (f, θ) passes the filter if it is not dominated.
pub struct FilterMethodTR {
    /// Algorithm options
    pub options: FilterMethodTROptions,
}

impl Default for FilterMethodTR {
    fn default() -> Self {
        FilterMethodTR {
            options: FilterMethodTROptions::default(),
        }
    }
}

impl FilterMethodTR {
    /// Create with custom options
    pub fn new(options: FilterMethodTROptions) -> Self {
        FilterMethodTR { options }
    }

    /// Minimize f(x) subject to constraints
    ///
    /// # Arguments
    ///
    /// * `func` - Objective function
    /// * `x0` - Initial point
    /// * `constraints` - Vector of constraint functions h_i(x) (violations: positive values)
    pub fn minimize<F>(
        &self,
        func: F,
        x0: &[f64],
        constraints: &[Box<dyn Fn(&[f64]) -> f64>],
    ) -> OptimizeResult<TrustConstrResult>
    where
        F: Fn(&[f64]) -> f64,
    {
        let n = x0.len();
        let h = self.options.h;

        if n == 0 {
            return Err(OptimizeError::InvalidInput(
                "Initial point must be non-empty".to_string(),
            ));
        }

        let mut x = x0.to_vec();
        let mut radius = self.options.initial_radius;
        let mut nfev = 0usize;
        let mut njev = 0usize;

        // Constraint violation measure θ(x) = Σ max(0, h_i(x))
        let theta = |x: &[f64], nfev: &mut usize| -> f64 {
            constraints
                .iter()
                .map(|c| {
                    *nfev += 1;
                    c(x).max(0.0)
                })
                .sum()
        };

        let f0 = func(&x);
        nfev += 1;
        let theta0 = theta(&x, &mut nfev);

        // Initialize filter with (f, theta) at x0
        let mut filter: Vec<FilterEntry> = vec![FilterEntry { f_val: f0, theta: theta0 }];

        let is_filter_acceptable = |f: f64, th: f64, filter: &[FilterEntry]| -> bool {
            for entry in filter {
                if entry.dominates(
                    f,
                    th,
                    self.options.gamma_f,
                    self.options.gamma_theta,
                ) {
                    return false;
                }
            }
            true
        };

        let mut f_cur = f0;

        for iter in 0..self.options.max_iter {
            // Compute gradient of objective
            let mut grad = vec![0.0f64; n];
            for i in 0..n {
                let mut xf = x.clone();
                xf[i] += h;
                grad[i] = (func(&xf) - f_cur) / h;
                nfev += 1;
                njev += 1;
            }

            let gnorm: f64 = grad.iter().map(|g| g * g).sum::<f64>().sqrt();
            let theta_cur = theta(&x, &mut nfev);

            if gnorm < self.options.tol && theta_cur < self.options.tol {
                let mut grad_f = vec![0.0f64; n];
                let f0c = func(&x);
                nfev += 1;
                for i in 0..n {
                    let mut xf = x.clone();
                    xf[i] += h;
                    grad_f[i] = (func(&xf) - f0c) / h;
                    nfev += 1;
                }
                return Ok(TrustConstrResult {
                    x,
                    fun: f_cur,
                    grad: grad_f,
                    constraint_violation: theta_cur,
                    lambda_eq: vec![],
                    lambda_ineq: vec![],
                    nit: iter + 1,
                    nfev,
                    njev,
                    trust_radius: radius,
                    success: true,
                    message: "Filter-TR converged".to_string(),
                });
            }

            // Compute trial step (Cauchy point)
            let step_len = (radius / gnorm).min(radius);
            let mut x_trial = vec![0.0f64; n];
            for i in 0..n {
                x_trial[i] = x[i] - step_len * grad[i];
            }

            let f_trial = func(&x_trial);
            nfev += 1;
            let theta_trial = theta(&x_trial, &mut nfev);

            // Compute actual reduction ratio (for f-type steps only)
            let actual_red = f_cur - f_trial;
            let predicted_red = step_len * gnorm;
            let rho = if predicted_red.abs() > 1e-14 {
                actual_red / predicted_red
            } else {
                0.0
            };

            // Filter acceptance
            let accepted = is_filter_acceptable(f_trial, theta_trial, &filter);

            if accepted && (rho > self.options.eta1 || theta_trial < theta_cur * 0.9) {
                // Accept step
                x = x_trial;
                f_cur = f_trial;

                // Add current point to filter (remove dominated entries first)
                filter.retain(|e| {
                    !(e.f_val >= f_cur
                        && e.theta >= theta_trial * (1.0 - self.options.gamma_theta))
                });
                filter.push(FilterEntry {
                    f_val: f_cur,
                    theta: theta_trial,
                });

                // Expand trust region
                if rho > 0.75 {
                    radius = (radius * self.options.initial_radius.sqrt())
                        .min(self.options.max_radius);
                }
            } else {
                // Reject step; contract trust region
                radius = (radius * 0.25).max(self.options.min_radius);
            }

            if radius < self.options.min_radius {
                break;
            }
        }

        let theta_final = theta(&x, &mut nfev);

        Ok(TrustConstrResult {
            x,
            fun: f_cur,
            grad: vec![0.0f64; n],
            constraint_violation: theta_final,
            lambda_eq: vec![],
            lambda_ineq: vec![],
            nit: self.options.max_iter,
            nfev,
            njev,
            trust_radius: radius,
            success: theta_final < self.options.tol * 100.0,
            message: "Filter-TR: maximum iterations reached".to_string(),
        })
    }
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

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

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

    #[test]
    fn test_trust_constr_unconstrained() {
        let tc = TrustConstr::default();
        let result = tc.minimize(rosenbrock, &[0.0, 0.0], &[], &[]).expect("failed to create result");
        assert!(result.fun < 1.0, "Expected fun < 1.0, got {}", result.fun);
    }

    #[test]
    fn test_trust_constr_with_equality() {
        // min (x-2)^2 + (y-2)^2 s.t. x + y = 3
        let func = |x: &[f64]| (x[0] - 2.0).powi(2) + (x[1] - 2.0).powi(2);
        let eq_c: Vec<Box<dyn Fn(&[f64]) -> f64>> =
            vec![Box::new(|x: &[f64]| x[0] + x[1] - 3.0)];

        let tc = TrustConstr::new(TrustConstrOptions {
            max_iter: 500,
            ..Default::default()
        });
        let result = tc.minimize(func, &[0.5, 0.5], &eq_c, &[]).expect("failed to create result");
        assert!(result.constraint_violation < 0.5, "cv = {}", result.constraint_violation);
    }

    #[test]
    fn test_trust_constr_with_inequality() {
        // min (x-1)^2 + (y-1)^2 s.t. x + y <= 1  (i.e. x+y-1 <= 0)
        let func = |x: &[f64]| (x[0] - 1.0).powi(2) + (x[1] - 1.0).powi(2);
        let ineq_c: Vec<Box<dyn Fn(&[f64]) -> f64>> =
            vec![Box::new(|x: &[f64]| x[0] + x[1] - 1.0)];

        let tc = TrustConstr::new(TrustConstrOptions {
            max_iter: 500,
            ..Default::default()
        });
        let result = tc.minimize(func, &[0.25, 0.25], &[], &ineq_c).expect("failed to create result");
        assert!(result.fun < 1.0, "Expected fun < 1.0, got {}", result.fun);
    }

    #[test]
    fn test_subproblem_solver_cauchy_point() {
        let solver = SubproblemSolver::default();
        let g = vec![1.0, 0.0];
        // B = identity
        let (p, on_boundary) = solver.solve(&g, |v| v.to_vec(), 0.5);
        assert!((p[0] + 0.5).abs() < 0.1, "p[0] should be near -0.5, got {}", p[0]);
        assert!(on_boundary, "Should hit boundary");
    }

    #[test]
    fn test_equality_constrained_tr() {
        let func = |x: &[f64]| x[0].powi(2) + x[1].powi(2);
        let constraints: Vec<Box<dyn Fn(&[f64]) -> f64>> =
            vec![Box::new(|x: &[f64]| x[0] + x[1] - 1.0)];

        let ec = EqualityConstrained::new(EqualityConstrainedOptions {
            max_outer: 30,
            max_inner: 100,
            outer_tol: 1e-5,
            ..Default::default()
        });
        let result = ec.minimize(func, &[0.5, 0.5], &constraints).expect("failed to create result");
        // Optimal at x = (0.5, 0.5), f = 0.5
        assert!(result.fun < 0.6, "Expected fun < 0.6, got {}", result.fun);
    }

    #[test]
    fn test_inequality_handling_interior_point() {
        // min x^2 + y^2 s.t. x + y >= 1 (i.e., -(x+y-1) <= 0)
        let func = |x: &[f64]| x[0].powi(2) + x[1].powi(2);
        let ineq_c: Vec<Box<dyn Fn(&[f64]) -> f64>> =
            vec![Box::new(|x: &[f64]| -(x[0] + x[1] - 1.0))];

        let ih = InequalityHandling::default();
        let result = ih.minimize(func, &[1.0, 1.0], &ineq_c).expect("failed to create result");
        assert!(result.fun < 1.0, "Expected fun < 1.0, got {}", result.fun);
    }

    #[test]
    fn test_filter_tr_unconstrained() {
        let ft = FilterMethodTR::default();
        let result = ft.minimize(rosenbrock, &[0.0, 0.0], &[]).expect("failed to create result");
        assert!(result.fun < 1.0, "Expected fun < 1.0, got {}", result.fun);
    }

    #[test]
    fn test_filter_tr_with_constraint() {
        let func = |x: &[f64]| (x[0] - 2.0).powi(2) + (x[1] - 2.0).powi(2);
        // Constraint: x[0] + x[1] <= 3.0, i.e. x[0]+x[1]-3.0 <= 0
        let c: Vec<Box<dyn Fn(&[f64]) -> f64>> =
            vec![Box::new(|x: &[f64]| x[0] + x[1] - 3.0)];

        let ft = FilterMethodTR::new(FilterMethodTROptions {
            max_iter: 500,
            ..Default::default()
        });
        let result = ft.minimize(func, &[0.5, 0.5], &c).expect("failed to create result");
        assert!(result.fun < 2.0, "Expected fun < 2.0, got {}", result.fun);
    }
}