use eunomia::{FloatElement, NumericElement, RealField};
use leto::{Array1, Array2};
use std::collections::VecDeque;
use crate::application::nonlinear::linalg::{
add_scaled, add_scaled_in_place, dot, matrix_zeros, scale, sub, vector_len, vector_zeros,
};
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub enum AndersonMethod {
NormalEquations,
#[default]
QR,
}
#[derive(Debug, Clone)]
pub struct AndersonConfig<T: RealField> {
pub history_depth: usize,
pub relaxation: T,
pub drop_tolerance: T,
pub method: AndersonMethod,
}
impl<T: RealField> Default for AndersonConfig<T> {
fn default() -> Self {
Self {
history_depth: 5,
relaxation: T::ONE,
drop_tolerance: <T as FloatElement>::from_f64(1e-12),
method: AndersonMethod::QR,
}
}
}
#[derive(Debug)]
pub(super) struct QrState<T: RealField> {
pub(super) q_cols: VecDeque<Array1<T>>,
r_mat: Array2<T>,
max_depth: usize,
drop_tol: T,
}
impl<T: RealField + std::fmt::Debug> QrState<T> {
fn new(max_depth: usize, drop_tol: T) -> Self {
Self {
q_cols: VecDeque::with_capacity(max_depth + 1),
r_mat: matrix_zeros(0, 0),
max_depth,
drop_tol,
}
}
fn append_column(&mut self, v: &Array1<T>, _m_hint: usize) -> bool {
let m_current = self.q_cols.len();
if m_current >= self.max_depth && !self.q_cols.is_empty() {
self.q_cols.pop_front();
let m = self.q_cols.len();
if m > 0 {
let mut new_r = matrix_zeros(m, m);
for row in 0..m {
for col in 0..m {
new_r[[row, col]] = self.r_mat[[row + 1, col + 1]];
}
}
self.r_mat = new_r;
} else {
self.r_mat = matrix_zeros(0, 0);
}
}
let m_new = self.q_cols.len();
let mut w = v.clone();
let mut r_col = vector_zeros(m_new + 1);
for (j, qj) in self.q_cols.iter().enumerate() {
let alpha = dot(qj, &w);
r_col[[j]] = alpha;
add_scaled_in_place(&mut w, qj, T::ZERO - alpha);
}
let norm_w = NumericElement::sqrt(dot(&w, &w));
r_col[[m_new]] = norm_w;
if norm_w <= self.drop_tol {
return false;
}
self.q_cols.push_back(scale(&w, T::ONE / norm_w));
let new_m = m_new + 1;
let mut new_r = matrix_zeros(new_m, new_m);
if m_new > 0 && self.r_mat.shape() == [m_new, m_new] {
for row in 0..m_new {
for col in 0..m_new {
new_r[[row, col]] = self.r_mat[[row, col]];
}
}
}
for i in 0..=m_new {
new_r[[i, m_new]] = r_col[[i]];
}
self.r_mat = new_r;
true
}
fn solve_least_squares(&self, f: &Array1<T>) -> Option<Array1<T>> {
let m = self.q_cols.len();
if m == 0 || self.r_mat.shape() != [m, m] {
return None;
}
let mut rhs = vector_zeros(m);
for (j, qj) in self.q_cols.iter().enumerate() {
rhs[[j]] = dot(qj, f);
}
let mut gamma = vector_zeros(m);
for i in (0..m).rev() {
let diag = self.r_mat[[i, i]];
if NumericElement::abs(diag) < self.drop_tol {
return None; }
let mut s = rhs[[i]];
for j in (i + 1)..m {
s -= self.r_mat[[i, j]] * gamma[[j]];
}
gamma[[i]] = s / diag;
}
Some(gamma)
}
fn reset(&mut self) {
self.q_cols.clear();
self.r_mat = matrix_zeros(0, 0);
}
}
#[derive(Debug)]
pub struct AndersonAccelerator<T: RealField> {
config: AndersonConfig<T>,
prev_x: Option<Array1<T>>,
prev_f: Option<Array1<T>>,
pub(super) delta_x: VecDeque<Array1<T>>,
pub(super) delta_f: VecDeque<Array1<T>>,
pub(super) qr_state: Option<QrState<T>>,
}
impl<T: RealField + std::fmt::Debug> AndersonAccelerator<T> {
#[must_use]
pub fn new(config: AndersonConfig<T>) -> Self {
let depth = config.history_depth;
let qr_state = if config.method == AndersonMethod::QR {
Some(QrState::new(config.history_depth, config.drop_tolerance))
} else {
None
};
Self {
config,
prev_x: None,
prev_f: None,
delta_x: VecDeque::with_capacity(depth + 1),
delta_f: VecDeque::with_capacity(depth + 1),
qr_state,
}
}
#[allow(clippy::many_single_char_names)]
pub fn compute_next(&mut self, x: &Array1<T>, g_x: &Array1<T>) -> Array1<T> {
let f = sub(g_x, x);
let mut x_next = add_scaled(x, &f, self.config.relaxation);
if let (Some(prev_x), Some(prev_f)) = (&self.prev_x, &self.prev_f) {
let dx = sub(x, prev_x);
let df = sub(&f, prev_f);
if self.delta_x.len() >= self.config.history_depth {
self.delta_x.pop_front();
self.delta_f.pop_front();
}
self.delta_x.push_back(dx);
self.delta_f.push_back(df.clone());
let gamma_opt = match self.config.method {
AndersonMethod::QR => {
let accepted = if let Some(qr) = &mut self.qr_state {
qr.append_column(&df, 0)
} else {
false
};
if accepted {
if let Some(qr) = &self.qr_state {
qr.solve_least_squares(&f)
} else {
None
}
} else {
self.delta_x.pop_back();
self.delta_f.pop_back();
None
}
}
AndersonMethod::NormalEquations => self.solve_normal_equations(&f),
};
#[cfg(debug_assertions)]
if let Some(qr) = &self.qr_state {
debug_assert_eq!(
self.delta_x.len(),
qr.q_cols.len(),
"Anderson QR lockstep violated: \
delta_x.len()={}, qr.q_cols.len()={}",
self.delta_x.len(),
qr.q_cols.len(),
);
debug_assert_eq!(
self.delta_f.len(),
qr.q_cols.len(),
"Anderson QR lockstep violated: \
delta_f.len()={}, qr.q_cols.len()={}",
self.delta_f.len(),
qr.q_cols.len(),
);
}
if let Some(gamma) = gamma_opt {
for j in 0..vector_len(&gamma) {
let term =
add_scaled(&self.delta_x[j], &self.delta_f[j], self.config.relaxation);
add_scaled_in_place(&mut x_next, &term, T::ZERO - gamma[j]);
}
} else {
self.delta_x.clear();
self.delta_f.clear();
if let Some(qr) = &mut self.qr_state {
qr.reset();
}
}
}
self.prev_x = Some(x.clone());
self.prev_f = Some(f);
x_next
}
fn solve_normal_equations(&self, f: &Array1<T>) -> Option<Array1<T>> {
let m = self.delta_f.len();
if m == 0 {
return None;
}
let mut normal = matrix_zeros(m, m);
let mut rhs = vector_zeros(m);
for row in 0..m {
rhs[[row]] = dot(&self.delta_f[row], f);
for col in 0..m {
normal[[row, col]] = dot(&self.delta_f[row], &self.delta_f[col]);
}
}
solve_dense_pivoted(normal, rhs, self.config.drop_tolerance)
}
pub fn reset(&mut self) {
self.prev_x = None;
self.prev_f = None;
self.delta_x.clear();
self.delta_f.clear();
if let Some(qr) = &mut self.qr_state {
qr.reset();
}
}
}
fn solve_dense_pivoted<T: RealField + std::fmt::Debug>(
mut matrix: Array2<T>,
mut rhs: Array1<T>,
drop_tolerance: T,
) -> Option<Array1<T>> {
let [rows, cols] = matrix.shape();
let n = vector_len(&rhs);
if rows != n || cols != n {
return None;
}
for pivot_col in 0..n {
let mut pivot_row = pivot_col;
let mut pivot_abs = NumericElement::abs(matrix[[pivot_col, pivot_col]]);
for candidate in (pivot_col + 1)..n {
let candidate_abs = NumericElement::abs(matrix[[candidate, pivot_col]]);
if candidate_abs > pivot_abs {
pivot_abs = candidate_abs;
pivot_row = candidate;
}
}
if pivot_abs < drop_tolerance {
return None;
}
if pivot_row != pivot_col {
for col in pivot_col..n {
let pivot_value = matrix[[pivot_col, col]];
matrix[[pivot_col, col]] = matrix[[pivot_row, col]];
matrix[[pivot_row, col]] = pivot_value;
}
let pivot_rhs = rhs[pivot_col];
rhs[pivot_col] = rhs[pivot_row];
rhs[pivot_row] = pivot_rhs;
}
for row in (pivot_col + 1)..n {
let factor = matrix[[row, pivot_col]] / matrix[[pivot_col, pivot_col]];
matrix[[row, pivot_col]] = T::ZERO;
for col in (pivot_col + 1)..n {
matrix[[row, col]] = matrix[[row, col]] - factor * matrix[[pivot_col, col]];
}
rhs[[row]] = rhs[[row]] - factor * rhs[[pivot_col]];
}
}
let mut solution = vector_zeros(n);
for row in (0..n).rev() {
let diag = matrix[[row, row]];
if NumericElement::abs(diag) < drop_tolerance {
return None;
}
let mut value = rhs[[row]];
for col in (row + 1)..n {
value -= matrix[[row, col]] * solution[[col]];
}
solution[[row]] = value / diag;
}
Some(solution)
}