use dyn_stack::{MemBuffer, MemStack};
use faer::linalg::cholesky::ldlt;
use faer::linalg::triangular_solve::{
solve_unit_lower_triangular_in_place, solve_unit_upper_triangular_in_place,
};
use crate::kernel::KernelScalar;
use faer::{Mat, MatMut, MatRef, Par};
use wide::f64x4;
const PANEL: usize = 8;
pub(crate) fn solve_unit_lower_rows_f64(lt: &Mat<f64>, w: &mut [f64]) {
let n = w.len();
let mut i = 0;
while i + PANEL <= n {
let rows: [&[f64]; PANEL] = std::array::from_fn(|k| <.col_as_slice(i + k)[..i]);
let dots = dot_rows(rows, &w[..i]);
let mut x: [f64; PANEL] = std::array::from_fn(|k| w[i + k] - dots[k]);
for a in 1..PANEL {
let row = <.col_as_slice(i + a)[i..i + a];
for (b, l) in row.iter().enumerate() {
x[a] -= l * x[b];
}
}
w[i..i + PANEL].copy_from_slice(&x);
i += PANEL;
}
for i in i.max(1)..n {
let (head, rest) = w.split_at_mut(i);
rest[0] -= dot_f64(<.col_as_slice(i)[..i], head);
}
}
fn dot_rows(rows: [&[f64]; PANEL], b: &[f64]) -> [f64; PANEL] {
let len = b.len();
let body = len - len % 4;
let mut acc = [f64x4::ZERO; PANEL];
for at in (0..body).step_by(4) {
let vb = f64x4::from([b[at], b[at + 1], b[at + 2], b[at + 3]]);
for (slot, row) in acc.iter_mut().zip(&rows) {
let va = f64x4::from([row[at], row[at + 1], row[at + 2], row[at + 3]]);
*slot = va.mul_add(vb, *slot);
}
}
std::array::from_fn(|k| {
let tail: f64 = rows[k][body..len]
.iter()
.zip(&b[body..len])
.map(|(x, y)| x * y)
.sum();
acc[k].reduce_add() + tail
})
}
fn dot_f64(a: &[f64], b: &[f64]) -> f64 {
const LANES: usize = 4;
const STEP: usize = 4 * LANES;
let mut acc = [f64x4::ZERO; 4];
let body = a.len() - a.len() % STEP;
for (ca, cb) in a[..body]
.as_chunks::<STEP>()
.0
.iter()
.zip(b[..body].as_chunks::<STEP>().0)
{
for (k, slot) in acc.iter_mut().enumerate() {
let at = k * LANES;
let va = f64x4::from([ca[at], ca[at + 1], ca[at + 2], ca[at + 3]]);
let vb = f64x4::from([cb[at], cb[at + 1], cb[at + 2], cb[at + 3]]);
*slot = va.mul_add(vb, *slot);
}
}
let total = (acc[0] + acc[1]) + (acc[2] + acc[3]);
let mut sum = total.reduce_add();
for (x, y) in a[body..].iter().zip(&b[body..]) {
sum += x * y;
}
sum
}
pub(crate) fn solve_unit_lower_rows_f32(lt: &Mat<f32>, w: &mut [f32]) {
let mut solved: Vec<f64> = w.iter().map(|&v| f64::from(v)).collect();
for i in 1..solved.len() {
let (head, rest) = solved.split_at_mut(i);
let mut sum = rest[0];
for (l, x) in lt.col_as_slice(i)[..i].iter().zip(head.iter()) {
sum -= f64::from(*l) * *x;
}
rest[0] = sum;
}
for (out, v) in w.iter_mut().zip(&solved) {
*out = *v as f32;
}
}
pub(crate) fn ldlt_delete_faer(
ld: MatMut<'_, f64>,
index: usize,
_n: usize,
scratch: &mut MemBuffer,
) {
let mut indices = [index];
let stack = MemStack::new(scratch);
ldlt::update::delete_rows_and_cols_clobber(ld, &mut indices, Par::Seq, stack);
}
pub(crate) fn ldlt_delete_via_f64(mut ld: MatMut<'_, f32>, index: usize, n: usize) {
let mut ld64 = Mat::<f64>::zeros(n, n);
for col in 0..n {
for row in col..n {
ld64[(row, col)] = f64::from(ld[(row, col)]);
}
}
let scratch_req = ldlt::update::delete_rows_and_cols_clobber_scratch::<f64>(n.max(1), 1);
let mut scratch = MemBuffer::new(scratch_req);
let stack = MemStack::new(&mut scratch);
let mut indices = [index];
ldlt::update::delete_rows_and_cols_clobber(ld64.as_mut(), &mut indices, Par::Seq, stack);
for col in 0..n {
for row in col..n {
ld[(row, col)] = ld64[(row, col)] as f32;
}
}
}
pub(crate) fn solve_ldlt_in_place<T: KernelScalar>(
ld: MatRef<'_, T>,
rhs: MatMut<'_, T>,
n: usize,
) {
T::solve_ldlt_in_place(ld, rhs, n);
}
pub(crate) fn solve_ldlt_f64_accum(ld: MatRef<'_, f32>, mut rhs: MatMut<'_, f32>, n: usize) {
let mut x = vec![0.0f64; n];
for col in 0..rhs.ncols() {
for (i, v) in x.iter_mut().enumerate() {
*v = f64::from(rhs[(i, col)]);
}
for i in 1..n {
let mut sum = x[i];
for j in 0..i {
sum -= f64::from(ld[(i, j)]) * x[j];
}
x[i] = sum;
}
for (i, v) in x.iter_mut().enumerate() {
*v /= f64::from(ld[(i, i)]);
}
for i in (0..n).rev() {
let mut sum = x[i];
for j in (i + 1)..n {
sum -= f64::from(ld[(j, i)]) * x[j];
}
x[i] = sum;
}
for (i, v) in x.iter().enumerate() {
rhs[(i, col)] = *v as f32;
}
}
}
pub(crate) fn solve_ldlt_faer<T: KernelScalar>(
ld: MatRef<'_, T>,
mut rhs: MatMut<'_, T>,
n: usize,
) {
if n == 0 {
return;
}
let ld_n = ld.submatrix(0, 0, n, n);
solve_unit_lower_triangular_in_place(ld_n, rhs.as_mut(), Par::Seq);
for i in 0..n {
rhs[(i, 0)] /= ld[(i, i)];
}
solve_unit_upper_triangular_in_place(ld_n.transpose(), rhs, Par::Seq);
}
pub(crate) fn apply_ldlt_inv_l<T: KernelScalar>(ld: MatRef<'_, T>, rhs: MatMut<'_, T>, n: usize) {
if n == 0 {
return;
}
let ld_n = ld.submatrix(0, 0, n, n);
solve_unit_lower_triangular_in_place(ld_n, rhs, Par::Seq);
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn panel_solve_matches_faer_across_panel_edges() {
for n in [1usize, 2, 7, 8, 9, 16, 17, 100] {
let lt = Mat::<f64>::from_fn(n, n, |i, j| {
if i < j {
(((i * 131 + j * 17) % 97) as f64 - 48.0) * 1e-3
} else if i == j {
2.0
} else {
0.0
}
});
let b: Vec<f64> = (0..n).map(|i| ((i * 7) % 13) as f64 - 6.0).collect();
let mut got = b.clone();
solve_unit_lower_rows_f64(<, &mut got);
let mut want = b.clone();
solve_unit_lower_triangular_in_place(
lt.as_ref().transpose(),
MatMut::from_column_major_slice_mut(&mut want, n, 1),
Par::Seq,
);
for (g, w) in got.iter().zip(&want) {
assert!(
(g - w).abs() <= 1e-12 * (1.0 + w.abs()),
"n={n}: {g} vs {w}"
);
}
}
}
}