use crate::application::linalg::thresholds::rank_pivot_ratio;
use crate::domain::real::RealScalar;
use leto::{ArrayView2, LetoError, Result};
pub(super) struct Factored<T> {
pub(super) u: Vec<T>,
pub(super) d: Vec<T>,
pub(super) n: usize,
}
pub(super) fn factor<T: RealScalar>(matrix: &ArrayView2<'_, T>) -> Result<Factored<T>> {
let [n, cols] = matrix.shape();
if n != cols {
return Err(LetoError::ShapeMismatch {
lhs: vec![n, cols],
rhs: vec![n, n],
});
}
let contiguous;
let a = if let Some(slice) = matrix.as_slice() {
slice
} else {
contiguous = matrix.to_contiguous();
leto::Storage::as_slice(contiguous.storage())
};
let mut scale = T::ZERO;
for i in 0..n {
for j in 0..n {
let value = a[i * n + j];
if !value.is_finite() {
return Err(LetoError::StorageError {
reason: "UDU input contains a non-finite value".to_string(),
});
}
if value.abs() > scale {
scale = value.abs();
}
}
}
let sym_tol = scale.mul(T::ONE.div(T::from_count(1_000_000_000)));
for i in 0..n {
for j in (i + 1)..n {
if a[i * n + j].sub(a[j * n + i]).abs() > sym_tol {
return Err(LetoError::StorageError {
reason: "UDU requires a symmetric matrix".to_string(),
});
}
}
}
let pivot_tol = scale.mul(rank_pivot_ratio::<T>());
let mut u = vec![T::ZERO; n * n];
let mut d = vec![T::ZERO; n];
let mut w = vec![T::ZERO; n];
for j in (0..n).rev() {
let w = &mut w[..n - (j + 1)];
for (wk, (&ujk, &dk)) in w
.iter_mut()
.zip(u[j * n + (j + 1)..j * n + n].iter().zip(&d[j + 1..n]))
{
*wk = ujk.mul(dk);
}
let dj = a[j * n + j].sub(T::dot_slice(&u[j * n + (j + 1)..j * n + n], w));
if dj.abs() <= pivot_tol {
return Err(LetoError::StorageError {
reason: "UDU encountered a zero pivot (needs symmetric pivoting)".to_string(),
});
}
d[j] = dj;
u[j * n + j] = T::ONE;
for i in (0..j).rev() {
let uij = a[i * n + j].sub(T::dot_slice(&u[i * n + (j + 1)..i * n + n], w));
u[i * n + j] = uij.div(dj);
}
}
Ok(Factored { u, d, n })
}