use crate::algorithms::decomposition::flatten;
use crate::errors::linear_error::LinearError;
use crate::traits::matrix_view::MatrixView;
use crate::types::dense_matrix::DenseMatrix;
use crate::types::dense_vector::DenseVector;
use alloc::vec::Vec;
use deep_causality_algebra::{ConjugateScalar, Real};
use deep_causality_num::Zero;
type Re<T> = <T as ConjugateScalar>::Real;
pub fn cholesky<M>(m: &M) -> Result<DenseMatrix<M::Scalar>, LinearError>
where
M: MatrixView,
M::Scalar: ConjugateScalar,
{
let (rows, cols) = (m.rows(), m.cols());
if rows != cols {
return Err(LinearError::NotSquare((rows, cols)));
}
let n = rows;
let a = flatten(m)?;
let mut l = alloc::vec![M::Scalar::zero(); n * n];
for i in 0..n {
for j in 0..=i {
let mut sum = M::Scalar::zero();
for k in 0..j {
sum += l[i * n + k] * l[j * n + k].conjugate();
}
if i == j {
let radicand = a[i * n + i].real_part() - sum.real_part();
if radicand <= Re::<M::Scalar>::zero() {
return Err(LinearError::NotPositiveDefinite(i));
}
l[i * n + j] = <M::Scalar as ConjugateScalar>::from_real(radicand.sqrt());
} else {
l[i * n + j] = (a[i * n + j] - sum) / l[j * n + j];
}
}
}
Ok(DenseMatrix::from_vec(l, n, n).expect("built from the shape"))
}
pub fn solve_least_squares<M>(
a: &M,
b: &DenseVector<M::Scalar>,
) -> Result<DenseVector<M::Scalar>, LinearError>
where
M: MatrixView,
M::Scalar: ConjugateScalar,
{
let (rows, cols) = (a.rows(), a.cols());
if b.len() != rows {
return Err(LinearError::LengthMismatch(rows, b.len()));
}
let n = cols;
let flat = flatten(a)?;
let rhs = b.as_slice();
let mut ata = alloc::vec![M::Scalar::zero(); n * n];
for i in 0..n {
for j in 0..n {
let mut acc = M::Scalar::zero();
for k in 0..rows {
acc += flat[k * n + i].conjugate() * flat[k * n + j];
}
ata[i * n + j] = acc;
}
}
let mut atb: Vec<M::Scalar> = alloc::vec![M::Scalar::zero(); n];
for (i, slot) in atb.iter_mut().enumerate() {
let mut acc = M::Scalar::zero();
for k in 0..rows {
acc += flat[k * n + i].conjugate() * rhs[k];
}
*slot = acc;
}
let l = cholesky(&DenseMatrix::from_vec(ata, n, n).expect("built from the shape"))?;
let lf = l.as_slice();
let mut z = alloc::vec![M::Scalar::zero(); n];
for i in 0..n {
let mut sum = M::Scalar::zero();
for j in 0..i {
sum += lf[i * n + j] * z[j];
}
z[i] = (atb[i] - sum) / lf[i * n + i];
}
let mut x = alloc::vec![M::Scalar::zero(); n];
for i in (0..n).rev() {
let mut sum = M::Scalar::zero();
for j in (i + 1)..n {
sum += lf[j * n + i].conjugate() * x[j];
}
x[i] = (z[i] - sum) / lf[i * n + i].conjugate();
}
Ok(DenseVector::from_vec(x))
}