gam-linalg 0.3.151

//! Exact Euclidean trust-region solve for a symmetric positive-semidefinite model.
//!
//! For
//!
//! ```text
//! minimize  1/2 xᵀ G x - bᵀ x    subject to ||x||₂ <= radius,
//! ```
//!
//! with `G ⪰ 0`, the KKT solution is `(G + μI)x = b`, `μ >= 0`. A null-space
//! component of `b` is therefore not an error when the radius is finite: it makes
//! `μ > 0` and puts the solution on the boundary. This is deliberately distinct
//! from an unconstrained Moore–Penrose normal-equation solve, where such a
//! component proves the objective is unbounded.

use crate::LinalgError;
use crate::faer_ndarray::FaerEigh;
use crate::roundoff::accumulation_growth;
use faer::Side;
use ndarray::{Array1, ArrayView1, ArrayView2};

/// Overflow-safe Euclidean norm (the LAPACK `xLASSQ` recurrence).
fn scaled_norm(values: impl IntoIterator<Item = f64>) -> f64 {
    let mut scale = 0.0_f64;
    let mut sumsq = 1.0_f64;
    for value in values {
        let magnitude = value.abs();
        if magnitude == 0.0 {
            continue;
        }
        if scale < magnitude {
            let ratio = scale / magnitude;
            sumsq = 1.0 + sumsq * ratio * ratio;
            scale = magnitude;
        } else {
            let ratio = magnitude / scale;
            sumsq += ratio * ratio;
        }
    }
    if scale == 0.0 {
        0.0
    } else {
        scale * sumsq.sqrt()
    }
}

/// Solve a convex PSD quadratic inside a finite Euclidean trust ball.
///
/// The eigensystem is scaled by `max(||G||₂, ||b||₂ / radius)` before the
/// secular solve, so its bracket is dimensionless and `O(1)` under arbitrary
/// common rescaling of `(G, b)`. Numerical-null RHS components participate in
/// the boundary solution instead of being discarded.
pub fn solve_psd_trust_region(
    gram: ArrayView2<'_, f64>,
    rhs: ArrayView1<'_, f64>,
    radius: f64,
) -> Result<Array1<f64>, LinalgError> {
    let dimension = gram.nrows();
    if dimension == 0 || gram.ncols() != dimension || rhs.len() != dimension {
        return Err(LinalgError::InvalidInput(format!(
            "PSD trust-region shape mismatch: gram={:?}, rhs={}",
            gram.dim(),
            rhs.len()
        )));
    }
    if !(radius.is_finite() && radius > 0.0) {
        return Err(LinalgError::InvalidInput(format!(
            "PSD trust-region radius must be finite and positive, got {radius}"
        )));
    }
    if gram.iter().any(|value| !value.is_finite())
        || rhs.iter().any(|value| !value.is_finite())
    {
        return Err(LinalgError::InvalidInput(
            "PSD trust-region inputs must be finite".to_string(),
        ));
    }

    let mut symmetric = gram.to_owned();
    symmetric += &gram.t();
    symmetric *= 0.5;
    let (eigenvalues, eigenvectors) = symmetric.eigh(Side::Lower).map_err(|error| {
        LinalgError::InvalidInput(format!(
            "PSD trust-region eigendecomposition failed: {error}"
        ))
    })?;
    let spectral_scale = eigenvalues
        .iter()
        .map(|value| value.abs())
        .fold(0.0_f64, f64::max);
    let rank_tolerance = dimension as f64 * f64::EPSILON * spectral_scale;
    let minimum_eigenvalue = eigenvalues.iter().copied().fold(f64::INFINITY, f64::min);
    if minimum_eigenvalue < -rank_tolerance {
        return Err(LinalgError::InvalidInput(format!(
            "PSD trust-region matrix is not positive semidefinite: minimum eigenvalue \
             {minimum_eigenvalue:.17e}, backward-error tolerance {rank_tolerance:.17e}"
        )));
    }

    let projected = eigenvectors.t().dot(&rhs);
    let rhs_norm = scaled_norm(projected.iter().copied());
    if rhs_norm == 0.0 {
        return Ok(Array1::zeros(dimension));
    }

    // Try the canonical interior Moore–Penrose solution. A material RHS in a
    // numerical-null mode makes the unconstrained model unbounded, so it must go
    // through the finite-radius boundary solve below.
    let null_rhs_norm = scaled_norm(
        eigenvalues
            .iter()
            .zip(projected.iter())
            .filter_map(|(&eigenvalue, &coefficient)| {
                (eigenvalue <= rank_tolerance).then_some(coefficient)
            }),
    );
    let rhs_tolerance = dimension as f64 * f64::EPSILON * rhs_norm;
    if null_rhs_norm <= rhs_tolerance {
        let mut spectral_step = Array1::<f64>::zeros(dimension);
        for mode in 0..dimension {
            if eigenvalues[mode] > rank_tolerance {
                spectral_step[mode] = projected[mode] / eigenvalues[mode];
            }
        }
        let step_norm = scaled_norm(spectral_step.iter().copied());
        if step_norm.is_finite() && step_norm <= radius {
            return Ok(eigenvectors.dot(&spectral_step));
        }
    }

    // Dimensionless boundary equation. With
    //   s = max(||G||₂, ||b||₂/r), ell=λ/s, d=c/(r s), nu=μ/s,
    // the secular equation is ||d/(ell+nu)||₂ = 1 and `||d||₂ <= 1`.
    let shift_scale = spectral_scale.max(rhs_norm / radius);
    if !(shift_scale.is_finite() && shift_scale > 0.0) {
        return Err(LinalgError::InvalidInput(
            "PSD trust-region shift is not representable in f64".to_string(),
        ));
    }
    let scaled_eigenvalues = eigenvalues.mapv(|value| value.max(0.0) / shift_scale);
    let scaled_rhs = projected.mapv(|value| (value / shift_scale) / radius);
    let mut lower = 0.0_f64;
    let mut upper = scaled_norm(scaled_rhs.iter().copied());
    if !(upper.is_finite() && upper > 0.0) {
        return Err(LinalgError::InvalidInput(
            "PSD trust-region secular bracket is not representable in f64".to_string(),
        ));
    }

    // `norm` is a `dimension`-term accumulation, so it cannot resolve a
    // deviation from one that is finer than its own backward-error band; below
    // that band `norm == 1` and `norm != 1` are the same statement about the
    // exact secular function. The norm is `O(1)` at the root, so the band is
    // the growth factor itself. (`scaled_norm` finishes with a square root,
    // which halves relative error rather than growing it, so the plain
    // accumulation bound is conservative.)
    let residual_band = accumulation_growth(dimension);
    let mut shift = 0.5 * upper;
    if shift == 0.0 {
        return Err(LinalgError::InvalidInput(
            "PSD trust-region secular shift underflowed".to_string(),
        ));
    }

    // `‖d/(ℓ+ν)‖₂` is strictly decreasing in `ν ≥ 0`, and `ν = ‖d‖₂` already
    // gives a norm of at most one because `ℓ ≥ 0`, so `[lower, upper]` brackets
    // the root on entry and every update preserves that. Newton is safeguarded
    // by the bracket, but a Newton step may land arbitrarily close to an
    // endpoint, so bracket progress is enforced separately: an iteration that
    // fails to halve the bracket forces the next step to bisect. The bracket
    // therefore contracts by at least 2x every two iterations, which bounds the
    // loop by the number of times an f64 interval can be halved before its
    // midpoint is bit-identical to an endpoint. Reaching that point is not a
    // failure to converge — it means the root is pinned between adjacent
    // representable values and no finer answer exists in this precision — so
    // both exits are certified and neither is an iteration budget.
    let mut previous_width = upper - lower;
    let mut force_bisection = false;
    loop {
        let norm = scaled_norm(
            scaled_rhs
                .iter()
                .zip(scaled_eigenvalues.iter())
                .map(|(&coefficient, &eigenvalue)| coefficient / (eigenvalue + shift)),
        );
        if norm.is_finite() {
            if norm > 1.0 {
                lower = shift;
            } else {
                upper = shift;
            }
            if (norm - 1.0).abs() <= residual_band {
                break;
            }
        } else {
            // `shift` sits on a pole of the secular equation, hence below the
            // root: the norm diverges to `+inf` as the pole is approached from
            // above, and the root is where it falls to one.
            lower = shift;
        }

        let width = upper - lower;
        let midpoint = 0.5 * (lower + upper);
        if !(midpoint > lower && midpoint < upper) {
            break;
        }
        let bisect = force_bisection || !norm.is_finite();
        force_bisection = width > 0.5 * previous_width;
        previous_width = width;

        shift = if bisect {
            midpoint
        } else {
            // Safeguarded Newton on phi(nu)=1/||y(nu)||-1. Expressing the
            // derivative through normalized y avoids squaring a huge near-pole step.
            let inverse_norm = 1.0 / norm;
            let mut weighted_inverse_denominator = 0.0_f64;
            for (&coefficient, &eigenvalue) in scaled_rhs.iter().zip(scaled_eigenvalues.iter())
            {
                let denominator = eigenvalue + shift;
                let normalized = (coefficient / denominator) * inverse_norm;
                weighted_inverse_denominator += normalized * normalized / denominator;
            }
            let phi = inverse_norm - 1.0;
            let phi_derivative = inverse_norm * weighted_inverse_denominator;
            let candidate = shift - phi / phi_derivative;
            if candidate.is_finite() && candidate > lower && candidate < upper {
                candidate
            } else {
                midpoint
            }
        };
    }

    // Use the feasible side of the final bracket. The spectral vector has norm
    // at most one, so multiplication by the finite radius cannot overflow.
    let spectral_step = Array1::from_iter(
        scaled_rhs
            .iter()
            .zip(scaled_eigenvalues.iter())
            .map(|(&coefficient, &eigenvalue)| {
                radius * coefficient / (eigenvalue + upper)
            }),
    );
    let step = eigenvectors.dot(&spectral_step);
    if step.iter().any(|value| !value.is_finite()) {
        return Err(LinalgError::InvalidInput(
            "PSD trust-region solution is not representable in f64".to_string(),
        ));
    }
    Ok(step)
}

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

    fn norm(vector: &Array1<f64>) -> f64 {
        scaled_norm(vector.iter().copied())
    }

    #[test]
    fn null_space_rhs_takes_the_exact_boundary_descent_step() {
        let gram = Array2::<f64>::zeros((2, 2));
        let rhs = array![3.0, -4.0];
        let step = solve_psd_trust_region(gram.view(), rhs.view(), 0.25).unwrap();
        assert!((step[0] - 0.15).abs() < 1.0e-14);
        assert!((step[1] + 0.20).abs() < 1.0e-14);
        assert!((norm(&step) - 0.25).abs() < 1.0e-14);
        // (G + 20 I) step = rhs.
        for axis in 0..2 {
            assert!((20.0 * step[axis] - rhs[axis]).abs() < 1.0e-13);
        }
    }

    #[test]
    fn range_identified_interior_solution_is_the_moore_penrose_step() {
        let gram = array![[2.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 4.0]];
        let rhs = array![2.0, 0.0, 8.0];
        let step = solve_psd_trust_region(gram.view(), rhs.view(), 3.0).unwrap();
        assert_eq!(step, array![1.0, 0.0, 2.0]);
    }

    #[test]
    fn boundary_solution_is_scale_equivariant_and_satisfies_common_shift_kkt() {
        let base_gram = array![[1.0, 0.0], [0.0, 4.0]];
        let base_rhs = array![2.0, 1.0];
        let reference =
            solve_psd_trust_region(base_gram.view(), base_rhs.view(), 0.5).unwrap();
        assert!((norm(&reference) - 0.5).abs() < 1.0e-13);
        let shift_0 = (base_rhs[0] - base_gram[[0, 0]] * reference[0]) / reference[0];
        let shift_1 = (base_rhs[1] - base_gram[[1, 1]] * reference[1]) / reference[1];
        assert!(shift_0 > 0.0);
        assert!((shift_0 - shift_1).abs() < 1.0e-12 * shift_0);

        for scale in [1.0e-200, 1.0, 1.0e200] {
            let gram = &base_gram * scale;
            let rhs = &base_rhs * scale;
            let step = solve_psd_trust_region(gram.view(), rhs.view(), 0.5).unwrap();
            for axis in 0..2 {
                assert!((step[axis] - reference[axis]).abs() < 1.0e-12);
            }
        }
    }

    #[test]
    fn genuinely_indefinite_model_is_rejected() {
        let gram = array![[-1.0, 0.0], [0.0, 2.0]];
        let rhs = array![1.0, 1.0];
        let error = solve_psd_trust_region(gram.view(), rhs.view(), 1.0).unwrap_err();
        assert!(error.to_string().contains("not positive semidefinite"));
    }
}