use super::super::lu_symbolic::SymbolicLu;
use super::super::CscMatrix;
use super::types::NumericLu;
use crate::domain::real::RealScalar;
use leto::{LetoError, Result};
pub fn factor_numeric<'a, T: RealScalar>(
csc: &CscMatrix<T>,
symbolic: &'a SymbolicLu,
pivot_tolerance: f64,
) -> Result<NumericLu<'a, T>> {
let (nrows, ncols) = csc.shape();
if nrows != ncols {
return Err(LetoError::ShapeMismatch {
lhs: vec![nrows, ncols],
rhs: vec![nrows, nrows],
});
}
let n = nrows;
if symbolic.n() != n {
return Err(LetoError::ShapeMismatch {
lhs: vec![symbolic.n()],
rhs: vec![n],
});
}
let tol = T::from_f64(pivot_tolerance.max(0.0));
let col_ptr = csc.col_ptr();
let row_indices = csc.row_indices();
let values = csc.values();
let l_col_ptr: &Vec<usize> = &symbolic.l_col_ptr;
let l_row_indices: &Vec<usize> = &symbolic.l_row_indices;
let u_col_ptr: &Vec<usize> = &symbolic.u_col_ptr;
let u_row_indices: &Vec<usize> = &symbolic.u_row_indices;
let mut l_values: Vec<T> = vec![T::ZERO; symbolic.l_row_indices.len()];
let mut u_values: Vec<T> = vec![T::ZERO; symbolic.u_row_indices.len()];
let mut row_perm: Vec<usize> = (0..n).collect();
let mut row_inv: Vec<usize> = (0..n).collect();
let mut work: Vec<T> = vec![T::ZERO; n];
let mut work_mark: Vec<usize> = vec![usize::MAX; n];
let mut work_pattern: Vec<usize> = Vec::with_capacity(n);
for j in 0..n {
work_pattern.clear();
for p in col_ptr[j]..col_ptr[j + 1] {
let i = row_indices[p];
let slot = row_inv[i];
work[slot] = values[p];
if work_mark[slot] != j {
work_mark[slot] = j;
work_pattern.push(slot);
}
}
{
let mut fp_idx = 0;
while fp_idx < work_pattern.len() {
let k = work_pattern[fp_idx];
fp_idx += 1;
if k >= j {
continue; }
for &i in l_row_indices
.iter()
.take(l_col_ptr[k + 1])
.skip(l_col_ptr[k])
{
let slot = row_inv[i];
if work_mark[slot] != j {
work_mark[slot] = j;
work_pattern.push(slot);
}
}
}
}
work_pattern.sort_unstable();
for &k in &work_pattern {
if k >= j {
continue;
}
let u_kj = work[k];
for (lp, &i) in l_row_indices
.iter()
.enumerate()
.take(l_col_ptr[k + 1])
.skip(l_col_ptr[k])
{
let slot = row_inv[i];
work[slot] -= l_values[lp] * u_kj;
}
}
let mut pivot_slot = j;
let mut pivot_mag = work[j].abs();
for (slot, &value) in work.iter().enumerate().take(n).skip(j + 1) {
let mag = value.abs();
if mag > pivot_mag {
pivot_mag = mag;
pivot_slot = slot;
}
}
let max_in_col = pivot_mag;
if max_in_col == T::ZERO || pivot_mag < tol * max_in_col {
return Err(LetoError::StorageError {
reason: format!(
"SparseLu: matrix singular to working precision at column {j} \
(pivot magnitude {pivot_mag:?} < tolerance * {max_in_col:?})"
),
});
}
if pivot_slot != j {
work.swap(j, pivot_slot);
let r_a = row_perm[j];
let r_b = row_perm[pivot_slot];
row_perm[j] = r_b;
row_perm[pivot_slot] = r_a;
row_inv[r_b] = j;
row_inv[r_a] = pivot_slot;
if work_mark[pivot_slot] == usize::MAX {
work_mark[pivot_slot] = j;
work_pattern.push(pivot_slot);
}
}
let u_start = u_col_ptr[j];
let u_end = u_col_ptr[j + 1];
for p in u_start..u_end {
let i = u_row_indices[p];
if i == j {
u_values[p] = work[j];
} else {
let slot = row_inv[i];
u_values[p] = work[slot];
}
}
let l_start = l_col_ptr[j];
let l_end = l_col_ptr[j + 1];
let pivot_inv = work[j];
if pivot_inv != T::ZERO {
for p in l_start..l_end {
let i = l_row_indices[p];
let slot = row_inv[i];
if slot > j {
l_values[p] = work[slot] / pivot_inv;
} else {
l_values[p] = work[slot] / pivot_inv;
}
}
}
for &slot in &work_pattern {
work[slot] = T::ZERO;
}
}
let pivoting_free = row_perm
.iter()
.enumerate()
.all(|(slot, &orig)| slot == orig);
if !pivoting_free {
return Err(LetoError::NumericalBreakdown(
"SparseLu: partial pivoting required for this matrix; \
use the dense LU path (SparseLuSolver dispatches automatically)"
.into(),
));
}
Ok(NumericLu {
symbolic,
l_values,
u_values,
row_perm,
})
}