use super::super::triangular_pair::triangular_svd;
use super::chase::{Chase, Direction, Down, Oriented, Up};
use super::deflation::{
chase_negligible_diagonal_column, chase_negligible_diagonal_row, diagonal_is_negligible,
Deflation,
};
use super::rotation::{givens, TransposedFactors};
use super::zero_shift::zero_shift_sweep;
use crate::application::linalg::scaling::KernelWindow;
use crate::application::linalg::thresholds;
use crate::domain::real::RealScalar;
use leto::Result;
const MAX_ITER: usize = 4000;
fn wilkinson_shift<T: RealScalar>(dq: T, dq1: T, eq1: T, eq2: T) -> T {
let t11 = dq1.mul(dq1).add(eq2.mul(eq2));
let t22 = dq.mul(dq).add(eq1.mul(eq1));
let t12 = dq1.mul(eq1);
let half = T::from_f64(0.5);
let delta = t11.sub(t22).mul(half);
let denom = delta.abs().add(delta.mul(delta).add(t12.mul(t12)).sqrt());
if denom == T::ZERO {
return t22;
}
let sign = if delta < T::ZERO {
T::ONE.neg()
} else {
T::ONE
};
t22.sub(sign.mul(t12.mul(t12)).div(denom))
}
#[derive(Clone, Copy)]
pub(super) struct SweepWindows<T> {
pub(super) rotation: KernelWindow<T>,
shift: KernelWindow<T>,
}
impl<T: RealScalar> SweepWindows<T> {
pub(super) fn new() -> Self {
Self {
rotation: KernelWindow::new(2, 2, 1),
shift: KernelWindow::new(4, 4, 1),
}
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
pub(super) struct IterationCounts {
pub(super) sweeps: usize,
pub(super) chases: usize,
}
impl IterationCounts {
pub(super) fn total(self) -> usize {
self.sweeps + self.chases
}
}
pub(super) fn qr_iterate<T: RealScalar, const VEC: bool>(
d: &mut [T],
e: &mut [T],
k: usize,
factors: &mut TransposedFactors<'_, T>,
) -> Result<IterationCounts> {
if k <= 1 {
return Ok(IterationCounts::default());
}
let windows = SweepWindows::new();
let deflation = Deflation::new(d, e, k);
let mut q = k - 1;
let mut counts = IterationCounts::default();
let mut chased: Option<(usize, usize, Direction)> = None;
loop {
while q > 0 && e[q - 1] == T::ZERO {
q -= 1;
}
if q == 0 {
return Ok(counts);
}
let mut p = q;
while p > 0 {
if e[p - 1].abs() <= deflation.thresh {
e[p - 1] = T::ZERO;
break;
}
p -= 1;
}
if p == q {
continue;
}
if q == p + 1 {
let pair = triangular_svd(d[p], e[p], d[q]);
d[p] = pair.ssmax;
e[p] = T::ZERO;
d[q] = pair.ssmin;
if VEC {
factors.rotate_right(p, q, pair.csr, pair.snr);
factors.rotate_left(p, q, pair.csl, pair.snl);
}
continue;
}
let direction = match chased {
Some((old_p, old_q, direction)) if p <= old_q && q >= old_p => direction,
_ => Direction::of_block(d, p, q),
};
let split = match direction {
Direction::Down => deflation.split(&Oriented::<T, Down>::new(d, e, p, q)),
Direction::Up => deflation.split(&Oriented::<T, Up>::new(d, e, p, q)),
};
if let Some(i) = split {
e[i] = T::ZERO;
continue;
}
chased = Some((p, q, direction));
if counts.total() >= MAX_ITER {
return Err(leto::LetoError::StorageError {
reason: "bidiagonal SVD QR failed to converge".to_string(),
});
}
if let Some(i) = (p..=q).find(|&i| diagonal_is_negligible(d, e, i, p, q)) {
if i < q {
chase_negligible_diagonal_row::<T, VEC>(d, e, i, q, factors, windows.rotation);
} else {
chase_negligible_diagonal_column::<T, VEC>(d, e, p, q, factors, windows.rotation);
}
counts.chases += 1;
continue;
}
match direction {
Direction::Down => sweep::<T, VEC, Down>(d, e, p, q, deflation, factors, windows),
Direction::Up => sweep::<T, VEC, Up>(d, e, p, q, deflation, factors, windows),
}
counts.sweeps += 1;
}
}
fn sweep<T: RealScalar, const VEC: bool, C: Chase>(
d: &mut [T],
e: &mut [T],
p: usize,
q: usize,
deflation: Deflation<T>,
factors: &mut TransposedFactors<'_, T>,
windows: SweepWindows<T>,
) {
let ruins = deflation.shift_ruins_accuracy(&Oriented::<T, C>::new(d, e, p, q));
if ruins {
zero_shift_sweep::<T, VEC, C>(d, e, p, q, factors, windows.rotation);
} else {
qr_step::<T, VEC, C>(d, e, p, q, factors, windows);
}
}
pub(super) fn qr_step<T: RealScalar, const VEC: bool, C: Chase>(
d: &mut [T],
e: &mut [T],
p: usize,
q: usize,
factors: &mut TransposedFactors<'_, T>,
windows: SweepWindows<T>,
) {
let mut block = Oriented::<T, C>::new(d, e, p, q);
let eq2 = if q >= p + 2 { block.e(q - 2) } else { T::ZERO };
let local = [
block.d(q),
block.d(q - 1),
block.e(q - 1),
eq2,
block.d(p),
block.e(p),
];
let exponent = windows.shift.exponent(&local);
let [dq, dq1, eq1, eq2, dp, ep] = local.map(|v| v.scale_binary(-exponent));
let mu = wilkinson_shift(dq, dq1, eq1, eq2);
if mu.abs() < thresholds::machine_epsilon::<T>().mul(dp.mul(dp)) {
zero_shift_sweep::<T, VEC, C>(d, e, p, q, factors, windows.rotation);
return;
}
let mut y = dp.mul(dp).sub(mu);
let mut z = dp.mul(ep);
for k in p..q {
let (c, s, r_right) = givens(y, z, windows.rotation);
if VEC {
block.rotate_columns(factors, k, k + 1, (c, s));
}
if k > p {
block.set_e(k - 1, r_right);
}
let (dk, ek, dk1) = (block.d(k), block.e(k), block.d(k + 1));
let f = c.mul(dk).add(s.mul(ek));
let ek = c.mul(ek).sub(s.mul(dk));
let bulge_col = s.mul(dk1);
let dk1 = c.mul(dk1);
let (c, s, r_left) = givens(f, bulge_col, windows.rotation);
if VEC {
block.rotate_rows(factors, k, k + 1, (c, s));
}
block.set_d(k, r_left);
block.set_e(k, c.mul(ek).add(s.mul(dk1)));
block.set_d(k + 1, c.mul(dk1).sub(s.mul(ek)));
if k + 1 < q {
let ek1 = block.e(k + 1);
y = block.e(k);
z = s.mul(ek1);
block.set_e(k + 1, c.mul(ek1));
}
}
}