use ndarray::prelude::*;
use ndarray_linalg::SVD;
use ndarray_stats::QuantileExt;
use crate::types::{EPSILON, Error, Result};
pub trait PseudoInverse {
fn pinv(&self) -> Result<Self>
where
Self: Sized;
}
impl PseudoInverse for Array2<f64> {
fn pinv(&self) -> Result<Self> {
let a = *self.abs().max().unwrap_or(&1.);
let m = self / a;
let (u, s, vt) = m
.svd(true, true)
.map_err(|e| Error::Linalg(&format!("Failed to compute SVD: {e}")))?;
let u = u.ok_or_else(|| Error::Linalg("Failed to get U from the SVD."))?;
let vt = vt.ok_or_else(|| Error::Linalg("Failed to get VT from the SVD."))?;
let s_max = s.max().unwrap_or(&0.);
let r_tol = f64::max(EPSILON, s.len() as f64 * s_max * EPSILON);
let s_inv = Array2::from_diag(&s.mapv(|x| if x > r_tol { 1. / x } else { 0. }));
Ok(vt.t().dot(&s_inv).dot(&u.t()) / a)
}
}