use super::backend::AsFloatSlice;
use super::compact::{solve_upper_tri, solve_upper_tri_transposed};
use crate::core::math::Scalar;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub(crate) enum SubsmStatus {
InteriorStep,
BoundEncountered,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub(crate) enum SubsmError {
SingularK,
}
#[allow(clippy::too_many_arguments)]
#[inline(never)]
pub(crate) fn subsm<F: Scalar, V: AsFloatSlice<F>>(
x: &mut [F],
d: &mut [F],
xp: &mut [F],
xx: &[F],
gg: &[F],
ind: &[usize],
l: &[F],
u: &[F],
ws_cols: &[V],
wy_cols: &[V],
wn: &[F],
wv: &mut [F],
m: usize,
col: usize,
theta: F,
) -> Result<SubsmStatus, SubsmError> {
let n = x.len();
let nsub = ind.len();
debug_assert_eq!(xp.len(), n);
debug_assert_eq!(xx.len(), n);
debug_assert_eq!(gg.len(), n);
debug_assert_eq!(l.len(), n);
debug_assert_eq!(u.len(), n);
debug_assert!(d.len() >= nsub);
debug_assert_eq!(ws_cols.len(), col);
debug_assert_eq!(wy_cols.len(), col);
debug_assert!(col <= m);
debug_assert!(wv.len() >= 2 * m);
if nsub == 0 {
return Ok(SubsmStatus::InteriorStep);
}
let zero = F::zero();
let one = F::one();
let m2 = 2 * m;
let col2 = 2 * col;
for i in 0..col {
let wy = wy_cols[i].as_float_slice();
let ws = ws_cols[i].as_float_slice();
let mut temp1 = zero;
let mut temp2 = zero;
for j in 0..nsub {
let k = ind[j];
temp1 = temp1 + wy[k] * d[j];
temp2 = temp2 + ws[k] * d[j];
}
wv[i] = temp1;
wv[col + i] = theta * temp2;
}
if col > 0 {
for i in 0..col2 {
let pivot = wn[i * m2 + i];
if pivot == zero || !pivot.is_finite() {
return Err(SubsmError::SingularK);
}
}
solve_upper_tri_transposed(wn, col2, m2, &mut wv[0..col2]);
for slot in wv.iter_mut().take(col) {
*slot = -*slot;
}
solve_upper_tri(wn, col2, m2, &mut wv[0..col2]);
}
for jy in 0..col {
let wy = wy_cols[jy].as_float_slice();
let ws = ws_cols[jy].as_float_slice();
let js = col + jy;
add_history_column(&mut d[..nsub], ind, wy, ws, wv[jy], wv[js], theta);
}
for slot in d.iter_mut().take(nsub) {
*slot = *slot / theta;
}
xp.copy_from_slice(x);
let mut iword = SubsmStatus::InteriorStep;
for i in 0..nsub {
let k = ind[i];
let dk = d[i];
let xk = x[k];
let l_finite = l[k].is_finite();
let u_finite = u[k].is_finite();
if !l_finite && !u_finite {
x[k] = xk + dk;
} else if l_finite && !u_finite {
let new_x = (xk + dk).max(l[k]);
x[k] = new_x;
if new_x == l[k] {
iword = SubsmStatus::BoundEncountered;
}
} else if l_finite && u_finite {
let tmp = (xk + dk).max(l[k]);
x[k] = tmp.min(u[k]);
if x[k] == l[k] || x[k] == u[k] {
iword = SubsmStatus::BoundEncountered;
}
} else {
let new_x = (xk + dk).min(u[k]);
x[k] = new_x;
if new_x == u[k] {
iword = SubsmStatus::BoundEncountered;
}
}
}
if iword == SubsmStatus::InteriorStep {
return Ok(iword);
}
let mut dd_p = zero;
for i in 0..n {
dd_p = dd_p + (x[i] - xx[i]) * gg[i];
}
if dd_p <= zero {
return Ok(iword);
}
x.copy_from_slice(xp);
let mut alpha = one;
let mut temp1 = alpha;
let mut ibd: Option<usize> = None;
for i in 0..nsub {
let k = ind[i];
let dk = d[i];
let l_finite = l[k].is_finite();
let u_finite = u[k].is_finite();
if !l_finite && !u_finite {
continue;
}
if dk < zero && l_finite {
let temp2 = l[k] - x[k];
if temp2 >= zero {
temp1 = zero;
} else if dk * alpha < temp2 {
temp1 = temp2 / dk;
}
} else if dk > zero && u_finite {
let temp2 = u[k] - x[k];
if temp2 <= zero {
temp1 = zero;
} else if dk * alpha > temp2 {
temp1 = temp2 / dk;
}
}
if temp1 < alpha {
alpha = temp1;
ibd = Some(i);
}
}
if alpha < one {
if let Some(ibd) = ibd {
let dk = d[ibd];
let k = ind[ibd];
if dk > zero {
x[k] = u[k];
d[ibd] = zero;
} else if dk < zero {
x[k] = l[k];
d[ibd] = zero;
}
}
}
for i in 0..nsub {
let k = ind[i];
x[k] = x[k] + alpha * d[i];
}
Ok(iword)
}
fn add_history_column<F: Scalar>(
d: &mut [F],
ind: &[usize],
wy: &[F],
ws: &[F],
y_weight: F,
s_weight: F,
theta: F,
) {
let end = d.len() / 4 * 4;
for (di, indices) in d[..end].chunks_exact_mut(4).zip(ind.chunks_exact(4)) {
let y = [
wy[indices[0]],
wy[indices[1]],
wy[indices[2]],
wy[indices[3]],
];
let s = [
ws[indices[0]],
ws[indices[1]],
ws[indices[2]],
ws[indices[3]],
];
for i in 0..4 {
di[i] = di[i] + y[i] * y_weight / theta + s[i] * s_weight;
}
}
for (di, &k) in d[end..].iter_mut().zip(&ind[end..]) {
*di = *di + wy[k] * y_weight / theta + ws[k] * s_weight;
}
}
#[cfg(test)]
#[allow(clippy::identity_op, clippy::erasing_op)]
mod tests {
use super::*;
#[test]
fn history_correction_preserves_index_order_and_scalar_arithmetic() {
fn check<F: Scalar>() {
let n = 19;
let s: Vec<_> = (0..n)
.map(|i| F::from_f64((i as f64 - 9.0) / 7.0).unwrap())
.collect();
let mut y: Vec<_> = s.iter().map(|&s| s * s - F::one()).collect();
y[3] = F::nan();
let indices: Vec<_> = (0..n).rev().filter(|&i| i != 3).collect();
for nfree in 0..=indices.len() {
let ind = &indices[..nfree];
let mut d: Vec<_> = ind.iter().map(|&i| s[i]).collect();
let mut expected = d.clone();
let theta = F::from_f64(1.7).unwrap();
let yw = F::from_f64(-0.3).unwrap();
let sw = F::from_f64(0.8).unwrap();
for (di, &i) in expected.iter_mut().zip(ind) {
*di = *di + y[i] * yw / theta + s[i] * sw;
}
add_history_column(&mut d, ind, &y, &s, yw, sw, theta);
assert_eq!(d, expected);
}
}
check::<f32>();
check::<f64>();
}
#[test]
fn history_correction_keeps_division_after_product_at_extreme_scales() {
fn check<F: Scalar>() {
let two = F::one() + F::one();
let ind = [0, 1, 2, 3, 4];
let mut d = [F::zero(); 5];
let s = [F::zero(); 5];
let y = [F::max_value(); 5];
add_history_column(&mut d, &ind, &y, &s, two, F::zero(), two);
assert!(d.iter().all(|x| *x == F::infinity()));
let half = F::one() / two;
let tiny = F::min_positive_value() * F::epsilon();
let y = [tiny; 5];
d.fill(F::zero());
add_history_column(&mut d, &ind, &y, &s, half, F::zero(), half);
assert_eq!(d, [F::zero(); 5]);
}
check::<f32>();
check::<f64>();
}
#[test]
fn empty_subspace_is_no_op() {
let mut x = vec![0.5_f64, 1.5];
let x_in = x.clone();
let mut d = Vec::<f64>::new();
let mut xp = vec![0.0_f64; 2];
let xx = vec![0.0_f64; 2];
let gg = vec![0.0_f64; 2];
let ind: Vec<usize> = Vec::new();
let l = vec![0.0, 0.0];
let u = vec![2.0, 2.0];
let ws_cols: Vec<&[f64]> = Vec::new();
let wy_cols: Vec<&[f64]> = Vec::new();
let wn = Vec::<f64>::new();
let mut wv = vec![0.0; 2];
let status = subsm(
&mut x, &mut d, &mut xp, &xx, &gg, &ind, &l, &u, &ws_cols,
&wy_cols, &wn, &mut wv, 1, 0, 1.0,
)
.unwrap();
assert_eq!(status, SubsmStatus::InteriorStep);
assert_eq!(x, x_in);
}
#[test]
fn col_zero_unbounded_uses_scaled_reduced_gradient() {
let mut x = vec![1.0, 2.0];
let mut d = vec![3.0, -1.0];
let mut xp = vec![0.0; 2];
let xx = vec![0.0, 0.0];
let gg = vec![0.0, 0.0];
let ind = vec![0usize, 1];
let l = vec![f64::NEG_INFINITY, f64::NEG_INFINITY];
let u = vec![f64::INFINITY, f64::INFINITY];
let ws_cols: Vec<&[f64]> = Vec::new();
let wy_cols: Vec<&[f64]> = Vec::new();
let wn = Vec::<f64>::new();
let mut wv = vec![0.0; 2];
let status = subsm(
&mut x, &mut d, &mut xp, &xx, &gg, &ind, &l, &u, &ws_cols,
&wy_cols, &wn, &mut wv, 1, 0, 1.0,
)
.unwrap();
assert_eq!(status, SubsmStatus::InteriorStep);
assert!((x[0] - 4.0).abs() < 1e-12);
assert!((x[1] - 1.0).abs() < 1e-12);
assert!((d[0] - 3.0).abs() < 1e-12);
assert!((d[1] - (-1.0)).abs() < 1e-12);
}
#[test]
fn col_zero_theta_two_halves_the_step() {
let mut x = vec![1.0];
let mut d = vec![4.0];
let mut xp = vec![0.0; 1];
let xx = vec![0.0];
let gg = vec![0.0];
let ind = vec![0usize];
let l = vec![f64::NEG_INFINITY];
let u = vec![f64::INFINITY];
let ws_cols: Vec<&[f64]> = Vec::new();
let wy_cols: Vec<&[f64]> = Vec::new();
let wn = Vec::<f64>::new();
let mut wv = vec![0.0; 2];
subsm(
&mut x, &mut d, &mut xp, &xx, &gg, &ind, &l, &u, &ws_cols,
&wy_cols, &wn, &mut wv, 1, 0, 2.0,
)
.unwrap();
assert!((d[0] - 2.0).abs() < 1e-12);
assert!((x[0] - 3.0).abs() < 1e-12);
}
#[test]
fn col_zero_step_clips_at_upper_bound() {
let mut x = vec![0.5];
let mut d = vec![1.0];
let mut xp = vec![0.0; 1];
let xx = vec![0.5];
let gg = vec![0.0];
let ind = vec![0usize];
let l = vec![f64::NEG_INFINITY];
let u = vec![1.0];
let ws_cols: Vec<&[f64]> = Vec::new();
let wy_cols: Vec<&[f64]> = Vec::new();
let wn = Vec::<f64>::new();
let mut wv = vec![0.0; 2];
let status = subsm(
&mut x, &mut d, &mut xp, &xx, &gg, &ind, &l, &u, &ws_cols,
&wy_cols, &wn, &mut wv, 1, 0, 1.0,
)
.unwrap();
assert_eq!(status, SubsmStatus::BoundEncountered);
assert_eq!(x[0], 1.0);
assert!((d[0] - 1.0).abs() < 1e-12);
}
#[test]
fn col_zero_step_clips_at_lower_bound_both_bounded() {
let mut x = vec![0.5];
let mut d = vec![-1.0];
let mut xp = vec![0.0; 1];
let xx = vec![0.5];
let gg = vec![0.0];
let ind = vec![0usize];
let l = vec![0.0];
let u = vec![1.0];
let ws_cols: Vec<&[f64]> = Vec::new();
let wy_cols: Vec<&[f64]> = Vec::new();
let wn = Vec::<f64>::new();
let mut wv = vec![0.0; 2];
let status = subsm(
&mut x, &mut d, &mut xp, &xx, &gg, &ind, &l, &u, &ws_cols,
&wy_cols, &wn, &mut wv, 1, 0, 1.0,
)
.unwrap();
assert_eq!(status, SubsmStatus::BoundEncountered);
assert_eq!(x[0], 0.0);
}
#[test]
fn col_one_one_free_one_active_matches_closed_form_newton() {
let s = [1.0_f64, 2.0];
let y = [1.0_f64, 1.0];
let theta = 1.0_f64;
let m = 1;
let m2 = 2 * m;
let mut wn = vec![0.0_f64; m2 * m2];
wn[0 * m2 + 0] = 2.0;
wn[0 * m2 + 1] = 0.5;
wn[1 * m2 + 1] = 4.25_f64.sqrt();
let mut x = vec![0.0_f64, 0.0];
let mut d = vec![1.0_f64];
let mut xp = vec![0.0_f64; 2];
let xx = vec![0.0_f64, 0.0];
let gg = vec![0.0_f64, 0.0];
let ind = vec![0usize];
let l = vec![f64::NEG_INFINITY, 0.0];
let u = vec![f64::INFINITY, f64::INFINITY];
let ws_cols: Vec<&[f64]> = vec![&s];
let wy_cols: Vec<&[f64]> = vec![&y];
let mut wv = vec![0.0_f64; 2 * m];
let status = subsm(
&mut x, &mut d, &mut xp, &xx, &gg, &ind, &l, &u, &ws_cols,
&wy_cols, &wn, &mut wv, m, 1, theta,
)
.unwrap();
assert_eq!(status, SubsmStatus::InteriorStep);
let expected = 15.0 / 17.0;
assert!(
(d[0] - expected).abs() < 1e-12,
"d_out = {} vs {}",
d[0],
expected
);
assert!(
(x[0] - expected).abs() < 1e-12,
"x[0] = {} vs {}",
x[0],
expected
);
assert_eq!(x[1], 0.0);
}
#[test]
fn col_zero_backtracking_caps_step_at_binding_variable() {
let mut x = vec![0.5, 0.0];
let mut d = vec![2.0, -0.1];
let mut xp = vec![0.0; 2];
let xx = vec![0.0, 0.0];
let gg = vec![10.0, -0.1];
let ind = vec![0usize, 1];
let l = vec![0.0, f64::NEG_INFINITY];
let u = vec![1.0, f64::INFINITY];
let ws_cols: Vec<&[f64]> = Vec::new();
let wy_cols: Vec<&[f64]> = Vec::new();
let wn = Vec::<f64>::new();
let mut wv = vec![0.0; 2];
let status = subsm(
&mut x, &mut d, &mut xp, &xx, &gg, &ind, &l, &u, &ws_cols,
&wy_cols, &wn, &mut wv, 1, 0, 1.0,
)
.unwrap();
assert_eq!(status, SubsmStatus::BoundEncountered);
assert!((x[0] - 1.0).abs() < 1e-12);
assert_eq!(d[0], 0.0);
assert!((x[1] - (-0.025)).abs() < 1e-12);
}
}