use crate::errors::linear_error::LinearError;
use crate::traits::row_ops::RowOps;
use crate::types::dense_vector::DenseVector;
use alloc::vec::Vec;
use deep_causality_algebra::NormedScalar;
use deep_causality_num::{One, Zero};
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct Lu<T> {
factors: Vec<T>,
order: usize,
permutation: Vec<usize>,
sign_is_negative: bool,
}
impl<T> Lu<T>
where
T: NormedScalar,
{
pub fn factor<M>(m: &M) -> Result<Self, LinearError>
where
M: RowOps<Scalar = T> + Clone,
{
let (rows, cols) = (m.rows(), m.cols());
if rows != cols {
return Err(LinearError::NotSquare((rows, cols)));
}
let n = rows;
let mut a = alloc::vec::Vec::with_capacity(n * n);
for i in 0..n {
for j in 0..n {
a.push(m.get(i, j)?);
}
}
let mut perm: alloc::vec::Vec<usize> = (0..n).collect();
let mut negative = false;
for col in 0..n {
let mut best = col;
let mut best_mag = a[col * n + col].modulus_squared();
for r in (col + 1)..n {
let mag = a[r * n + col].modulus_squared();
if mag > best_mag {
best = r;
best_mag = mag;
}
}
if a[best * n + col].is_zero() {
return Err(LinearError::Singular(col));
}
if best != col {
for j in 0..n {
a.swap(col * n + j, best * n + j);
}
perm.swap(col, best);
negative = !negative;
}
let head = a[col * n + col];
for r in (col + 1)..n {
let factor = a[r * n + col] / head;
a[r * n + col] = factor;
for j in (col + 1)..n {
let upd = a[r * n + j] - factor * a[col * n + j];
a[r * n + j] = upd;
}
}
}
Ok(Self {
factors: a,
order: n,
permutation: perm,
sign_is_negative: negative,
})
}
pub fn apply(&self, b: &DenseVector<T>) -> Result<DenseVector<T>, LinearError> {
let n = self.order;
if b.len() != n {
return Err(LinearError::LengthMismatch(n, b.len()));
}
let mut y = alloc::vec::Vec::with_capacity(n);
for i in 0..n {
let mut acc = b.get(self.permutation[i])?;
for (j, yj) in y.iter().enumerate().take(i) {
acc = acc - self.factors[i * n + j] * *yj;
}
y.push(acc);
}
let mut x = alloc::vec![T::zero(); n];
for i in (0..n).rev() {
let mut acc = y[i];
for (j, xj) in x.iter().enumerate().skip(i + 1) {
acc = acc - self.factors[i * n + j] * *xj;
}
x[i] = acc / self.factors[i * n + i];
}
Ok(DenseVector::from_vec(x))
}
pub fn permutation(&self) -> &[usize] {
&self.permutation
}
pub fn determinant(&self) -> T {
let n = self.order;
let mut d = T::one();
for i in 0..n {
d = d * self.factors[i * n + i];
}
if self.sign_is_negative {
T::zero() - d
} else {
d
}
}
}
pub fn solve<M>(a: &M, b: &DenseVector<M::Scalar>) -> Result<DenseVector<M::Scalar>, LinearError>
where
M: RowOps + Clone,
M::Scalar: NormedScalar,
{
if b.len() != a.rows() {
return Err(LinearError::LengthMismatch(a.rows(), b.len()));
}
Lu::factor(a)?.apply(b)
}
pub fn solve_lower<M>(
a: &M,
b: &DenseVector<M::Scalar>,
) -> Result<DenseVector<M::Scalar>, LinearError>
where
M: RowOps,
M::Scalar: NormedScalar,
{
let n = a.rows();
if n != a.cols() {
return Err(LinearError::NotSquare((n, a.cols())));
}
if b.len() != n {
return Err(LinearError::LengthMismatch(n, b.len()));
}
for i in 0..n {
for j in 0..n {
if j > i && !a.get(i, j)?.is_zero() {
return Err(LinearError::WrongTriangle((i, j)));
}
}
}
let mut x = alloc::vec![M::Scalar::zero(); n];
for i in 0..n {
let d = a.get(i, i)?;
if d.is_zero() {
return Err(LinearError::ZeroDiagonal(i));
}
let mut acc = b.get(i)?;
for (j, xj) in x.iter().enumerate().take(i) {
acc = acc - a.get(i, j)? * *xj;
}
x[i] = acc / d;
}
Ok(DenseVector::from_vec(x))
}
pub fn solve_upper<M>(
a: &M,
b: &DenseVector<M::Scalar>,
) -> Result<DenseVector<M::Scalar>, LinearError>
where
M: RowOps,
M::Scalar: NormedScalar,
{
let n = a.rows();
if n != a.cols() {
return Err(LinearError::NotSquare((n, a.cols())));
}
if b.len() != n {
return Err(LinearError::LengthMismatch(n, b.len()));
}
for i in 0..n {
for j in 0..n {
if j < i && !a.get(i, j)?.is_zero() {
return Err(LinearError::WrongTriangle((i, j)));
}
}
}
let mut x = alloc::vec![M::Scalar::zero(); n];
for i in (0..n).rev() {
let d = a.get(i, i)?;
if d.is_zero() {
return Err(LinearError::ZeroDiagonal(i));
}
let mut acc = b.get(i)?;
for (j, xj) in x.iter().enumerate().skip(i + 1) {
acc = acc - a.get(i, j)? * *xj;
}
x[i] = acc / d;
}
Ok(DenseVector::from_vec(x))
}
pub fn inverse<M>(a: &M) -> Result<M, LinearError>
where
M: RowOps + Clone + crate::traits::matrix_build::MatrixBuild,
M::Scalar: NormedScalar,
{
let n = a.rows();
if n != a.cols() {
return Err(LinearError::NotSquare((n, a.cols())));
}
let lu = Lu::factor(a)?;
let mut out = M::zeros(n, n);
for j in 0..n {
let mut e = alloc::vec![M::Scalar::zero(); n];
e[j] = M::Scalar::one();
let col = lu.apply(&DenseVector::from_vec(e))?;
for i in 0..n {
out.set(i, j, col.get(i)?)?;
}
}
Ok(out)
}