use super::QrDecomposition;
use crate::application::linalg::reflector_block::apply_block_left;
use crate::domain::real::RealScalar;
use leto::{ArrayView2, LetoError, Result};
const BLOCK_WIDTH: usize = 32;
const BLOCK_MIN_ROWS: usize = 256;
pub fn qr_decompose<T: RealScalar>(matrix: &ArrayView2<'_, T>) -> Result<QrDecomposition<T>> {
let [rows, cols] = matrix.shape();
if rows < cols {
return Err(LetoError::ShapeMismatch {
lhs: vec![rows, cols],
rhs: vec![cols, cols],
});
}
let mut a = if let Some(slice) = matrix.as_slice() {
slice.to_vec()
} else {
matrix.to_contiguous().into_storage().into_inner()
};
if !a.iter().all(|value| value.is_finite()) {
return Err(LetoError::StorageError {
reason: "QR input contains a non-finite value".to_string(),
});
}
let mut heads_stack = [T::ZERO; 128];
let mut heads_vec = Vec::new();
let heads = if cols <= 128 {
&mut heads_stack[..cols]
} else {
heads_vec.resize(cols, T::ZERO);
&mut heads_vec[..]
};
let mut betas_stack = [T::ZERO; 128];
let mut betas_vec = Vec::new();
let betas = if cols <= 128 {
&mut betas_stack[..cols]
} else {
betas_vec.resize(cols, T::ZERO);
&mut betas_vec[..]
};
let mut w_stack = [T::ZERO; 128];
let mut w_vec = Vec::new();
let w = if cols <= 128 {
&mut w_stack[..cols]
} else {
w_vec.resize(cols, T::ZERO);
&mut w_vec[..]
};
let panel_width = if rows >= BLOCK_MIN_ROWS {
BLOCK_WIDTH
} else {
cols.max(1)
};
let mut panel_start = 0;
let mut v = Vec::new();
let mut panel_betas = Vec::new();
let mut c_block = Vec::new();
while panel_start < cols {
let panel_end = (panel_start + panel_width).min(cols);
for k in panel_start..panel_end {
let mut norm_sq = T::ZERO;
for r in k..rows {
let x = a[r * cols + k];
norm_sq = norm_sq.add(x.mul(x));
}
let norm = norm_sq.sqrt();
if norm == T::ZERO {
return Err(LetoError::StorageError {
reason: format!("QR pivot column {k} has zero norm: matrix is rank-deficient"),
});
}
let pivot = a[k * cols + k];
let alpha = if pivot > T::ZERO {
T::ZERO.sub(norm)
} else {
norm
};
let head = pivot.sub(alpha);
let mut v_norm_sq = head.mul(head);
for r in (k + 1)..rows {
let x = a[r * cols + k];
v_norm_sq = v_norm_sq.add(x.mul(x));
}
let beta = T::ONE.add(T::ONE).div(v_norm_sq);
let trail = panel_end - (k + 1);
let w = &mut w[..trail];
let row_k = &a[k * cols + (k + 1)..k * cols + panel_end];
for (wc, &akc) in w.iter_mut().zip(row_k) {
*wc = head.mul(akc);
}
for r in (k + 1)..rows {
let vr = a[r * cols + k];
let row_r = &a[r * cols + (k + 1)..r * cols + panel_end];
for (wc, &arc) in w.iter_mut().zip(row_r) {
*wc = wc.add(vr.mul(arc));
}
}
for wc in w.iter_mut() {
*wc = beta.mul(*wc);
}
let row_k = &mut a[k * cols + (k + 1)..k * cols + panel_end];
for (akc, &wc) in row_k.iter_mut().zip(w.iter()) {
*akc = akc.sub(wc.mul(head));
}
for r in (k + 1)..rows {
let vr = a[r * cols + k];
let row_r = &mut a[r * cols + (k + 1)..r * cols + panel_end];
for (arc, &wc) in row_r.iter_mut().zip(w.iter()) {
*arc = arc.sub(wc.mul(vr));
}
}
a[k * cols + k] = alpha; heads[k] = head;
betas[k] = beta;
}
let nb = panel_end - panel_start;
let ntrail = cols - panel_end;
if ntrail > 0 {
let m_sub = rows - panel_start;
v.clear();
v.resize(m_sub * nb, T::ZERO);
panel_betas.clear();
panel_betas.resize(nb, T::ZERO);
for j in 0..nb {
let col = panel_start + j;
panel_betas[j] = betas[col];
v[j * nb + j] = heads[col]; for r in (col + 1)..rows {
v[(r - panel_start) * nb + j] = a[r * cols + col];
}
}
c_block.clear();
c_block.resize(m_sub * ntrail, T::ZERO);
for r in panel_start..rows {
for (jc, slot) in c_block
[(r - panel_start) * ntrail..(r - panel_start) * ntrail + ntrail]
.iter_mut()
.enumerate()
{
*slot = a[r * cols + panel_end + jc];
}
}
apply_block_left(&v, &panel_betas, &mut c_block, m_sub, ntrail, nb);
for r in panel_start..rows {
for jc in 0..ntrail {
a[r * cols + panel_end + jc] = c_block[(r - panel_start) * ntrail + jc];
}
}
}
panel_start = panel_end;
}
Ok(QrDecomposition {
packed: a,
heads: heads.to_vec(),
betas: betas.to_vec(),
rows,
cols,
})
}