use crate::algebra::abstr::AbsDiffEq;
use crate::algebra::abstr::Field;
use crate::algebra::abstr::Scalar;
use crate::algebra::linear::matrix::substitute::SubstituteForward;
use crate::algebra::linear::matrix::{General, UnitLowerTriangular};
use crate::algebra::linear::vector::Vector;

impl<T> SubstituteForward<Vector<T>> for UnitLowerTriangular<T>
where
    T: Field + Scalar + AbsDiffEq,
{
    fn substitute_forward(&self, mut a: Vector<T>) -> Result<Vector<T>, ()> {
        let rows = self.matrix.nrows();

        for k in 0..rows {
            for l in 0..k {
                let b_l = a[l];
                a[k] -= self[[k, l]] * b_l;
            }
        }

        Ok(a)
    }
}

impl<T> SubstituteForward<General<T>> for UnitLowerTriangular<T>
where
    T: Field + Scalar + AbsDiffEq,
{
    fn substitute_forward(&self, mut a: General<T>) -> Result<General<T>, ()> {
        let (rows, cols) = a.dim();

        for j in 0..cols {
            for k in 0..rows {
                let a_kj = a[[k, j]];
                for l in k + 1..rows {
                    a[[l, j]] -= self[[l, k]] * a_kj;
                }
            }
        }

        Ok(a)
    }
}
