use crate::application::linalg::householder::reflect_in_place;
use crate::domain::real::RealScalar;
pub(super) fn tridiagonalize<T: RealScalar>(
a: &mut [T],
n: usize,
diagonal: &mut [T],
off_diagonal: &mut [T],
scales: &mut [T],
scratch: &mut Vec<T>,
) {
scratch.clear();
scratch.resize(n, T::ZERO);
let half = T::ONE.div(T::from_count(2));
for k in 0..n.saturating_sub(2) {
diagonal[k] = a[k * n + k];
let (head, trailing) = a.split_at_mut((k + 1) * n);
let v = &mut head[k * n + k + 1..];
let Some((beta, alpha)) = reflect_in_place(v) else {
off_diagonal[k] = T::ZERO;
scales[k] = T::ZERO;
continue;
};
off_diagonal[k] = alpha;
scales[k] = beta;
let v = &*v;
let width = n - k - 1;
let p = &mut scratch[..width];
for (pi, row) in p.iter_mut().zip(trailing.chunks_exact(n)) {
*pi = beta.mul(T::dot_slice(&row[k + 1..], v));
}
let correction = half.mul(beta).mul(T::dot_slice(p, v));
for (pi, &vi) in p.iter_mut().zip(v) {
*pi = pi.sub(correction.mul(vi));
}
let w = &*p;
for ((&vi, &wi), row) in v.iter().zip(w).zip(trailing.chunks_exact_mut(n)) {
for ((entry, &wj), &vj) in row[k + 1..].iter_mut().zip(w).zip(v) {
*entry = entry.sub(vi.mul(wj).add(wi.mul(vj)));
}
}
}
match n {
0 => {}
1 => {
diagonal[0] = a[0];
off_diagonal[0] = T::ZERO;
}
_ => {
diagonal[n - 2] = a[(n - 2) * n + n - 2];
off_diagonal[n - 2] = a[(n - 2) * n + n - 1];
diagonal[n - 1] = a[n * n - 1];
off_diagonal[n - 1] = T::ZERO;
}
}
}
pub(super) fn accumulate_transposed_factor<T: RealScalar>(
a: &[T],
n: usize,
scales: &[T],
transposed: &mut [T],
) {
transposed.fill(T::ZERO);
for i in 0..n {
transposed[i * n + i] = T::ONE;
}
for k in (0..n.saturating_sub(2)).rev() {
let beta = scales[k];
if beta == T::ZERO {
continue;
}
let v = &a[k * n + k + 1..(k + 1) * n];
for row in transposed[(k + 1) * n..].chunks_exact_mut(n) {
let window = &mut row[k + 1..];
let scale = beta.mul(T::dot_slice(window, v));
T::axpy_slice(T::ZERO.sub(scale), v, window);
}
}
}