use xingo_sparse::{DenseBlock, DenseError, DenseKernel, NaiveDense, Real};
use crate::real::{is_exact_zero, le_exact, lt};
use crate::small::{Rot, axpy_sub, check_finite, check_len, dot, givens, norm2};
use crate::{MatMut, MatRef, StatlaError};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct CholOptions {
pub pivot_tol: f64,
pub downdate_tol: f64,
}
impl Default for CholOptions {
fn default() -> Self {
Self {
pivot_tol: 0.0,
downdate_tol: 0.0,
}
}
}
impl CholOptions {
fn validate(&self) -> Result<(), StatlaError> {
let ok = |v: f64| v.is_finite() && le_exact(0.0, v);
if !ok(self.pivot_tol) {
return Err(StatlaError::InvalidArgument {
what: "CholOptions::pivot_tol must be finite and >= 0",
});
}
if !ok(self.downdate_tol) {
return Err(StatlaError::InvalidArgument {
what: "CholOptions::downdate_tol must be finite and >= 0",
});
}
Ok(())
}
}
#[derive(Clone, Debug)]
pub struct Cholesky<T: Real = f64, K: DenseKernel<T> = NaiveDense> {
r: DenseBlock<T>,
#[allow(dead_code)]
kernel: K,
opts: CholOptions,
}
#[derive(Clone, Copy, Debug, PartialEq)]
#[non_exhaustive]
pub struct DowndateInfo {
pub margin: f64,
}
impl<T: Real> Cholesky<T, NaiveDense> {
pub fn factor(a: MatRef<'_, T>, opts: &CholOptions) -> Result<Self, StatlaError> {
Self::factor_with(a, NaiveDense, opts)
}
}
impl<T: Real, K: DenseKernel<T>> Cholesky<T, K> {
pub fn factor_with(
a: MatRef<'_, T>,
kernel: K,
opts: &CholOptions,
) -> Result<Self, StatlaError> {
opts.validate()?;
let n = a.rows();
if a.cols() != n {
return Err(StatlaError::InvalidArgument {
what: "Cholesky::factor: matrix not square",
});
}
let mut w = DenseBlock::zeros(n, n);
for j in 0..n {
let col = a.col(j).unwrap_or(&[]);
for (i, &v) in col.iter().enumerate().take(j + 1) {
if !v.is_finite() {
return Err(StatlaError::NonFinite {
what: "a",
index: i + j * n,
});
}
w.data[j + i * n] = v;
}
}
kernel.potrf(&mut w).map_err(|e| match e {
DenseError::NotPositiveDefinite { position } => {
StatlaError::NotPositiveDefinite { column: position }
}
DenseError::Dimension => StatlaError::InvalidArgument {
what: "Cholesky::factor: the kernel rejected the dimensions",
},
})?;
let tol = T::from_f64(opts.pivot_tol);
for j in 0..n {
let l = w.data[j + j * n];
let ajj = a.get(j, j).unwrap_or(T::ZERO);
if !l.is_finite() || !lt(T::ZERO, l) || le_exact(l * l, tol * ajj) {
return Err(StatlaError::NotPositiveDefinite { column: j });
}
}
let mut r = DenseBlock::zeros(n, n);
for j in 0..n {
for i in 0..=j {
r.data[i + j * n] = w.data[j + i * n];
}
}
let c = Self {
r,
kernel,
opts: *opts,
};
debug_assert_eq!(c.check_invariants(), Ok(()), "(H1) after factor");
Ok(c)
}
pub fn from_upper(
r: DenseBlock<T>,
kernel: K,
opts: &CholOptions,
) -> Result<Self, StatlaError> {
opts.validate()?;
let n = r.rows;
if r.cols != n || r.data.len() != n.saturating_mul(n) {
return Err(StatlaError::InvalidArgument {
what: "Cholesky::from_upper: not a square block",
});
}
check_finite("r", &r.data)?;
let c = Self {
r,
kernel,
opts: *opts,
};
c.check_invariants()
.map_err(|what| StatlaError::InvalidArgument { what })?;
Ok(c)
}
#[must_use]
pub fn n(&self) -> usize {
self.r.rows
}
#[must_use]
pub fn upper(&self) -> MatRef<'_, T> {
MatRef::from_block(&self.r)
}
#[must_use]
pub fn into_upper(self) -> DenseBlock<T> {
self.r
}
pub fn update(&mut self, x: &[T]) -> Result<(), StatlaError> {
let n = self.n();
check_len("x", n, x.len())?;
check_finite("x", x)?;
let mut rots = vec![Rot::IDENTITY; n];
let r = &mut self.r.data;
for j in 0..n {
let mut xi = x[j];
let col = &mut r[j * n..(j + 1) * n];
for (rij, g) in col[..j].iter_mut().zip(&rots[..j]) {
let (p, q) = g.apply(*rij, xi);
*rij = p;
xi = q;
}
let (g, rho) = givens(col[j], xi);
rots[j] = g;
col[j] = rho;
}
debug_assert_eq!(self.check_invariants(), Ok(()), "(H1) after update");
Ok(())
}
pub fn downdate(&mut self, x: &[T]) -> Result<DowndateInfo, StatlaError> {
let n = self.n();
check_len("x", n, x.len())?;
check_finite("x", x)?;
let mut a = x.to_vec();
self.forward(&mut a);
let nu = norm2(&a);
let alpha2 = T::ONE - nu * nu;
if !alpha2.is_finite() {
return Err(StatlaError::DowndateIndefinite {
margin: f64::NEG_INFINITY,
});
}
let margin = alpha2.to_f64();
if le_exact(alpha2, T::from_f64(self.opts.downdate_tol)) {
return Err(StatlaError::DowndateIndefinite { margin });
}
let mut alpha = alpha2.sqrt();
let mut rots = vec![Rot::IDENTITY; n];
for i in (0..n).rev() {
let mu = alpha + a[i].abs();
let ah = alpha / mu;
let b = a[i] / mu;
let nu = (ah * ah + b * b).sqrt();
rots[i] = Rot {
c: ah / nu,
s: b / nu,
};
alpha = mu * nu;
}
for (j, g) in rots.iter().enumerate() {
let d = g.c * self.r.data[j + j * n];
if !d.is_finite() || !lt(T::ZERO, d) {
return Err(StatlaError::DowndateIndefinite { margin });
}
}
let r = &mut self.r.data;
for j in 0..n {
let col = &mut r[j * n..(j + 1) * n];
let mut xi = T::ZERO;
for i in (0..=j).rev() {
let g = rots[i];
let rij = col[i];
col[i] = g.c * rij - g.s * xi;
xi = g.c * xi + g.s * rij;
}
}
for i in 0..n {
if lt(self.r.data[i + i * n], T::ZERO) {
for j in i..n {
self.r.data[i + j * n] = -self.r.data[i + j * n];
}
}
}
debug_assert_eq!(self.check_invariants(), Ok(()), "(H1) after downdate");
Ok(DowndateInfo { margin })
}
fn forward(&self, b: &mut [T]) {
let n = self.n();
for j in 0..n {
let col = &self.r.data[(j * n)..=(j * n + j)];
let s = b[j] - dot(&col[..j], &b[..j]);
b[j] = s / col[j];
}
}
fn backward(&self, b: &mut [T]) {
let n = self.n();
for j in (0..n).rev() {
let col = &self.r.data[(j * n)..=(j * n + j)];
let xj = b[j] / col[j];
b[j] = xj;
axpy_sub(xj, &col[..j], &mut b[..j]);
}
}
pub fn solve(&self, b: &mut [T]) -> Result<(), StatlaError> {
check_len("b", self.n(), b.len())?;
self.forward(b);
self.backward(b);
Ok(())
}
pub fn solve_block(&self, mut b: MatMut<'_, T>) -> Result<(), StatlaError> {
let cols = b.as_ref().cols();
check_len("b (rows)", self.n(), b.as_ref().rows())?;
for j in 0..cols {
if let Some(col) = b.col_mut(j) {
self.forward(col);
self.backward(col);
}
}
Ok(())
}
pub fn solve_upper_t(&self, b: &mut [T]) -> Result<(), StatlaError> {
check_len("b", self.n(), b.len())?;
self.forward(b);
Ok(())
}
#[must_use]
pub fn log2_det_parts(&self) -> (T, i64) {
let n = self.n();
let two = T::from_f64(2.0);
let half = T::from_f64(0.5);
let mut m = T::ONE;
let mut e: i64 = 0;
for j in 0..n {
let d = self.r.data[j + j * n];
let (f, k) = split_pow2(d);
m *= f;
e += k;
while le_exact(two, m) {
m *= half;
e += 1;
}
while lt(m, T::ONE) && lt(T::ZERO, m) {
m *= two;
e -= 1;
}
}
(m, e)
}
pub fn check_invariants(&self) -> Result<(), &'static str> {
let n = self.r.rows;
if self.r.cols != n || self.r.data.len() != n * n {
return Err("(H1) R is not a consistent square block");
}
for j in 0..n {
for i in 0..n {
let v = self.r.data[i + j * n];
if !v.is_finite() {
return Err("(H1) R has a non-finite entry");
}
if i > j && !is_exact_zero(v) {
return Err("(H1) R is not upper triangular");
}
if i == j && !lt(T::ZERO, v) {
return Err("(H1) R has a nonpositive diagonal entry");
}
}
}
Ok(())
}
}
fn split_pow2<T: Real>(d: T) -> (T, i64) {
let bits = d.to_f64().to_bits();
let exp = i64::try_from((bits >> 52) & 0x7ff).unwrap_or(0);
if exp == 0 || exp == 0x7ff {
return (d, 0); }
let k = exp - 1023;
let inv = match u64::try_from(1023 - k) {
Ok(0) | Err(_) => f64::from_bits(1 << 51), Ok(b) => f64::from_bits(b << 52),
};
(d * T::from_f64(inv), k)
}