use crate::application::linalg::householder::{apply_left, apply_right, reflector};
use crate::domain::real::RealScalar;
use leto::{ArrayView2, LetoError, Result};
pub(super) fn reduce_to_hessenberg<T: RealScalar, const ACCUMULATE_Q: bool>(
matrix: &ArrayView2<'_, T>,
) -> Result<(Vec<T>, Vec<T>, usize)> {
let [n, _] = matrix.shape();
let mut h = if let Some(slice) = matrix.as_slice() {
slice.to_vec()
} else {
matrix.to_contiguous().into_storage().into_inner()
};
if !h.iter().all(|value| value.is_finite()) {
return Err(LetoError::StorageError {
reason: "Hessenberg input contains a non-finite value".to_string(),
});
}
let mut q = if ACCUMULATE_Q {
let mut q = vec![T::ZERO; n * n];
for i in 0..n {
q[i * n + i] = T::ONE;
}
q
} else {
Vec::new()
};
let mut alw: Vec<T> = Vec::with_capacity(n);
for k in 0..n.saturating_sub(2) {
let len = n - (k + 1);
let mut x_stack = [T::ZERO; 128];
let mut x_vec = Vec::new();
let x = if len <= 128 {
for i in 0..len {
x_stack[i] = h[(k + 1 + i) * n + k];
}
&x_stack[..len]
} else {
x_vec.reserve_exact(len);
for i in 0..len {
x_vec.push(h[(k + 1 + i) * n + k]);
}
&x_vec[..]
};
if let Some((refl, _alpha)) = reflector(x) {
apply_left(&refl, &mut h, n, k + 1, k, n, &mut alw); apply_right(&refl, &mut h, n, k + 1, 0, n); if ACCUMULATE_Q {
apply_right(&refl, &mut q, n, k + 1, 0, n); }
}
}
for i in 0..n {
for j in 0..i.saturating_sub(1) {
h[i * n + j] = T::ZERO;
}
}
Ok((h, q, n))
}