use crate::application::linalg::scaling::hypot;
use crate::application::linalg::thresholds::machine_epsilon;
use crate::domain::real::RealScalar;
use leto::{LetoError, Result};
const SWEEPS_PER_EIGENVALUE: usize = 30;
pub(super) fn sweep_budget(n: usize) -> usize {
SWEEPS_PER_EIGENVALUE.saturating_mul(n)
}
#[inline]
fn negligible<T: RealScalar>(value: T, scale: T) -> bool {
scale.add(value.abs()) == scale
}
#[inline]
fn rotate_adjacent_rows<T: RealScalar>(rows: &mut [T], n: usize, i: usize, c: T, s: T) {
let (upper, lower) = rows[i * n..(i + 2) * n].split_at_mut(n);
for (a, b) in upper.iter_mut().zip(lower.iter_mut()) {
let (va, vb) = (*a, *b);
*b = s.mul(va).add(c.mul(vb));
*a = c.mul(va).sub(s.mul(vb));
}
}
pub(super) fn diagonalize<T: RealScalar>(
diagonal: &mut [T],
off_diagonal: &mut [T],
vectors: &mut [T],
n: usize,
budget: usize,
) -> Result<()> {
let mut shift_total = T::ZERO;
let mut sweeps = 0_usize;
let two = T::from_count(2);
let norm_estimate = diagonal
.iter()
.zip(off_diagonal.iter())
.fold(T::ZERO, |acc, (&d, &e)| {
let local = d.abs().add(e.abs());
if local > acc {
local
} else {
acc
}
});
for l in 0..n {
let mut m = l;
while m + 1 < n && !negligible(off_diagonal[m], norm_estimate) {
m += 1;
}
if m > l {
loop {
sweeps += 1;
if sweeps > budget {
return Err(LetoError::ConvergenceError {
max_iters: budget,
residual: off_diagonal[l].abs().div(norm_estimate).to_f64(),
tol: machine_epsilon::<T>().to_f64() / 2.0,
});
}
let g = diagonal[l];
let mut p = diagonal[l + 1].sub(g).div(two.mul(off_diagonal[l]));
let mut r = hypot(p, T::ONE);
if p < T::ZERO {
r = r.neg();
}
diagonal[l] = off_diagonal[l].div(p.add(r));
diagonal[l + 1] = off_diagonal[l].mul(p.add(r));
let leading = diagonal[l + 1];
let mut h = g.sub(diagonal[l]);
for d in &mut diagonal[l + 2..] {
*d = d.sub(h);
}
shift_total = shift_total.add(h);
p = diagonal[m];
let (mut c, mut c2, mut c3) = (T::ONE, T::ONE, T::ONE);
let first_off = off_diagonal[l + 1];
let (mut s, mut s2) = (T::ZERO, T::ZERO);
for i in (l..m).rev() {
c3 = c2;
c2 = c;
s2 = s;
let g = c.mul(off_diagonal[i]);
h = c.mul(p);
r = hypot(p, off_diagonal[i]);
off_diagonal[i + 1] = s.mul(r);
s = off_diagonal[i].div(r);
c = p.div(r);
p = c.mul(diagonal[i]).sub(s.mul(g));
diagonal[i + 1] = h.add(s.mul(c.mul(g).add(s.mul(diagonal[i]))));
rotate_adjacent_rows(vectors, n, i, c, s);
}
p = s
.neg()
.mul(s2)
.mul(c3)
.mul(first_off)
.mul(off_diagonal[l])
.div(leading);
off_diagonal[l] = s.mul(p);
diagonal[l] = c.mul(p);
if negligible(off_diagonal[l], norm_estimate) {
break;
}
}
}
diagonal[l] = diagonal[l].add(shift_total);
off_diagonal[l] = T::ZERO;
}
sort_ascending(diagonal, vectors, n);
crate::application::linalg::transformation_counts::record_ql_sweeps(sweeps);
Ok(())
}
fn sort_ascending<T: RealScalar>(diagonal: &mut [T], vectors: &mut [T], n: usize) {
for i in 0..n {
let mut smallest = i;
for j in i + 1..n {
if diagonal[j] < diagonal[smallest] {
smallest = j;
}
}
if smallest != i {
diagonal.swap(i, smallest);
let (upper, lower) = vectors.split_at_mut(smallest * n);
upper[i * n..(i + 1) * n].swap_with_slice(&mut lower[..n]);
}
}
}