use super::svd_decompose;
use crate::application::linalg::thresholds::rank_pivot_ratio;
use crate::domain::real::RealScalar;
use leto::{Array2, ArrayView2, LetoError, Result, Storage};
fn rank_cutoff_ratio<T: RealScalar>() -> T {
rank_pivot_ratio::<T>()
}
pub fn pinv<T: RealScalar>(matrix: &ArrayView2<'_, T>) -> Result<Array2<T>> {
let [rows, cols] = matrix.shape();
let svd = svd_decompose(matrix)?;
let sigma_max = svd
.singular_values
.iter()
.copied()
.fold(T::ZERO, |m, s| if s > m { s } else { m });
let cutoff = sigma_max.mul(rank_cutoff_ratio::<T>());
let k = svd.singular_values.len();
let mut inv_sigma_stack = [T::ZERO; 128];
let mut inv_sigma_vec = Vec::new();
let inv_sigma = if k <= 128 {
&mut inv_sigma_stack[..k]
} else {
inv_sigma_vec.resize(k, T::ZERO);
&mut inv_sigma_vec[..]
};
for (i, inv_sig_i) in inv_sigma.iter_mut().enumerate().take(k) {
let sigma = svd.singular_values[i];
*inv_sig_i = if sigma > cutoff {
let reciprocal = T::ONE.div(sigma);
if !reciprocal.is_finite() {
return Err(LetoError::Overflow {
reason: "pseudoinverse: a retained singular value's reciprocal exceeds the scalar range",
});
}
reciprocal
} else {
T::ZERO
};
}
let u = &svd.left_singular_vectors; let v = &svd.right_singular_vectors;
let u_slice = u.storage().as_slice();
let v_slice = v.storage().as_slice();
let u_cols = u.shape()[1];
let v_cols = v.shape()[1];
let mut values = vec![T::ZERO; cols * rows];
for i in 0..cols {
for j in 0..rows {
let mut acc = T::ZERO;
for (t, &inv_s) in inv_sigma.iter().enumerate() {
let v_it = v_slice[i * v_cols + t];
let u_jt = u_slice[j * u_cols + t];
acc = acc.add(v_it.mul(inv_s).mul(u_jt));
}
values[i * rows + j] = acc;
}
}
Ok(Array2::from_shape_vec([cols, rows], values)
.expect("pseudoinverse shape [cols, rows] matches storage"))
}