use crate::newton::{newton, norm2, NewtonOpts, NewtonReport};
use crate::prec::{max_abs, parts};
use crate::Problem;
use multidouble::dd::DD;
use multidouble::dual::Lane;
use multidouble::field::Fl;
use multidouble::precision::Precision;
use multidouble::simd::Native;
use rayon::prelude::*;
#[derive(Clone, Copy, Debug)]
pub struct LmOpts {
pub tol: f64,
pub max_iter: usize,
pub tau: f64,
pub newton_after: f64,
pub newton: NewtonOpts,
pub par: bool,
}
impl Default for LmOpts {
fn default() -> Self {
LmOpts {
tol: 1e-28,
max_iter: 50,
tau: 1e-3,
newton_after: 1e-12,
newton: NewtonOpts::default(),
par: true,
}
}
}
#[derive(Clone, Debug, Default)]
pub struct LmReport {
pub fnorm: Vec<f64>,
pub mu: Vec<f64>,
pub factorizations: usize,
pub converged: bool,
pub newton: Option<NewtonReport>,
}
fn lanes<P: Precision<Native>>(n: usize, nv: usize, col: impl Fn(usize, usize) -> f64) -> Vec<P::V> {
let mut buf = vec![[0.0f64; 8]; P::PARTS];
(0..nv)
.map(|v| {
for (q, bq) in buf.iter_mut().enumerate() {
for (k, x) in bq.iter_mut().enumerate() {
let i = 8 * v + k;
*x = if i < n { col(q, i) } else { 0.0 };
}
}
let sl: [&[f64]; 8] = std::array::from_fn(|q| if q < P::PARTS { &buf[q][..] } else { &[][..] });
P::load_field::<1>(&sl[..P::PARTS]).0[0]
})
.collect()
}
pub fn gram<P: Precision<Native>>(
n: usize,
ld: usize,
c: &[Vec<f64>],
b: &[P::Scalar],
par: bool,
) -> (Vec<Vec<f64>>, Vec<P::Scalar>) {
let nv = ld / 8;
let cols: Vec<Vec<P::V>> = (0..n).map(|j| lanes::<P>(n, nv, |q, i| c[q][j * ld + i])).collect();
let bv = lanes::<P>(n, nv, |q, i| P::part(b[i], q));
let dot = |x: &[P::V], y: &[P::V]| -> P::Scalar {
let mut a = <P::V as Lane>::acc0();
for (xv, yv) in x.iter().zip(y) {
<P::V as Lane>::acc_mut(&mut a, xv, yv);
}
let s = Fl::<P::V, 1>([<P::V as Lane>::fin(a)]);
(0..8).fold(P::ZERO, |t, k| t + P::field_get(&s, k))
};
let upper = |j: usize| -> Vec<P::Scalar> { (0..=j).map(|i| dot(&cols[i], &cols[j])).collect() };
let tri: Vec<Vec<P::Scalar>> = if par {
(0..n).into_par_iter().map(upper).collect()
} else {
(0..n).map(upper).collect()
};
let mut g = vec![vec![0.0; ld * n]; P::PARTS];
for (j, col) in tri.iter().enumerate() {
for (i, &v) in col.iter().enumerate() {
for (q, gq) in g.iter_mut().enumerate() {
gq[j * ld + i] = P::part(v, q);
gq[i * ld + j] = P::part(v, q);
}
}
}
let jb = cols.iter().map(|col| dot(col, &bv)).collect();
(g, jb)
}
fn scalars<P: Precision<Native>>(z: &[&[f64]]) -> Vec<P::Scalar> {
let mut buf = vec![0.0; P::PARTS];
(0..z[0].len())
.map(|i| {
for (q, b) in buf.iter_mut().enumerate() {
*b = z[q][i];
}
P::from_raw(&buf)
})
.collect()
}
fn add<P: Precision<Native>>(z: &[&[f64]], dz: &[P::Scalar]) -> Vec<Vec<f64>> {
let s: Vec<P::Scalar> = scalars::<P>(z).iter().zip(dz).map(|(&a, &b)| a + b).collect();
(0..P::PARTS)
.map(|q| s.iter().map(|&v| P::part(v, q)).collect())
.collect()
}
pub fn levenberg_marquardt<P: Precision<Native>, Pr: Problem<P> + ?Sized>(
pr: &Pr,
z: &mut Vec<Vec<f64>>,
opts: &LmOpts,
) -> Result<LmReport, String> {
let n = pr.len();
let mut rep = LmReport::default();
let mut f = pr.residual(&parts(z), opts.par)?;
let mut mu = -1.0f64;
let mut nu = 2.0f64;
let mut handover = opts.newton_after > 0.0;
for _ in 0..opts.max_iter {
let fmax = max_abs::<P>(&f);
rep.fnorm.push(fmax);
if !fmax.is_finite() {
return Ok(rep);
}
if fmax <= opts.tol {
rep.converged = true;
return Ok(rep);
}
if handover && fmax < opts.newton_after {
handover = false;
let mut zn = z.clone();
let nopts = NewtonOpts {
tol: opts.tol,
..opts.newton
};
if let Ok(nr) = newton::<P, Pr>(pr, &mut zn, &nopts) {
let last = nr.fnorm.last().copied();
let done = nr.converged && last.is_some_and(|f| f <= opts.tol);
rep.newton = Some(nr);
if let (true, Some(f)) = (done, last) {
*z = zn;
rep.fnorm.push(f);
rep.converged = true;
return Ok(rep);
}
}
}
let (c, ld) = pr.jacobian(&parts(z), opts.par)?;
let nf: Vec<P::Scalar> = f.iter().map(|&v| -v).collect();
let (g, jf) = gram::<P>(n, ld, &c, &nf, opts.par);
let mut buf = vec![0.0; P::PARTS];
let mut diag: Vec<P::Scalar> = (0..n)
.map(|i| {
for (q, b) in buf.iter_mut().enumerate() {
*b = g[q][i * ld + i];
}
P::from_raw(&buf)
})
.collect();
let floor = diag.iter().map(|&d| P::to_dd(d).hi).fold(0.0, f64::max) * 2f64.powi(-104);
for d in diag.iter_mut() {
if P::to_dd(*d).hi < floor {
*d = P::from_dd(DD::from_f64(floor));
}
}
if mu < 0.0 {
mu = opts.tau;
}
let f2 = norm2::<P>(&f).powi(2);
loop {
let mut a = g.clone();
let m = P::from_dd(DD::from_f64(mu));
for (i, &d) in diag.iter().enumerate() {
for (q, b) in buf.iter_mut().enumerate() {
*b = a[q][i * ld + i];
}
let v = P::from_raw(&buf) + m * d;
for (q, aq) in a.iter_mut().enumerate() {
aq[i * ld + i] = P::part(v, q);
}
}
rep.factorizations += 1;
let Ok(lu) = P::lu_factor(n, ld, a, opts.par) else {
mu *= nu;
nu *= 2.0;
if !mu.is_finite() {
rep.fnorm.push(fmax);
return Ok(rep);
}
continue;
};
let mut dz = jf.clone();
P::lu_solve(&lu, &mut dz);
let zt = add::<P>(&parts(z), &dz);
let ft = pr.residual(&parts(&zt), opts.par)?;
let pred: f64 = dz
.iter()
.zip(&diag)
.zip(&jf)
.map(|((&h, &d), &j)| P::to_dd(h * (P::from_dd(DD::from_f64(mu)) * d * h + j)).hi)
.sum::<f64>()
/ 2.0;
let ft2 = norm2::<P>(&ft).powi(2);
let rho = (f2 - ft2) / 2.0 / pred;
if ft2.is_finite() && rho > 0.0 {
*z = zt;
f = ft;
rep.mu.push(mu);
mu *= (1.0f64 / 3.0).max(1.0 - (2.0 * rho - 1.0).powi(3));
nu = 2.0;
break;
}
mu *= nu;
nu *= 2.0;
if !mu.is_finite() {
rep.fnorm.push(fmax);
return Ok(rep);
}
}
}
let fmax = max_abs::<P>(&f);
rep.fnorm.push(fmax);
rep.converged = fmax <= opts.tol;
Ok(rep)
}