use super::standard_block::standardize_block;
use crate::domain::real::RealScalar;
#[inline]
fn at<T: Copy>(h: &[T], i: usize, j: usize, n: usize) -> T {
h[i * n + j]
}
fn apply_rotation<T: RealScalar>(h: &mut [T], z: &mut [T], p: usize, c: T, s: T, n: usize) {
for j in (p + 2)..n {
let t0 = at(h, p, j, n);
let t1 = at(h, p + 1, j, n);
h[p * n + j] = c.mul(t0).add(s.mul(t1));
h[(p + 1) * n + j] = s.neg().mul(t0).add(c.mul(t1));
}
for i in 0..p {
let t0 = at(h, i, p, n);
let t1 = at(h, i, p + 1, n);
h[i * n + p] = c.mul(t0).add(s.mul(t1));
h[i * n + (p + 1)] = s.neg().mul(t0).add(c.mul(t1));
}
for i in 0..n {
let z0 = at(z, i, p, n);
let z1 = at(z, i, p + 1, n);
z[i * n + p] = c.mul(z0).add(s.mul(z1));
z[i * n + (p + 1)] = s.neg().mul(z0).add(c.mul(z1));
}
}
pub(super) fn standardize<T: RealScalar>(h: &mut [T], z: &mut [T], n: usize) {
let mut p = 0usize;
while p + 1 < n {
if at(h, p + 1, p, n) == T::ZERO {
p += 1;
continue;
}
let block = standardize_block(
at(h, p, p, n),
at(h, p, p + 1, n),
at(h, p + 1, p, n),
at(h, p + 1, p + 1, n),
);
apply_rotation(h, z, p, block.cs, block.sn, n);
h[p * n + p] = block.a;
h[p * n + p + 1] = block.b;
h[(p + 1) * n + p] = block.c;
h[(p + 1) * n + p + 1] = block.d;
p += 2;
}
}