use alloc::vec;
use alloc::vec::Vec;
use crate::ode::OdeError;
use crate::Float;
const C: [f64; 7] = [0.0, 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0];
const A: [[f64; 6]; 6] = [
[1.0 / 5.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[3.0 / 40.0, 9.0 / 40.0, 0.0, 0.0, 0.0, 0.0],
[44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0, 0.0, 0.0, 0.0],
[
19372.0 / 6561.0,
-25360.0 / 2187.0,
64448.0 / 6561.0,
-212.0 / 729.0,
0.0,
0.0,
],
[
9017.0 / 3168.0,
-355.0 / 33.0,
46732.0 / 5247.0,
49.0 / 176.0,
-5103.0 / 18656.0,
0.0,
],
[
35.0 / 384.0,
0.0,
500.0 / 1113.0,
125.0 / 192.0,
-2187.0 / 6784.0,
11.0 / 84.0,
],
];
const B5: [f64; 7] = [
35.0 / 384.0,
0.0,
500.0 / 1113.0,
125.0 / 192.0,
-2187.0 / 6784.0,
11.0 / 84.0,
0.0,
];
const B4: [f64; 7] = [
5179.0 / 57600.0,
0.0,
7571.0 / 16695.0,
393.0 / 640.0,
-92097.0 / 339200.0,
187.0 / 2100.0,
1.0 / 40.0,
];
pub fn dopri5_vec<F>(
f: F,
t0: f64,
t1: f64,
y0: &[f64],
rtol: f64,
atol: f64,
max_steps: usize,
) -> Result<Vec<f64>, OdeError>
where
F: Fn(f64, &[f64]) -> Vec<f64>,
{
let n = y0.len();
let span = t1 - t0;
if span == 0.0 {
return Ok(y0.to_vec());
}
let sign = span.signum();
let total = span.abs();
let mut t = t0;
let mut y = y0.to_vec();
let mut h = total / 100.0;
let min_step = 16.0 * f64::EPSILON * total.max(t0.abs());
for _ in 0..max_steps {
let remaining = (t1 - t).abs();
if remaining == 0.0 {
return Ok(y);
}
let step = h.min(remaining);
let hs = sign * step;
let mut k: Vec<Vec<f64>> = Vec::with_capacity(7);
k.push(f(t, &y));
for s in 0..6 {
let stage: Vec<f64> = (0..n)
.map(|i| {
let mut acc = y[i];
for (j, kj) in k.iter().enumerate() {
acc += hs * A[s][j] * kj[i];
}
acc
})
.collect();
k.push(f(t + C[s + 1] * hs, &stage));
}
let y_new: Vec<f64> = (0..n)
.map(|i| {
let mut acc = y[i];
for (j, kj) in k.iter().enumerate().take(6) {
acc += hs * B5[j] * kj[i];
}
acc
})
.collect();
let err: Vec<f64> = (0..n)
.map(|i| {
let mut e = 0.0;
for (j, kj) in k.iter().enumerate() {
e += hs * (B5[j] - B4[j]) * kj[i];
}
e
})
.collect();
let mut sum_sq = 0.0;
for i in 0..n {
let scale = atol + rtol * y[i].abs().max(y_new[i].abs());
let e = err[i] / scale;
sum_sq += e * e;
}
let error = if n == 0 {
0.0
} else {
Float::sqrt(sum_sq / n as f64)
};
let accepted = error <= 1.0;
if accepted {
t = if step == remaining { t1 } else { t + hs };
y = y_new;
}
let factor = if error == 0.0 {
5.0
} else {
Float::min(Float::max(0.9 * Float::powf(error, -0.2), 0.2), 5.0)
};
let factor = if accepted { factor } else { factor.min(1.0) };
h = step * factor;
if accepted && (t1 - t).abs() == 0.0 {
return Ok(y);
}
if h < min_step {
return Err(OdeError::StepSizeUnderflow);
}
}
Err(OdeError::MaxStepsExceeded)
}
#[derive(Clone, Debug, PartialEq)]
pub struct Sensitivities {
pub y: Vec<f64>,
pub s: Vec<Vec<f64>>,
}
#[allow(clippy::too_many_arguments)]
pub fn forward_sensitivity<F, Jy, Jp>(
f: F,
jac_y: Jy,
jac_p: Jp,
t0: f64,
t1: f64,
y0: &[f64],
p: &[f64],
rtol: f64,
atol: f64,
max_steps: usize,
) -> Result<Sensitivities, OdeError>
where
F: Fn(f64, &[f64], &[f64]) -> Vec<f64>,
Jy: Fn(f64, &[f64], &[f64]) -> Vec<Vec<f64>>,
Jp: Fn(f64, &[f64], &[f64]) -> Vec<Vec<f64>>,
{
let n = y0.len();
let np = p.len();
let mut z0 = vec![0.0; n + n * np];
z0[..n].copy_from_slice(y0);
let rhs = |t: f64, z: &[f64]| -> Vec<f64> {
let y = &z[..n];
let dy = f(t, y, p);
let jy = jac_y(t, y, p);
let jp = jac_p(t, y, p);
let mut dz = vec![0.0; n + n * np];
dz[..n].copy_from_slice(&dy);
for i in 0..n {
for k in 0..np {
let mut acc = jp[i][k];
for j in 0..n {
acc += jy[i][j] * z[n + j * np + k];
}
dz[n + i * np + k] = acc;
}
}
dz
};
let z = dopri5_vec(rhs, t0, t1, &z0, rtol, atol, max_steps)?;
let s = (0..n)
.map(|i| z[n + i * np..n + (i + 1) * np].to_vec())
.collect();
Ok(Sensitivities {
y: z[..n].to_vec(),
s,
})
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn exponential_decay_sensitivity_matches_the_closed_form() {
let (k, y0, t) = (0.5, 2.0, 2.0);
let out = forward_sensitivity(
|_t, y, p| vec![-p[0] * y[0]],
|_t, _y, p| vec![vec![-p[0]]],
|_t, y, _p| vec![vec![-y[0]]],
0.0,
t,
&[y0],
&[k],
1e-11,
1e-13,
100_000,
)
.unwrap();
let y_exact = y0 * (-k * t).exp();
let s_exact = -t * y0 * (-k * t).exp();
assert!((out.y[0] - y_exact).abs() < 1e-8);
assert!((out.s[0][0] - s_exact).abs() < 1e-7, "{:?}", out.s);
}
#[test]
fn two_parameter_oscillator_sensitivities_match_finite_differences() {
let f = |_t: f64, y: &[f64], p: &[f64]| vec![y[1], -p[0] * p[0] * y[0] - p[1] * y[1]];
let jy = |_t: f64, _y: &[f64], p: &[f64]| vec![vec![0.0, 1.0], vec![-p[0] * p[0], -p[1]]];
let jp =
|_t: f64, y: &[f64], p: &[f64]| vec![vec![0.0, 0.0], vec![-2.0 * p[0] * y[0], -y[1]]];
let run = |p: &[f64]| {
dopri5_vec(
|t, y| f(t, y, p),
0.0,
3.0,
&[1.0, 0.0],
1e-12,
1e-14,
1_000_000,
)
.unwrap()
};
let p = [1.3, 0.2];
let out = forward_sensitivity(
f,
jy,
jp,
0.0,
3.0,
&[1.0, 0.0],
&p,
1e-12,
1e-14,
1_000_000,
)
.unwrap();
let h = 1e-6;
for k in 0..2 {
let mut hi = p;
let mut lo = p;
hi[k] += h;
lo[k] -= h;
let (yh, yl) = (run(&hi), run(&lo));
for i in 0..2 {
let fd = (yh[i] - yl[i]) / (2.0 * h);
assert!(
(out.s[i][k] - fd).abs() < 1e-5,
"∂y{i}/∂p{k}: {} vs {fd}",
out.s[i][k]
);
}
}
}
#[test]
fn dopri5_vec_matches_the_fixed_array_solver() {
let y = dopri5_vec(|_t, y| vec![y[0]], 0.0, 1.0, &[1.0], 1e-10, 1e-12, 10_000).unwrap();
assert!((y[0] - core::f64::consts::E).abs() < 1e-8);
}
}