use super::at;
use super::shift::{first_column, shift_pair, Shift};
use crate::application::linalg::scaling::KernelWindow;
use crate::domain::real::RealScalar;
const SPAN_SIMD_MIN: usize = 32;
pub(super) fn step_window<T: RealScalar>() -> KernelWindow<T> {
KernelWindow::new(2, 2, 2)
}
pub(super) struct StepWorkspace<'a, T> {
pub(super) scratch: &'a mut [T],
pub(super) window: KernelWindow<T>,
}
struct StackReflector<T> {
v: [T; 3],
len: usize,
tau: T,
}
fn stack_reflector<T: RealScalar>(
x: &[T],
window: KernelWindow<T>,
) -> Option<(StackReflector<T>, T)> {
let len = x.len();
if !(2..=3).contains(&len) || x[1..].iter().all(|&xi| xi == T::ZERO) {
return None;
}
let exponent = window.exponent(x);
let mut v = [T::ZERO; 3];
for (slot, &xi) in v.iter_mut().zip(x) {
*slot = xi.scale_binary(-exponent);
}
let mut norm_sq = T::ZERO;
for &xi in &v[..len] {
norm_sq = norm_sq.add(xi.mul(xi));
}
let norm = norm_sq.sqrt();
let alpha = if v[0] < T::ZERO { norm } else { norm.neg() }; let head = v[0].sub(alpha); let tau = alpha.sub(v[0]).div(alpha);
v[0] = T::ONE;
for vi in &mut v[1..len] {
*vi = vi.div(head);
}
Some((StackReflector { v, len, tau }, alpha.scale_binary(exponent)))
}
#[allow(clippy::too_many_arguments)]
fn apply_left<T: RealScalar>(
h: &mut [T],
v: &[T],
beta: T,
k: usize,
n: usize,
c_lo: usize,
c_hi: usize,
scratch: &mut [T],
) {
if c_hi < c_lo {
return;
}
let span = c_hi - c_lo + 1;
if span < SPAN_SIMD_MIN {
for j in c_lo..=c_hi {
let mut acc = T::ZERO;
for (i, &vi) in v.iter().enumerate() {
acc = acc.add(vi.mul(h[(k + i) * n + j]));
}
acc = acc.mul(beta);
for (i, &vi) in v.iter().enumerate() {
let cell = (k + i) * n + j;
h[cell] = h[cell].sub(vi.mul(acc));
}
}
return;
}
let w = &mut scratch[..span];
w.fill(T::ZERO);
for (i, &vi) in v.iter().enumerate() {
let base = (k + i) * n + c_lo;
T::axpy_slice(vi, &h[base..base + span], w); }
for wj in w.iter_mut() {
*wj = beta.mul(*wj);
}
for (i, &vi) in v.iter().enumerate() {
let base = (k + i) * n + c_lo;
T::axpy_slice(T::ZERO.sub(vi), w, &mut h[base..base + span]); }
}
fn apply_right<T: RealScalar>(
h: &mut [T],
v: &[T],
beta: T,
k: usize,
n: usize,
r_lo: usize,
r_hi: usize,
) {
for i in r_lo..=r_hi {
let mut w = T::ZERO;
for (c, &vc) in v.iter().enumerate() {
w = w.add(h[i * n + (k + c)].mul(vc));
}
w = w.mul(beta);
for (c, &vc) in v.iter().enumerate() {
let cell = i * n + (k + c);
h[cell] = h[cell].sub(w.mul(vc));
}
}
}
pub(super) fn francis_step<T: RealScalar, const ACCUMULATE_Q: bool>(
h: &mut [T],
z: &mut [T],
lo: usize,
hi: usize,
n: usize,
shift: Shift,
workspace: &mut StepWorkspace<'_, T>,
) {
let window = workspace.window;
let scratch = &mut *workspace.scratch;
let dat1 = T::from_f64(0.75);
let dat2 = T::from_f64(-0.4375);
let (h11, h12, h21, h22) = match shift {
Shift::ExceptionalBottom => {
let s = at(h, hi, hi - 1, n)
.abs()
.add(at(h, hi - 1, hi - 2, n).abs());
let h11 = dat1.mul(s).add(at(h, hi, hi, n));
(h11, dat2.mul(s), s, h11)
}
Shift::ExceptionalTop => {
let s = at(h, lo + 1, lo, n)
.abs()
.add(at(h, lo + 2, lo + 1, n).abs());
let h11 = dat1.mul(s).add(at(h, lo, lo, n));
(h11, dat2.mul(s), s, h11)
}
Shift::Wilkinson => (
at(h, hi - 1, hi - 1, n),
at(h, hi - 1, hi, n),
at(h, hi, hi - 1, n),
at(h, hi, hi, n),
),
};
let (rt1r, rt1i, rt2r, rt2i) = shift_pair(h11, h12, h21, h22);
let ulp = crate::application::linalg::thresholds::machine_epsilon::<T>();
let mut m = hi - 2;
let (mut x, mut y, mut zz) = first_column(h, n, m, (rt1r, rt1i, rt2r, rt2i));
while m > lo {
let coupling = at(h, m, m - 1, n).abs().mul(y.abs().add(zz.abs()));
let local = ulp.mul(x.abs()).mul(
at(h, m - 1, m - 1, n)
.abs()
.add(at(h, m, m, n).abs())
.add(at(h, m + 1, m + 1, n).abs()),
);
if coupling <= local {
break;
}
m -= 1;
(x, y, zz) = first_column(h, n, m, (rt1r, rt1i, rt2r, rt2i));
}
for k in m..=(hi - 1) {
let len = if k < hi - 1 { 3 } else { 2 };
let refl_opt = if len == 3 {
let arr = [x, y, zz];
stack_reflector(&arr, window)
} else {
let arr = [x, y, T::ZERO];
stack_reflector(&arr[..2], window)
};
if let Some((refl, alpha)) = refl_opt {
let v_slice = &refl.v[..refl.len];
if k > m {
h[k * n + (k - 1)] = alpha;
h[(k + 1) * n + (k - 1)] = T::ZERO;
if refl.len == 3 {
h[(k + 2) * n + (k - 1)] = T::ZERO;
}
} else if m > lo {
h[k * n + (k - 1)] = h[k * n + (k - 1)].mul(T::ONE.sub(refl.tau));
}
let last_row = (k + refl.len).min(hi);
if ACCUMULATE_Q {
apply_left(h, v_slice, refl.tau, k, n, k, n - 1, scratch);
apply_right(h, v_slice, refl.tau, k, n, 0, last_row);
apply_right(z, v_slice, refl.tau, k, n, 0, n - 1);
} else {
apply_left(h, v_slice, refl.tau, k, n, k, hi, scratch);
apply_right(h, v_slice, refl.tau, k, n, lo, last_row);
}
}
if k + 1 < hi {
x = at(h, k + 1, k, n);
y = at(h, k + 2, k, n);
zz = if k + 3 <= hi {
at(h, k + 3, k, n)
} else {
T::ZERO
};
}
}
}