use crate::error::{BarracudaError, Result};
#[derive(Debug, Clone)]
pub struct QrDecomposition {
pub q: Vec<f64>,
pub r: Vec<f64>,
pub m: usize,
pub n: usize,
}
impl QrDecomposition {
pub fn solve_least_squares(&self, b: &[f64]) -> Result<Vec<f64>> {
if b.len() != self.m {
return Err(BarracudaError::InvalidInput {
message: format!("b has length {}, expected {}", b.len(), self.m),
});
}
let mut qtb = vec![0.0; self.m];
for (i, qtb_i) in qtb.iter_mut().enumerate() {
for (j, &b_j) in b.iter().enumerate() {
*qtb_i = self.q[j * self.m + i].mul_add(b_j, *qtb_i); }
}
let mut x = vec![0.0; self.n];
for i in (0..self.n).rev() {
let mut sum = qtb[i];
for (j, &x_j) in x.iter().enumerate().skip(i + 1) {
sum = self.r[i * self.n + j].mul_add(-x_j, sum);
}
let diag = self.r[i * self.n + i];
if diag.abs() < 1e-14 {
return Err(BarracudaError::Numerical {
message: "Matrix is rank deficient".to_string(),
});
}
x[i] = sum / diag;
}
Ok(x)
}
}
pub fn qr_decompose(a: &[f64], m: usize, n: usize) -> Result<QrDecomposition> {
if a.len() != m * n {
return Err(BarracudaError::InvalidInput {
message: format!("Matrix has {} elements, expected {}×{}", a.len(), m, n),
});
}
if m == 0 || n == 0 {
return Err(BarracudaError::InvalidInput {
message: "Matrix dimensions must be positive".to_string(),
});
}
if m < n {
return Err(BarracudaError::InvalidInput {
message: format!("QR requires m ≥ n, got m={m}, n={n}"),
});
}
let mut q = vec![0.0_f64; m * m];
for i in 0..m {
q[i * m + i] = 1.0;
}
let mut r = vec![0.0; m * n];
for i in 0..m {
for j in 0..n {
r[i * n + j] = a[i * n + j];
}
}
for k in 0..n {
let mut norm_sq = 0.0;
for i in k..m {
norm_sq = r[i * n + k].mul_add(r[i * n + k], norm_sq);
}
let norm = norm_sq.sqrt();
if norm < 1e-14 {
continue; }
let sign = if r[k * n + k] >= 0.0 { 1.0 } else { -1.0 };
let alpha = sign * norm;
let mut v = vec![0.0; m];
for i in k..m {
v[i] = r[i * n + k];
}
v[k] += alpha;
let mut v_norm_sq = 0.0;
for &vi in &v[k..] {
v_norm_sq = vi.mul_add(vi, v_norm_sq);
}
if v_norm_sq < 1e-28 {
continue;
}
let beta = 2.0 / v_norm_sq;
for j in k..n {
let mut dot = 0.0;
for i in k..m {
dot = v[i].mul_add(r[i * n + j], dot);
}
dot *= beta;
for i in k..m {
r[i * n + j] -= dot * v[i];
}
}
for i in 0..m {
let mut dot = 0.0_f64;
for j in k..m {
dot = q[i * m + j].mul_add(v[j], dot);
}
dot *= beta;
for j in k..m {
q[i * m + j] = dot.mul_add(-v[j], q[i * m + j]);
}
}
}
for i in 0..m {
for j in 0..n {
if i > j || r[i * n + j].abs() < 1e-14 {
r[i * n + j] = 0.0;
}
}
}
Ok(QrDecomposition { q, r, m, n })
}
pub fn qr_least_squares(a: &[f64], m: usize, n: usize, b: &[f64]) -> Result<Vec<f64>> {
let qr = qr_decompose(a, m, n)?;
qr.solve_least_squares(b)
}
#[cfg(test)]
mod tests {
use super::*;
fn approx_eq(a: f64, b: f64, tol: f64) -> bool {
(a - b).abs() < tol
}
#[test]
fn test_qr_2x2() {
let a = vec![3.0, 4.0, 4.0, 3.0];
let qr = qr_decompose(&a, 2, 2).unwrap();
for i in 0..2 {
for j in 0..2 {
let mut dot = 0.0;
for k in 0..2 {
dot = qr.q[k * 2 + i].mul_add(qr.q[k * 2 + j], dot);
}
let expected = if i == j { 1.0 } else { 0.0 };
assert!(approx_eq(dot, expected, 1e-10), "Q^TQ[{i},{j}] = {dot}");
}
}
for i in 0..2 {
for j in 0..2 {
let mut val = 0.0;
for k in 0..2 {
val = qr.q[i * 2 + k].mul_add(qr.r[k * 2 + j], val);
}
assert!(
approx_eq(val, a[i * 2 + j], 1e-10),
"QR[{},{}] = {}, A = {}",
i,
j,
val,
a[i * 2 + j]
);
}
}
}
#[test]
fn test_qr_3x2_least_squares() {
let a = vec![1.0, 1.0, 1.0, 2.0, 1.0, 3.0];
let b = vec![1.0, 2.0, 2.0];
let x = qr_least_squares(&a, 3, 2, &b).unwrap();
let ata = [3.0_f64, 6.0, 6.0, 14.0];
let atb = [5.0, 11.0];
let lhs0 = ata[0].mul_add(x[0], ata[1] * x[1]);
let lhs1 = ata[2].mul_add(x[0], ata[3] * x[1]);
assert!(
approx_eq(lhs0, atb[0], 1e-10),
"normal eq 0: {} vs {}",
lhs0,
atb[0]
);
assert!(
approx_eq(lhs1, atb[1], 1e-10),
"normal eq 1: {} vs {}",
lhs1,
atb[1]
);
}
#[test]
fn test_qr_identity() {
let a = vec![1.0, 0.0, 0.0, 1.0];
let qr = qr_decompose(&a, 2, 2).unwrap();
for i in 0..2 {
for j in 0..2 {
let mut val = 0.0;
for k in 0..2 {
val = qr.q[i * 2 + k].mul_add(qr.r[k * 2 + j], val);
}
assert!(approx_eq(val, a[i * 2 + j], 1e-10));
}
}
}
#[test]
fn test_qr_tall_matrix() {
let a = vec![1.0, 2.0, 3.0];
let qr = qr_decompose(&a, 3, 1).unwrap();
let norm_a = (1.0f64 + 4.0 + 9.0).sqrt();
assert!(approx_eq(qr.r[0].abs(), norm_a, 1e-10));
for i in 0..3 {
for j in 0..3 {
let mut dot = 0.0;
for k in 0..3 {
dot = qr.q[k * 3 + i].mul_add(qr.q[k * 3 + j], dot);
}
let expected = if i == j { 1.0 } else { 0.0 };
assert!(approx_eq(dot, expected, 1e-10));
}
}
}
#[test]
fn test_qr_solve_square() {
let a = vec![2.0, 1.0, 1.0, 3.0];
let b = vec![5.0, 7.0];
let x = qr_least_squares(&a, 2, 2, &b).unwrap();
let ax0 = 2.0f64.mul_add(x[0], 1.0 * x[1]);
let ax1 = 1.0f64.mul_add(x[0], 3.0 * x[1]);
assert!(approx_eq(ax0, b[0], 1e-10));
assert!(approx_eq(ax1, b[1], 1e-10));
}
#[test]
fn test_qr_errors() {
assert!(qr_decompose(&[1.0, 2.0, 3.0, 4.0, 5.0, 6.0], 2, 3).is_err());
assert!(qr_decompose(&[1.0, 2.0, 3.0], 2, 2).is_err());
assert!(qr_decompose(&[], 0, 0).is_err());
}
}