mod column_norms;
use self::column_norms::{tail_norm_sq, ColumnNorms};
use crate::application::linalg::householder::{apply_left, apply_right, reflector};
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) q: Vec<T>,
pub(super) r: Vec<T>,
pub(super) perm: Vec<usize>,
pub(super) rank: usize,
pub(super) m: usize,
pub(super) n: usize,
}
pub(super) fn factor<T: RealScalar>(matrix: &ArrayView2<'_, T>) -> Result<Factored<T>> {
let [m, n] = matrix.shape();
let mut r = if let Some(slice) = matrix.as_slice() {
slice.to_vec()
} else {
matrix.to_contiguous().into_storage().into_inner()
};
for &value in &r {
if !value.is_finite() {
return Err(LetoError::StorageError {
reason: "ColPivQR input contains a non-finite value".to_string(),
});
}
}
let mut q = vec![T::ZERO; m * m];
for i in 0..m {
q[i * m + i] = T::ONE;
}
let mut perm: Vec<usize> = (0..n).collect();
let p = m.min(n);
let mut norms = ColumnNorms::new(&r, n, m);
let tol = norms.largest_squared().sqrt().mul(rank_pivot_ratio::<T>());
let mut rank = p;
let mut alw: Vec<T> = Vec::with_capacity(n);
let mut col_stack = [T::ZERO; 128];
let mut col_vec: Vec<T> = Vec::new();
for k in 0..p {
let best = norms.pivot(k);
if tail_norm_sq(&r, n, m, best, k).sqrt() <= tol {
rank = k;
break;
}
if best != k {
for i in 0..m {
r.swap(i * n + k, i * n + best);
}
perm.swap(k, best);
norms.swap(k, best);
}
let len = m - k;
let col = if len <= 128 {
for i in 0..len {
col_stack[i] = r[(k + i) * n + k];
}
&col_stack[..len]
} else {
col_vec.clear();
col_vec.reserve(len);
for i in 0..len {
col_vec.push(r[(k + i) * n + k]);
}
&col_vec[..]
};
if let Some((refl, _alpha)) = reflector(col) {
apply_left(&refl, &mut r, n, k, k, n, &mut alw); apply_right(&refl, &mut q, m, k, 0, m); }
norms.remove_row(&r, n, m, k);
}
for i in 1..m {
for j in 0..i.min(n) {
r[i * n + j] = T::ZERO;
}
}
Ok(Factored {
q,
r,
perm,
rank,
m,
n,
})
}