use num_complex::Complex;
use crate::complex::{inv, times_i};
use crate::scalar::Real;
#[derive(Clone, Copy, Debug)]
pub(crate) enum Side<T> {
Delta(Complex<T>),
Direct(Complex<T>),
}
pub(crate) type Outer<T> = (Side<T>, Side<T>);
#[derive(Clone, Copy, Debug)]
pub(crate) struct OuterDerivs<T> {
pub(crate) hp: [Complex<T>; 2],
pub(crate) hplus: [Complex<T>; 2],
pub(crate) extra_m: [Complex<T>; 2],
}
#[derive(Clone, Debug)]
pub(crate) struct State<T: Real> {
pub(crate) x: T,
pub(crate) m: Complex<T>,
pub(crate) a: Vec<Complex<T>>,
pub(crate) b: Vec<Complex<T>>,
pub(super) derivatives: bool,
pub(crate) da_dx: Vec<Complex<T>>,
pub(crate) da_dm: Vec<Complex<T>>,
pub(crate) db_dx: Vec<Complex<T>>,
pub(crate) db_dm: Vec<Complex<T>>,
pub(crate) abs_sum: T,
pub(crate) abs_sum_jacobian: [T; 3],
u_z: Vec<Complex<T>>,
u_x: Vec<T>,
u_real: Vec<T>,
pub(crate) stored: Vec<[Complex<T>; 6]>,
}
pub(crate) fn real<T: Real>(v: T) -> Complex<T> {
Complex::new(v, T::zero())
}
pub(crate) fn int<T: Real>(k: usize) -> T {
T::from_f64(k as f64)
}
fn bits<T: Real>() -> f64 {
-T::epsilon().approx_f64().log2()
}
fn margin<T: Real>() -> usize {
(bits::<T>() / 2.0).ceil() as usize + 4
}
fn start_index<T: Real>(n_top: usize, rho_abs: f64) -> usize {
let bits = bits::<T>();
let k0 = n_top.max((1.5 * rho_abs).ceil() as usize);
let q = (2 * k0 + 1) as f64 / rho_abs - 0.5;
let enclosed = k0 + (bits / (2.0 * q.log2())).ceil() as usize + 2;
if n_top as f64 >= 1.5 * rho_abs {
return enclosed;
}
let s0 = n_top as f64 - rho_abs;
let s0_32 = if s0 > 0.0 { s0 * s0.sqrt() } else { 0.0 };
let s = (s0_32 + 1.4 * 0.37 * bits * rho_abs.sqrt()).powf(2.0 / 3.0);
let asymptotic = (rho_abs + s).ceil() as usize + margin::<T>();
asymptotic.min(enclosed)
}
#[inline]
fn step_complex<T: Real>(k: usize, inv_z: Complex<T>, u: Complex<T>) -> Complex<T> {
let c = inv_z.scale(int(2 * k + 1));
let mut den = c - u;
if den.re == T::zero() && den.im == T::zero() {
den = real(T::epsilon() * (c.norm() + u.norm()));
}
inv(den)
}
#[inline]
fn step_real<T: Real>(k: usize, inv_rho: T, u: T) -> T {
let c = inv_rho * int::<T>(2 * k + 1);
let mut den = c - u;
if den == T::zero() {
den = T::epsilon() * (c.abs() + u.abs());
}
den.recip()
}
#[inline]
fn interleave(
a: (usize, usize),
b: (usize, usize),
mut step_a: impl FnMut(usize),
mut step_b: impl FnMut(usize),
) {
for k in (a.1.min(b.1)..=a.0.max(b.0)).rev() {
if k <= a.0 && k >= a.1 {
step_a(k);
}
if k <= b.0 && k >= b.1 {
step_b(k);
}
}
}
pub(crate) fn ratios_complex<T: Real>(out: &mut Vec<Complex<T>>, z: Complex<T>, n_top: usize) {
let inv_z = inv(z);
let start = start_index::<T>(n_top + 1, z.norm().approx_f64());
out.clear();
out.resize(n_top + 2, real(T::zero()));
let mut u = real(T::zero());
for k in (1..=start).rev() {
u = step_complex(k, inv_z, u);
if k <= n_top + 1 {
out[k] = u;
}
}
}
pub(crate) fn ratios_real<T: Real>(out: &mut Vec<T>, rho: T, from: usize, n_top: usize) {
let inv_rho = rho.recip();
let start = start_index::<T>(n_top + 1, rho.approx_f64().abs());
out.clear();
out.resize(n_top + 2, T::zero());
let mut u = T::zero();
for k in (from.max(1)..=start).rev() {
u = step_real(k, inv_rho, u);
if k <= n_top + 1 {
out[k] = u;
}
}
}
pub(crate) fn ratios<T: Real>(
out: &mut Vec<Complex<T>>,
scratch: &mut Vec<T>,
m: Complex<T>,
x: T,
n_top: usize,
) {
let z = m.scale(x);
if m.im.is_zero() {
ratios_real(scratch, z.re, 1, n_top);
out.clear();
out.extend(scratch.iter().map(|&u| real(u)));
} else {
ratios_complex(out, z, n_top);
}
}
fn upward_orders<T: Real>(x: T, n: usize) -> usize {
let xf = x.approx_f64();
if xf >= 1.0 {
(xf.floor() as usize).min(n)
} else {
0
}
}
impl<T: Real> State<T> {
pub(crate) fn new() -> Self {
Self {
x: T::zero(),
m: real(T::zero()),
a: Vec::new(),
b: Vec::new(),
derivatives: false,
da_dx: Vec::new(),
da_dm: Vec::new(),
db_dx: Vec::new(),
db_dm: Vec::new(),
abs_sum: T::zero(),
abs_sum_jacobian: [T::zero(); 3],
u_z: Vec::new(),
u_x: Vec::new(),
u_real: Vec::new(),
stored: Vec::new(),
}
}
pub(crate) fn solve(&mut self, x: T, m: Complex<T>, n: usize, derivatives: bool) {
self.prepare(x, m, n);
self.finish(x, m, n, derivatives, None, None);
}
pub(crate) fn prepare(&mut self, x: T, m: Complex<T>, n: usize) {
let n_s = upward_orders(x, n);
let top = n + 1;
let x_steps = if n_s < n {
(
start_index::<T>(top, x.approx_f64().abs()),
(n_s + 1).max(1),
)
} else {
(0, 1) };
let inv_x = x.recip();
self.u_x.clear();
self.u_x.resize(n + 2, T::zero());
let u_x = &mut self.u_x;
let mut ux = T::zero();
let step_x = |k: usize| {
ux = step_real(k, inv_x, ux);
if k <= top {
u_x[k] = ux;
}
};
let z = m.scale(x);
if m.im.is_zero() {
let inv_rho = z.re.recip();
let scratch = &mut self.u_real;
scratch.clear();
scratch.resize(n + 2, T::zero());
let mut u = T::zero();
let start = start_index::<T>(top, z.re.approx_f64().abs());
interleave(
(start, 1),
x_steps,
|k| {
u = step_real(k, inv_rho, u);
if k <= top {
scratch[k] = u;
}
},
step_x,
);
self.u_z.clear();
self.u_z.extend(scratch.iter().map(|&u| real(u)));
} else {
let inv_z = inv(z);
let out = &mut self.u_z;
out.clear();
out.resize(n + 2, real(T::zero()));
let mut u = real(T::zero());
let start = start_index::<T>(top, z.norm().approx_f64());
interleave(
(start, 1),
x_steps,
|k| {
u = step_complex(k, inv_z, u);
if k <= top {
out[k] = u;
}
},
step_x,
);
}
}
pub(crate) fn u_z(&self) -> &[Complex<T>] {
&self.u_z
}
pub(crate) fn finish(
&mut self,
x: T,
m: Complex<T>,
n: usize,
derivatives: bool,
outer: Option<&[Outer<T>]>,
outer_derivs: Option<&[OuterDerivs<T>]>,
) {
debug_assert!(!(derivatives && outer.is_some() && outer_derivs.is_none()));
self.stored.clear();
let one = T::one();
let two = T::from_f64(2.0);
let i1 = Complex::new(T::zero(), one);
self.x = x;
self.m = m;
self.derivatives = derivatives;
let z = m.scale(x);
let n_s = upward_orders(x, n);
for v in [
&mut self.a,
&mut self.b,
&mut self.da_dx,
&mut self.da_dm,
&mut self.db_dx,
&mut self.db_dm,
] {
v.clear();
}
let inv_x = x.recip();
let inv_z = inv(z);
let inv_m = inv(m);
let m2 = m * m;
let inv_m2 = inv_m * inv_m;
let m2_minus_1 = (m - real(one)) * (m + real(one));
let c_a = -(m2_minus_1 * inv_m2); let (sin_x, cos_x) = x.sin_cos();
let (mut psi_prev, mut psi) = (cos_x, sin_x); let (mut chi_prev, mut chi) = (-sin_x, cos_x); let mut t = real(T::zero());
let mut inv_xi = real(T::zero());
let mut r = real(T::zero());
if n_s == 0 {
let xi0 = Complex::new(sin_x, -cos_x);
t = real(sin_x) * inv(xi0);
inv_xi = Complex::new(sin_x, cos_x); r = i1; }
let mut abs_sum = T::zero();
let mut jac = [T::zero(); 3];
for k in 1..=n {
let kf: T = int(k);
let k_x = kf * inv_x;
let d = inv_z.scale(int(k + 1)) - self.u_z[k + 1];
let side = outer.map(|o| o[k - 1]);
let h = |s: Option<Side<T>>| match s {
None => d,
Some(Side::Delta(delta)) => d + delta,
Some(Side::Direct(h)) => h,
};
let (hh_a, hh_b) = (h(side.map(|s| s.0)), h(side.map(|s| s.1)));
let d_m = hh_a * inv_m;
let md = hh_b * m;
let big_a = d_m + real(k_x);
let big_b = md + real(k_x);
let (an, bn, inv_da, inv_db, g_a, g_b, h_a, h_b);
if k <= n_s {
let c: T = int::<T>(2 * k - 1) * inv_x;
let psi_next = c * psi - psi_prev;
psi_prev = psi;
psi = psi_next;
let chi_next = c * chi - chi_prev;
chi_prev = chi;
chi = chi_next;
let xi = Complex::new(psi, -chi);
let xi_prev = Complex::new(psi_prev, -chi_prev);
inv_da = inv(big_a * xi - xi_prev);
inv_db = inv(big_b * xi - xi_prev);
match side {
None => {
an = (big_a.scale(psi) - real(psi_prev)) * inv_da;
bn = (big_b.scale(psi) - real(psi_prev)) * inv_db;
}
Some((sa, sb)) => {
let own = |f: Complex<T>| {
if m == real(one) {
real(T::zero())
} else {
(f + real(k_x)).scale(psi) - real(psi_prev)
}
};
an = match sa {
Side::Delta(delta) => {
(own(d * inv_m) + (delta * inv_m).scale(psi)) * inv_da
}
Side::Direct(_) => (big_a.scale(psi) - real(psi_prev)) * inv_da,
};
bn = match sb {
Side::Delta(delta) => (own(d * m) + (m * delta).scale(psi)) * inv_db,
Side::Direct(_) => (big_b.scale(psi) - real(psi_prev)) * inv_db,
};
}
}
g_a = xi * inv_da;
g_b = xi * inv_db;
h_a = xi_prev * inv_da;
h_b = xi_prev * inv_db;
if k == n_s {
inv_xi = inv(xi);
t = real(psi) * inv_xi;
r = xi_prev * inv_xi;
}
} else {
r = inv(real(int::<T>(2 * k - 1) * inv_x) - r);
t = (t * r).scale(self.u_x[k]);
inv_xi = inv_xi * r;
let u_z1 = self.u_z[k + 1];
let u_x1 = real(self.u_x[k + 1]);
let mut num_a = c_a.scale(int::<T>(k + 1) * inv_x) + u_x1 - u_z1 * inv_m;
let mut num_b = u_x1 - m * u_z1;
if let Some((sa, sb)) = side {
let d_x = real(int::<T>(k + 1) * inv_x) - u_x1;
num_a = match sa {
Side::Delta(delta) => num_a + delta * inv_m,
Side::Direct(_) => d_m - d_x,
};
num_b = match sb {
Side::Delta(delta) => num_b + m * delta,
Side::Direct(_) => md - d_x,
};
}
let ia = inv(big_a - r);
let ib = inv(big_b - r);
an = t * num_a * ia;
bn = t * num_b * ib;
inv_da = inv_xi * ia;
inv_db = inv_xi * ib;
g_a = ia;
g_b = ib;
h_a = r * ia;
h_b = r * ib;
}
self.a.push(an);
self.b.push(bn);
let na = inv_da.norm_sqr();
let nb = inv_db.norm_sqr();
let weight: T = int(2 * k + 1);
abs_sum += weight * (-(d_m.im * na) - md.im * nb);
let od = outer_derivs.map(|o| o[k - 1]);
if derivatives && od.is_none() {
let u = self.u_z[k + 1];
let one_u2 = real(one) + u * u;
let dp = (u.scale(two) * inv_z - inv_z * inv_z).scale(int(k + 1)) - one_u2;
let d_plus = u.scale(weight) - z * one_u2; let d_minus = d.scale(two) - d_plus; let kk1: T = int::<T>(k) * int::<T>(k + 1);
let ida2 = inv_da * inv_da;
let idb2 = inv_db * inv_db;
self.da_dm.push(times_i(d_minus) * inv_m2 * ida2);
self.db_dm.push(-(times_i(d_plus) * idb2));
self.da_dx.push(
times_i(m2_minus_1) * (d * d + real(kk1 * inv_x * inv_x)) * inv_m2 * ida2,
);
self.db_dx.push(times_i(m2_minus_1) * idb2);
let fa_m = -(d_minus * inv_m2); let fb_m = d_plus; let k_x2 = k_x * inv_x;
let la_x = g_a * (dp - real(k_x2) - big_a.scale(k_x) + real(one)) + h_a * d_m;
let lb_x = g_b * (m2 * dp - real(k_x2) - big_b.scale(k_x) + real(one)) + h_b * md;
let la_m = fa_m * g_a;
let lb_m = fb_m * g_b;
let term = |f: Complex<T>, df: Complex<T>, l: Complex<T>, n2: T| {
-((df.im - two * f.im * l.re) * n2)
};
jac[0] += weight * (term(d_m, dp, la_x, na) + term(md, m2 * dp, lb_x, nb));
jac[1] += weight * (term(d_m, fa_m, la_m, na) + term(md, fb_m, lb_m, nb));
jac[2] += weight
* (term(d_m, times_i(fa_m), times_i(la_m), na)
+ term(md, times_i(fb_m), times_i(lb_m), nb));
}
if let (true, Some(o)) = (derivatives, od) {
let kk1: T = int::<T>(k) * int::<T>(k + 1);
let ida2 = inv_da * inv_da;
let idb2 = inv_db * inv_db;
let [dpa, dpb] = o.hp;
let minus_a = hh_a.scale(two) - o.hplus[0]; let fa_m = -(minus_a * inv_m2) + o.extra_m[0] * inv_m; let fb_m = o.hplus[1] + m * o.extra_m[1]; self.da_dm.push(-(times_i(fa_m) * ida2));
self.db_dm.push(-(times_i(fb_m) * idb2));
self.da_dx.push(
times_i(m2_minus_1) * (hh_a * hh_a + real(kk1 * inv_x * inv_x)) * inv_m2 * ida2,
);
self.db_dx.push(times_i(m2_minus_1) * idb2);
let k_x2 = k_x * inv_x;
let la_x = g_a * (dpa - real(k_x2) - big_a.scale(k_x) + real(one)) + h_a * d_m;
let lb_x = g_b * (m2 * dpb - real(k_x2) - big_b.scale(k_x) + real(one)) + h_b * md;
let la_m = fa_m * g_a;
let lb_m = fb_m * g_b;
let term = |f: Complex<T>, df: Complex<T>, l: Complex<T>, n2: T| {
-((df.im - two * f.im * l.re) * n2)
};
jac[0] += weight * (term(d_m, dpa, la_x, na) + term(md, m2 * dpb, lb_x, nb));
jac[1] += weight * (term(d_m, fa_m, la_m, na) + term(md, fb_m, lb_m, nb));
jac[2] += weight
* (term(d_m, times_i(fa_m), times_i(la_m), na)
+ term(md, times_i(fb_m), times_i(lb_m), nb));
self.stored.push([inv_da, inv_db, g_a, g_b, d_m, md]);
}
}
self.abs_sum = abs_sum;
self.abs_sum_jacobian = jac;
}
}