herculesabqp 0.1.2

use std::sync::Arc;

use anyhow::{Result, bail};

use crate::matrix::QuadraticMatrix;

use super::apgd::solve_box_qp_apgd;
use super::lipschitz::estimate_lipschitz;
use super::operator::QuadraticOperator;
use super::problem::{BoxQPProblem, FirstOrderProblem, ImplicitBoxQPProblem};
use super::types::{LipschitzMethod, ScalingMode, ScalingSummary, SolverOptions, SolverResult};
use super::utils::DenseVec;

/// Reusable prepared solver state for repeated solves with the same `Q` and `c`.
///
/// This caches the expensive matrix-side preprocessing that is invariant across
/// branch-and-bound child nodes:
/// - optional symmetric validation / storage
/// - optional Hessian-diagonal scaling matrix
/// - scaled linear term
/// - working Lipschitz estimate
///
/// Child solves can then vary only in bounds and warm start.
#[derive(Clone, Debug)]
pub struct PreparedSolver {
    original_q: Arc<QuadraticMatrix>,
    original_c: Arc<DenseVec>,
    base_options: SolverOptions,
    scale: Option<DenseVec>,
    scaled_q: Option<Arc<QuadraticMatrix>>,
    scaled_c: Option<Arc<DenseVec>>,
    working_lipschitz: f64,
    scaling_mode: ScalingMode,
    assume_symmetric: bool,
    lipschitz_value: Option<f64>,
    lipschitz_method: LipschitzMethod,
}

impl PreparedSolver {
    /// Prepare reusable matrix-side state for repeated box-QP solves.
    pub fn new(q: &QuadraticMatrix, c: &[f64], options: &SolverOptions) -> Result<Self> {
        let original_q = Arc::new(if options.assume_symmetric {
            q.clone()
        } else {
            q.symmetrized()
        });
        let original_c = Arc::new(DenseVec::from_vec(c.to_vec()));
        match options.scaling.mode {
            ScalingMode::None => {
                let working_lipschitz = options.lipschitz.value.unwrap_or_else(|| {
                    estimate_lipschitz(original_q.as_ref(), &options.lipschitz.method)
                });
                if !working_lipschitz.is_finite() || working_lipschitz <= 0.0 {
                    bail!("L must be finite and positive, got {working_lipschitz}");
                }
                Ok(Self {
                    original_q,
                    original_c,
                    base_options: base_prepared_options(options),
                    scale: None,
                    scaled_q: None,
                    scaled_c: None,
                    working_lipschitz,
                    scaling_mode: ScalingMode::None,
                    assume_symmetric: options.assume_symmetric,
                    lipschitz_value: options.lipschitz.value,
                    lipschitz_method: options.lipschitz.method.clone(),
                })
            }
            ScalingMode::HessianDiag => {
                let scale = build_diagonal_scaling(
                    original_q.as_ref(),
                    options.scaling.eps,
                    options.scaling.min,
                    options.scaling.max,
                );
                let scaled_q = Arc::new(
                    original_q.as_ref().scaled(
                        scale
                            .as_slice()
                            .expect("dense scaling vector should be contiguous"),
                    ),
                );
                let mut c_scaled = DenseVec::zeros(scale.len());
                for i in 0..scale.len() {
                    c_scaled[i] = original_c[i] * scale[i];
                }
                let scaled_c = Arc::new(c_scaled);
                let working_lipschitz = options.lipschitz.value.unwrap_or_else(|| {
                    estimate_lipschitz(scaled_q.as_ref(), &options.lipschitz.method)
                });
                if !working_lipschitz.is_finite() || working_lipschitz <= 0.0 {
                    bail!("L must be finite and positive, got {working_lipschitz}");
                }
                Ok(Self {
                    original_q,
                    original_c,
                    base_options: base_prepared_options(options),
                    scale: Some(scale),
                    scaled_q: Some(scaled_q),
                    scaled_c: Some(scaled_c),
                    working_lipschitz,
                    scaling_mode: ScalingMode::HessianDiag,
                    assume_symmetric: options.assume_symmetric,
                    lipschitz_value: options.lipschitz.value,
                    lipschitz_method: options.lipschitz.method.clone(),
                })
            }
        }
    }

    /// Solve one node using the prepared matrix-side state and node-specific bounds.
    ///
    /// Only node-local options are expected to vary across solves:
    /// bounds, warm start, stopping, polishing, and logging. Matrix-side
    /// options such as symmetry handling, scaling mode, and Lipschitz setup
    /// are fixed when the prepared solver is created. If those change, build a
    /// new [`PreparedSolver`].
    pub fn solve(&self, lb: &[f64], ub: &[f64], options: &SolverOptions) -> Result<SolverResult> {
        if lb.len() != self.original_q.n() || ub.len() != self.original_q.n() {
            bail!(
                "dimension mismatch: prepared Q has size {}, lb has len {}, ub has len {}",
                self.original_q.n(),
                lb.len(),
                ub.len()
            );
        }
        if options.scaling.mode != self.scaling_mode {
            bail!(
                "prepared solver was built with scaling mode {:?}, but solve was called with {:?}",
                self.scaling_mode,
                options.scaling.mode
            );
        }
        if options.assume_symmetric != self.assume_symmetric {
            bail!(
                "prepared solver was built with assume_symmetric={}, but solve was called with assume_symmetric={}",
                self.assume_symmetric,
                options.assume_symmetric
            );
        }
        if options.lipschitz.value != self.lipschitz_value
            || !matches_lipschitz_method(&options.lipschitz.method, &self.lipschitz_method)
        {
            bail!(
                "prepared solver was built with a different Lipschitz configuration; build a new PreparedSolver to change it"
            );
        }

        match self.scaling_mode {
            ScalingMode::None => {
                let problem = BoxQPProblem::from_shared_parts(
                    Arc::clone(&self.original_q),
                    Arc::clone(&self.original_c),
                    DenseVec::from_vec(lb.to_vec()),
                    DenseVec::from_vec(ub.to_vec()),
                )?;
                let mut solve_options = options.clone();
                solve_options.lipschitz.value = Some(self.working_lipschitz);
                solve_box_qp_apgd(
                    &problem,
                    self.original_q.as_ref(),
                    Some(&problem),
                    &solve_options,
                )
            }
            ScalingMode::HessianDiag => {
                let scale = self
                    .scale
                    .as_ref()
                    .expect("scaled solver should have scale");
                let scaled_q = Arc::clone(
                    self.scaled_q
                        .as_ref()
                        .expect("scaled solver should have scaled Q"),
                );
                let scaled_c = Arc::clone(
                    self.scaled_c
                        .as_ref()
                        .expect("scaled solver should have scaled c"),
                );

                let mut lb_scaled = DenseVec::zeros(scale.len());
                let mut ub_scaled = DenseVec::zeros(scale.len());
                for i in 0..scale.len() {
                    lb_scaled[i] = lb[i] / scale[i];
                    ub_scaled[i] = ub[i] / scale[i];
                }
                let scaled_problem =
                    BoxQPProblem::from_shared_parts(scaled_q, scaled_c, lb_scaled, ub_scaled)?;

                let mut scaled_options = options.clone();
                scaled_options.lipschitz.value = Some(self.working_lipschitz);
                if let Some(x0) = &options.x0 {
                    if x0.len() != scale.len() {
                        bail!("x0 has len {}, expected {}", x0.len(), scale.len());
                    }
                    scaled_options.x0 = Some(
                        x0.iter()
                            .zip(scale.iter())
                            .map(|(&x_i, &s_i)| x_i / s_i)
                            .collect(),
                    );
                }

                let result_z = solve_box_qp_apgd(
                    &scaled_problem,
                    self.scaled_q
                        .as_ref()
                        .expect("scaled solver should have scaled Q")
                        .as_ref(),
                    Some(&scaled_problem),
                    &scaled_options,
                )?;

                let mut x = DenseVec::zeros(scale.len());
                for i in 0..scale.len() {
                    x[i] = scale[i] * result_z.x[i];
                }
                let original_problem = BoxQPProblem::from_shared_parts(
                    Arc::clone(&self.original_q),
                    Arc::clone(&self.original_c),
                    DenseVec::from_vec(lb.to_vec()),
                    DenseVec::from_vec(ub.to_vec()),
                )?;
                original_problem.clip_in_place(&mut x);

                let mut qx = DenseVec::zeros(scale.len());
                original_problem.matvec_into(&x, &mut qx);
                let diag = original_problem.diagnostics_from_qx(
                    &qx,
                    &x,
                    options.stopping.bound_tol,
                    options.stopping.dual_certification,
                );
                let objective = original_problem.objective_from_qx(&x, &qx);

                Ok(SolverResult {
                    x: x.iter().copied().collect(),
                    objective,
                    iterations: result_z.iterations,
                    num_restarts: result_z.num_restarts,
                    quality: super::apgd::quality_summary(&diag),
                    timing: result_z.timing,
                    lipschitz: result_z.lipschitz,
                    step_size: result_z.step_size,
                    scaling: ScalingSummary {
                        applied: true,
                        name: "hessian_diag",
                        scale_min: scale.iter().copied().fold(f64::INFINITY, f64::min),
                        scale_max: scale.iter().copied().fold(f64::NEG_INFINITY, f64::max),
                    },
                })
            }
        }
    }

    /// Solve a repeated box-QP subproblem with new bounds and an optional warm start.
    ///
    /// This is the ergonomic repeated-node entry point for branch-and-bound style
    /// integrations. All non-node settings are inherited from [`PreparedSolver::new`];
    /// the only per-node inputs are the lower bounds, upper bounds, and optional
    /// warm start.
    pub fn solve_subproblem(
        &self,
        lb: &[f64],
        ub: &[f64],
        x0: Option<&[f64]>,
    ) -> Result<SolverResult> {
        let mut options = self.base_options.clone();
        options.x0 = x0.map(|x| x.to_vec());
        self.solve(lb, ub, &options)
    }
}

/// Reusable first-order-only solver state for repeated solves with a matrix-free
/// quadratic operator.
///
/// This path intentionally skips active-set polishing. It is meant for very large
/// problems where the caller can supply `Qx` products but does not want to
/// materialize reduced submatrices.
///
/// If Hessian-diagonal scaling is requested and the operator exposes
/// [`QuadraticOperator::diagonal`], the prepared solver caches a scaled implicit
/// operator. Otherwise it falls back to an unscaled solve.
#[derive(Clone)]
pub struct PreparedImplicitSolver {
    original_operator: Arc<dyn QuadraticOperator>,
    original_c: Arc<DenseVec>,
    base_options: SolverOptions,
    scale: Option<DenseVec>,
    working_operator: Arc<dyn QuadraticOperator>,
    working_c: Arc<DenseVec>,
    working_lipschitz: f64,
    requested_scaling_mode: ScalingMode,
    assume_symmetric: bool,
    lipschitz_value: Option<f64>,
    lipschitz_method: LipschitzMethod,
}

#[derive(Clone)]
struct ScaledQuadraticOperator {
    inner: Arc<dyn QuadraticOperator>,
    scale: DenseVec,
}

impl QuadraticOperator for ScaledQuadraticOperator {
    fn n(&self) -> usize {
        self.inner.n()
    }

    fn matvec_into(&self, x: &DenseVec, out: &mut DenseVec) {
        let mut scaled_x = DenseVec::zeros(self.scale.len());
        for i in 0..self.scale.len() {
            scaled_x[i] = self.scale[i] * x[i];
        }
        self.inner.matvec_into(&scaled_x, out);
        for i in 0..self.scale.len() {
            out[i] *= self.scale[i];
        }
    }

    fn diagonal(&self) -> Option<DenseVec> {
        let diag = self.inner.diagonal()?;
        let mut out = DenseVec::zeros(diag.len());
        for i in 0..diag.len() {
            out[i] = self.scale[i] * self.scale[i] * diag[i];
        }
        Some(out)
    }

    fn gershgorin_upper_bound(&self) -> Option<f64> {
        None
    }
}

impl PreparedImplicitSolver {
    /// Prepare reusable operator-side state for repeated matrix-free solves.
    ///
    /// The caller must set `assume_symmetric = true`, because the implicit path
    /// cannot build `0.5 * (Q + Q^T)` without an explicit matrix. When
    /// `scaling.mode = HessianDiag`, diagonal scaling is applied only if the
    /// operator provides a diagonal.
    pub fn new(
        operator: Arc<dyn QuadraticOperator>,
        c: &[f64],
        options: &SolverOptions,
    ) -> Result<Self> {
        if !options.assume_symmetric {
            bail!(
                "implicit operators require assume_symmetric=true because matrix-free symmetrization is not available"
            );
        }

        let original_c = Arc::new(DenseVec::from_vec(c.to_vec()));
        if original_c.len() != operator.n() {
            bail!(
                "dimension mismatch: implicit Q has size {}, c has len {}",
                operator.n(),
                original_c.len()
            );
        }
        let (scale, working_operator, working_c, requested_scaling_mode) =
            match options.scaling.mode {
                ScalingMode::None => (
                    None,
                    Arc::clone(&operator),
                    Arc::clone(&original_c),
                    ScalingMode::None,
                ),
                ScalingMode::HessianDiag => {
                    if let Some(diag) = operator.diagonal() {
                        let scale = build_diagonal_scaling_from_diag(
                            &diag,
                            options.scaling.eps,
                            options.scaling.min,
                            options.scaling.max,
                        );
                        let mut c_scaled = DenseVec::zeros(scale.len());
                        for i in 0..scale.len() {
                            c_scaled[i] = original_c[i] * scale[i];
                        }
                        let scaled_operator: Arc<dyn QuadraticOperator> =
                            Arc::new(ScaledQuadraticOperator {
                                inner: Arc::clone(&operator),
                                scale: scale.clone(),
                            });
                        (
                            Some(scale),
                            scaled_operator,
                            Arc::new(c_scaled),
                            ScalingMode::HessianDiag,
                        )
                    } else {
                        (
                            None,
                            Arc::clone(&operator),
                            Arc::clone(&original_c),
                            ScalingMode::HessianDiag,
                        )
                    }
                }
            };
        let working_lipschitz = options.lipschitz.value.unwrap_or_else(|| {
            estimate_lipschitz(working_operator.as_ref(), &options.lipschitz.method)
        });
        if !working_lipschitz.is_finite() || working_lipschitz <= 0.0 {
            bail!("L must be finite and positive, got {working_lipschitz}");
        }

        Ok(Self {
            original_operator: operator,
            original_c,
            base_options: base_prepared_options(options),
            scale,
            working_operator,
            working_c,
            working_lipschitz,
            requested_scaling_mode,
            assume_symmetric: true,
            lipschitz_value: options.lipschitz.value,
            lipschitz_method: options.lipschitz.method.clone(),
        })
    }

    /// Solve one matrix-free node with new bounds and an optional warm start.
    ///
    /// The current implicit path is first-order only, so polishing is disabled
    /// internally even if the caller leaves it enabled in `options`.
    ///
    /// Bounds, warm starts, and stopping parameters may vary across solves.
    /// Matrix-side choices such as symmetry assumption, scaling mode, and
    /// Lipschitz setup are fixed when the prepared solver is created.
    pub fn solve(&self, lb: &[f64], ub: &[f64], options: &SolverOptions) -> Result<SolverResult> {
        if lb.len() != self.original_operator.n() || ub.len() != self.original_operator.n() {
            bail!(
                "dimension mismatch: implicit Q has size {}, lb has len {}, ub has len {}",
                self.original_operator.n(),
                lb.len(),
                ub.len()
            );
        }
        if options.scaling.mode != self.requested_scaling_mode {
            bail!(
                "prepared implicit solver was built with scaling mode {:?}, but solve was called with {:?}",
                self.requested_scaling_mode,
                options.scaling.mode
            );
        }
        if options.assume_symmetric != self.assume_symmetric {
            bail!(
                "prepared implicit solver requires assume_symmetric={}, but solve was called with assume_symmetric={}",
                self.assume_symmetric,
                options.assume_symmetric
            );
        }
        if options.lipschitz.value != self.lipschitz_value
            || !matches_lipschitz_method(&options.lipschitz.method, &self.lipschitz_method)
        {
            bail!(
                "prepared implicit solver was built with a different Lipschitz configuration; build a new PreparedImplicitSolver to change it"
            );
        }

        match &self.scale {
            None => {
                let problem = ImplicitBoxQPProblem::from_shared_parts(
                    Arc::clone(&self.original_operator),
                    Arc::clone(&self.original_c),
                    DenseVec::from_vec(lb.to_vec()),
                    DenseVec::from_vec(ub.to_vec()),
                )?;
                let mut solve_options = options.clone();
                solve_options.lipschitz.value = Some(self.working_lipschitz);
                solve_options.polish.enabled = false;
                let mut result = solve_box_qp_apgd(
                    &problem,
                    self.working_operator.as_ref(),
                    None,
                    &solve_options,
                )?;
                result.scaling = ScalingSummary {
                    applied: false,
                    name: "none",
                    scale_min: 1.0,
                    scale_max: 1.0,
                };
                Ok(result)
            }
            Some(scale) => {
                let mut lb_scaled = DenseVec::zeros(scale.len());
                let mut ub_scaled = DenseVec::zeros(scale.len());
                for i in 0..scale.len() {
                    lb_scaled[i] = lb[i] / scale[i];
                    ub_scaled[i] = ub[i] / scale[i];
                }
                let scaled_problem = ImplicitBoxQPProblem::from_shared_parts(
                    Arc::clone(&self.working_operator),
                    Arc::clone(&self.working_c),
                    lb_scaled,
                    ub_scaled,
                )?;

                let mut scaled_options = options.clone();
                scaled_options.lipschitz.value = Some(self.working_lipschitz);
                scaled_options.polish.enabled = false;
                if let Some(x0) = &options.x0 {
                    if x0.len() != scale.len() {
                        bail!("x0 has len {}, expected {}", x0.len(), scale.len());
                    }
                    scaled_options.x0 = Some(
                        x0.iter()
                            .zip(scale.iter())
                            .map(|(&x_i, &s_i)| x_i / s_i)
                            .collect(),
                    );
                }

                let result_z = solve_box_qp_apgd(
                    &scaled_problem,
                    self.working_operator.as_ref(),
                    None,
                    &scaled_options,
                )?;

                let mut x = DenseVec::zeros(scale.len());
                for i in 0..scale.len() {
                    x[i] = scale[i] * result_z.x[i];
                }
                let original_problem = ImplicitBoxQPProblem::from_shared_parts(
                    Arc::clone(&self.original_operator),
                    Arc::clone(&self.original_c),
                    DenseVec::from_vec(lb.to_vec()),
                    DenseVec::from_vec(ub.to_vec()),
                )?;
                original_problem.clip_in_place(&mut x);

                let mut qx = DenseVec::zeros(scale.len());
                original_problem.matvec_into(&x, &mut qx);
                let diag = original_problem.diagnostics_from_qx(
                    &qx,
                    &x,
                    options.stopping.bound_tol,
                    options.stopping.dual_certification,
                );
                let objective = original_problem.objective_from_qx(&x, &qx);

                Ok(SolverResult {
                    x: x.iter().copied().collect(),
                    objective,
                    iterations: result_z.iterations,
                    num_restarts: result_z.num_restarts,
                    quality: super::apgd::quality_summary(&diag),
                    timing: result_z.timing,
                    lipschitz: result_z.lipschitz,
                    step_size: result_z.step_size,
                    scaling: ScalingSummary {
                        applied: true,
                        name: "hessian_diag",
                        scale_min: scale.iter().copied().fold(f64::INFINITY, f64::min),
                        scale_max: scale.iter().copied().fold(f64::NEG_INFINITY, f64::max),
                    },
                })
            }
        }
    }

    /// Solve a repeated implicit subproblem with new bounds and an optional warm start.
    ///
    /// All non-node settings are inherited from [`PreparedImplicitSolver::new`].
    pub fn solve_subproblem(
        &self,
        lb: &[f64],
        ub: &[f64],
        x0: Option<&[f64]>,
    ) -> Result<SolverResult> {
        let mut options = self.base_options.clone();
        options.x0 = x0.map(|x| x.to_vec());
        self.solve(lb, ub, &options)
    }
}

fn matches_lipschitz_method(lhs: &LipschitzMethod, rhs: &LipschitzMethod) -> bool {
    matches!(
        (lhs, rhs),
        (LipschitzMethod::Gershgorin, LipschitzMethod::Gershgorin)
            | (LipschitzMethod::Auto, LipschitzMethod::Auto)
    )
}

fn base_prepared_options(options: &SolverOptions) -> SolverOptions {
    let mut base = options.clone();
    base.x0 = None;
    base
}

/// Solve a convex box-constrained quadratic program of the form
/// `min 0.5 x^T Q x + c^T x` subject to `lb <= x <= ub`.
///
/// The public entry point validates the problem once, optionally applies
/// Hessian-diagonal scaling, runs the shared first-order solver, and then
/// maps the result back into the original coordinates.
pub fn solve_box_qp(
    q: &QuadraticMatrix,
    c: &[f64],
    lb: &[f64],
    ub: &[f64],
    options: &SolverOptions,
) -> Result<SolverResult> {
    let prepared = PreparedSolver::new(q, c, options)?;
    prepared.solve(lb, ub, options)
}

/// Solve a convex box-constrained quadratic program using a matrix-free
/// quadratic operator.
///
/// The implicit path currently supports only the first-order solve. Active-set
/// polishing is skipped, and the caller must set `assume_symmetric = true`
/// because matrix-free symmetrization is not available.
///
/// If `scaling.mode = HessianDiag` is requested and the operator supplies a
/// diagonal, the solve is performed in scaled coordinates. If the diagonal is
/// unavailable, the solver falls back to an unscaled implicit solve.
pub fn solve_box_qp_implicit(
    q: Arc<dyn QuadraticOperator>,
    c: &[f64],
    lb: &[f64],
    ub: &[f64],
    options: &SolverOptions,
) -> Result<SolverResult> {
    let prepared = PreparedImplicitSolver::new(q, c, options)?;
    prepared.solve(lb, ub, options)
}

// Build Hessian-diagonal variable scaling for the optional scaled solve.
fn build_diagonal_scaling(
    q: &QuadraticMatrix,
    eps: f64,
    scale_min: f64,
    scale_max: f64,
) -> DenseVec {
    DenseVec::from_iter(q.diagonal().iter().copied().map(|d| {
        (d.abs().max(eps))
            .sqrt()
            .recip()
            .clamp(scale_min, scale_max)
    }))
}

fn build_diagonal_scaling_from_diag(
    diag: &DenseVec,
    eps: f64,
    scale_min: f64,
    scale_max: f64,
) -> DenseVec {
    DenseVec::from_iter(diag.iter().copied().map(|d| {
        (d.abs().max(eps))
            .sqrt()
            .recip()
            .clamp(scale_min, scale_max)
    }))
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::solver::{SolverOptions, default_bounds};
    use ndarray::{Array1, Array2};

    fn diagonal_matrix(diag: &[f64]) -> Array2<f64> {
        let n = diag.len();
        let mut out = Array2::<f64>::zeros((n, n));
        for (i, &value) in diag.iter().enumerate() {
            out[[i, i]] = value;
        }
        out
    }

    fn identity_matrix(n: usize) -> Array2<f64> {
        let mut out = Array2::<f64>::zeros((n, n));
        for i in 0..n {
            out[[i, i]] = 1.0;
        }
        out
    }

    struct DiagonalOperator {
        diag: Vec<f64>,
    }

    impl QuadraticOperator for DiagonalOperator {
        fn n(&self) -> usize {
            self.diag.len()
        }

        fn matvec_into(&self, x: &Array1<f64>, out: &mut Array1<f64>) {
            for i in 0..self.diag.len() {
                out[i] = self.diag[i] * x[i];
            }
        }

        fn diagonal(&self) -> Option<Array1<f64>> {
            Some(Array1::from_vec(self.diag.clone()))
        }

        fn gershgorin_upper_bound(&self) -> Option<f64> {
            Some(self.diag.iter().copied().fold(1e-12, f64::max))
        }
    }

    struct NoDiagonalOperator {
        n: usize,
    }

    impl QuadraticOperator for NoDiagonalOperator {
        fn n(&self) -> usize {
            self.n
        }

        fn matvec_into(&self, x: &Array1<f64>, out: &mut Array1<f64>) {
            for i in 0..self.n {
                out[i] = x[i];
            }
        }
    }

    #[test]
    fn solves_simple_diagonal_box_qp() {
        let q = QuadraticMatrix::dense(diagonal_matrix(&[2.0, 2.0, 2.0]));
        let c = vec![-1.0, 0.5, -3.0];
        let (lb, ub) = default_bounds(3);
        let mut options = SolverOptions::default();
        options.polish.enabled = false;
        let result = solve_box_qp(&q, &c, &lb, &ub, &options).unwrap();
        assert!(result.quality.rel_gap < 1e-5);
        assert!(result.x[0] > 0.4 && result.x[0] < 0.6);
        assert!(result.x[1] < 1e-6);
        assert!(result.x[2] > 0.99);
    }

    #[test]
    fn solves_diagonal_box_qp_with_nonunit_bounds() {
        let q = QuadraticMatrix::dense(diagonal_matrix(&[2.0, 4.0, 1.5]));
        let c = vec![-1.0, 0.8, -0.75];
        let lb = vec![-2.0, 0.25, -1.0];
        let ub = vec![0.25, 1.25, 0.2];
        let mut options = SolverOptions::default();
        options.polish.enabled = false;
        let result = solve_box_qp(&q, &c, &lb, &ub, &options).unwrap();

        assert!(result.quality.rel_gap < 1e-5);
        assert!((result.x[0] - 0.25).abs() < 1e-6);
        assert!((result.x[1] - 0.25).abs() < 1e-6);
        assert!((result.x[2] - 0.2).abs() < 1e-6);
    }

    #[test]
    fn scaling_and_unscaled_match_on_easy_problem() {
        let q = QuadraticMatrix::dense(diagonal_matrix(&[1.0, 2.0, 3.0, 4.0]));
        let c = vec![-0.25, 0.5, -1.2, 0.8];
        let lb = vec![0.0, 0.0, 0.0, 0.0];
        let ub = vec![1.0, 1.0, 1.0, 1.0];

        let mut unscaled_options = SolverOptions::default();
        unscaled_options.scaling.mode = ScalingMode::None;
        unscaled_options.polish.enabled = false;

        let mut scaled_options = unscaled_options.clone();
        scaled_options.scaling.mode = ScalingMode::HessianDiag;

        let unscaled = solve_box_qp(&q, &c, &lb, &ub, &unscaled_options).unwrap();
        let scaled = solve_box_qp(&q, &c, &lb, &ub, &scaled_options).unwrap();

        assert!(unscaled.quality.rel_gap < 1e-8);
        assert!(scaled.quality.rel_gap < 1e-8);
        for (xu, xs) in unscaled.x.iter().zip(scaled.x.iter()) {
            assert!((xu - xs).abs() < 1e-7);
        }
        assert!((unscaled.objective - scaled.objective).abs() < 1e-9);
    }

    #[test]
    fn fixed_variables_are_respected_and_do_not_inflate_kkt() {
        let q = QuadraticMatrix::dense(diagonal_matrix(&[2.0, 3.0, 4.0]));
        let c = vec![-10.0, -0.75, 2.0];
        let lb = vec![0.3, -1.0, 0.25];
        let ub = vec![0.3, 2.0, 0.25];
        let mut options = SolverOptions::default();
        options.polish.enabled = false;

        let result = solve_box_qp(&q, &c, &lb, &ub, &options).unwrap();

        assert!(result.quality.rel_gap < 1e-8);
        assert!((result.x[0] - 0.3).abs() < 1e-12);
        assert!((result.x[2] - 0.25).abs() < 1e-12);
        assert!(result.x[1] >= lb[1] - 1e-10 && result.x[1] <= ub[1] + 1e-10);
        assert!(result.quality.kkt_inf < 1e-8);
    }

    #[test]
    fn scaling_transforms_warm_start_coordinates() {
        let q = QuadraticMatrix::dense(diagonal_matrix(&[4.0, 9.0]));
        let c = vec![-2.0, 3.0];
        let lb = vec![0.0, 0.0];
        let ub = vec![1.0, 1.0];
        let x0 = vec![0.8, 0.2];

        let mut unscaled_options = SolverOptions::default();
        unscaled_options.scaling.mode = ScalingMode::None;
        unscaled_options.polish.enabled = false;
        unscaled_options.stopping.max_iter = 0;
        unscaled_options.x0 = Some(x0.clone());

        let mut scaled_options = unscaled_options.clone();
        scaled_options.scaling.mode = ScalingMode::HessianDiag;

        let unscaled = solve_box_qp(&q, &c, &lb, &ub, &unscaled_options).unwrap();
        let scaled = solve_box_qp(&q, &c, &lb, &ub, &scaled_options).unwrap();

        assert_eq!(unscaled.x, x0);
        assert_eq!(scaled.x, x0);
    }

    #[test]
    fn rejects_nonpositive_user_supplied_l() {
        let q = QuadraticMatrix::dense(identity_matrix(2));
        let c = vec![0.0, 0.0];
        let lb = vec![0.0, 0.0];
        let ub = vec![1.0, 1.0];
        let mut options = SolverOptions::default();
        options.lipschitz.value = Some(0.0);
        let err = solve_box_qp(&q, &c, &lb, &ub, &options).unwrap_err();
        assert!(err.to_string().contains("L must be finite and positive"));
    }

    #[test]
    fn can_skip_dual_certification() {
        let q = QuadraticMatrix::dense(diagonal_matrix(&[2.0, 3.0, 4.0]));
        let c = vec![-1.0, 0.5, -2.0];
        let (lb, ub) = default_bounds(3);
        let mut options = SolverOptions::default();
        options.stopping.dual_certification = false;
        options.polish.enabled = false;

        let result = solve_box_qp(&q, &c, &lb, &ub, &options).unwrap();

        assert!(result.quality.rel_gap.is_nan());
        assert!(result.quality.gap.is_nan());
        assert!(result.quality.certified_lower_bound.is_nan());
        assert!(result.quality.kkt_inf < 1e-3);
    }

    #[test]
    fn quality_exposes_minorant_lower_bound_and_gap() {
        let q = QuadraticMatrix::dense(diagonal_matrix(&[2.0, 2.0, 2.0]));
        let c = vec![-1.0, 0.5, -3.0];
        let (lb, ub) = default_bounds(3);
        let mut options = SolverOptions::default();
        options.polish.enabled = false;

        let result = solve_box_qp(&q, &c, &lb, &ub, &options).unwrap();

        assert!(result.quality.gap >= 0.0);
        assert!(result.quality.certified_lower_bound.is_finite());
        assert!(
            (result.objective
                - (result.quality.certified_lower_bound + result.quality.gap))
                .abs()
                < 1e-8
        );
    }

    #[test]
    fn prepared_solver_solve_subproblem_matches_explicit_child_solve() {
        let q = QuadraticMatrix::dense(diagonal_matrix(&[4.0, 2.0, 3.0]));
        let c = vec![-1.0, -0.25, 0.5];
        let root_lb = vec![0.0, 0.0, 0.0];
        let root_ub = vec![1.0, 1.0, 1.0];
        let child_lb = vec![1.0, 0.0, 0.0];
        let child_ub = vec![1.0, 1.0, 1.0];

        let mut options = SolverOptions::default();
        options.scaling.mode = ScalingMode::HessianDiag;
        options.polish.enabled = true;

        let root = solve_box_qp(&q, &c, &root_lb, &root_ub, &options).unwrap();
        let prepared = PreparedSolver::new(&q, &c, &options).unwrap();
        let child_from_prepared = prepared
            .solve_subproblem(&child_lb, &child_ub, Some(&root.x))
            .unwrap();

        let mut explicit_child_options = options.clone();
        explicit_child_options.x0 = Some(root.x.clone());
        let explicit_child = solve_box_qp(&q, &c, &child_lb, &child_ub, &explicit_child_options)
            .unwrap();

        assert_eq!(child_from_prepared.x.len(), explicit_child.x.len());
        for (xp, xe) in child_from_prepared.x.iter().zip(explicit_child.x.iter()) {
            assert!((xp - xe).abs() < 1e-9);
        }
        assert!((child_from_prepared.objective - explicit_child.objective).abs() < 1e-10);
        assert!(
            (child_from_prepared.quality.kkt_inf - explicit_child.quality.kkt_inf).abs() < 1e-10
        );
    }

    #[test]
    fn solves_simple_implicit_diagonal_box_qp() {
        let q = Arc::new(DiagonalOperator {
            diag: vec![2.0, 2.0, 2.0],
        });
        let c = vec![-1.0, 0.5, -3.0];
        let (lb, ub) = default_bounds(3);
        let mut options = SolverOptions::default();
        options.assume_symmetric = true;
        options.scaling.mode = ScalingMode::None;
        options.polish.enabled = true;

        let result = solve_box_qp_implicit(q, &c, &lb, &ub, &options).unwrap();

        assert!(result.quality.rel_gap < 1e-5);
        assert!(result.x[0] > 0.4 && result.x[0] < 0.6);
        assert!(result.x[1] < 1e-6);
        assert!(result.x[2] > 0.99);
        assert_eq!(result.timing.polish_time_sec, 0.0);
    }

    #[test]
    fn implicit_solver_rejects_missing_symmetry_assumption() {
        let q = Arc::new(DiagonalOperator {
            diag: vec![1.0, 2.0],
        });
        let c = vec![0.0, 0.0];
        let lb = vec![0.0, 0.0];
        let ub = vec![1.0, 1.0];
        let options = SolverOptions::default();

        let err = solve_box_qp_implicit(q, &c, &lb, &ub, &options).unwrap_err();
        assert!(err.to_string().contains("assume_symmetric=true"));
    }

    #[test]
    fn implicit_scaling_uses_diagonal_when_available() {
        let q = Arc::new(DiagonalOperator {
            diag: vec![4.0, 9.0],
        });
        let c = vec![-2.0, 3.0];
        let lb = vec![0.0, 0.0];
        let ub = vec![1.0, 1.0];

        let mut options = SolverOptions::default();
        options.assume_symmetric = true;
        options.scaling.mode = ScalingMode::HessianDiag;
        options.polish.enabled = true;

        let result = solve_box_qp_implicit(q, &c, &lb, &ub, &options).unwrap();
        assert!(result.scaling.applied);
        assert_eq!(result.scaling.name, "hessian_diag");
    }

    #[test]
    fn implicit_scaling_falls_back_to_unscaled_without_diagonal() {
        let q = Arc::new(NoDiagonalOperator { n: 2 });
        let c = vec![-0.25, 0.5];
        let lb = vec![0.0, 0.0];
        let ub = vec![1.0, 1.0];

        let mut options = SolverOptions::default();
        options.assume_symmetric = true;
        options.scaling.mode = ScalingMode::HessianDiag;
        options.polish.enabled = true;

        let result = solve_box_qp_implicit(q, &c, &lb, &ub, &options).unwrap();
        assert!(!result.scaling.applied);
        assert_eq!(result.scaling.name, "none");
    }
}