use crate::anm::series;
use crate::gcr::recip;
use crate::{Jvp, ResidualJet};
use multidouble::dd::DD;
use multidouble::precision::Precision;
use multidouble::simd::Native;
fn diff<P: Precision<Native>>(a: f64, b: f64) -> P::Scalar {
P::from_dd(DD::from_f64(a) - DD::from_f64(b))
}
fn combine<P: Precision<Native>>(w: &[P::Scalar], zs: &[&[&[f64]]]) -> Vec<Vec<f64>> {
let n = zs[0][0].len();
let mut buf = vec![0.0; P::PARTS];
let out: Vec<P::Scalar> = (0..n)
.map(|i| {
w.iter().zip(zs).fold(P::ZERO, |t, (&wk, z)| {
for (q, b) in buf.iter_mut().enumerate() {
*b = z[q][i];
}
t + wk * P::from_raw(&buf)
})
})
.collect();
(0..P::PARTS)
.map(|q| out.iter().map(|&v| P::part(v, q)).collect())
.collect()
}
pub fn lagrange<P: Precision<Native>>(pts: &[(f64, &[&[f64]])], p: f64) -> Vec<Vec<f64>> {
let w: Vec<P::Scalar> = pts
.iter()
.enumerate()
.map(|(i, &(pi, _))| {
pts.iter()
.enumerate()
.filter(|&(j, _)| j != i)
.fold(P::ONE, |t, (_, &(pj, _))| {
t * diff::<P>(p, pj) * recip::<P>(diff::<P>(pi, pj))
})
})
.collect();
let zs: Vec<&[&[f64]]> = pts.iter().map(|&(_, z)| z).collect();
combine::<P>(&w, &zs)
}
pub fn hermite<P: Precision<Native>>(
(p0, z0, t0): (f64, &[&[f64]], &[&[f64]]),
(p1, z1, t1): (f64, &[&[f64]], &[&[f64]]),
p: f64,
) -> Vec<Vec<f64>> {
let h = diff::<P>(p1, p0);
let u = diff::<P>(p, p0) * recip::<P>(h);
let (u2, one) = (u * u, P::ONE);
let u3 = u2 * u;
let (two, three) = (P::from_dd(DD::from_f64(2.0)), P::from_dd(DD::from_f64(3.0)));
let w = [
two * u3 - three * u2 + one,
(u3 - two * u2 + u) * h,
three * u2 - two * u3,
(u3 - u2) * h,
];
combine::<P>(&w, &[z0, t0, z1, t1])
}
pub fn tangent<P: Precision<Native>, Pr: ResidualJet<P> + Jvp<P> + ?Sized>(
pr: &Pr,
z: &[&[f64]],
refine: usize,
par: bool,
) -> Result<Vec<Vec<f64>>, String> {
let br = series::<P, Pr>(pr, z, 1, refine, par)?;
Ok((0..P::PARTS)
.map(|q| br.z[1].iter().map(|&v| P::part(v, q)).collect())
.collect())
}