use crate::matrix::vector::Vector;
use crate::traits::FloatScalar;
use crate::Matrix;
pub fn finite_difference_jacobian<T: FloatScalar, const M: usize, const N: usize>(
mut f: impl FnMut(&Vector<T, N>) -> Vector<T, M>,
x: &Vector<T, N>,
) -> Matrix<T, M, N> {
let sqrt_eps = T::epsilon().sqrt();
let f0 = f(x);
let mut jac = Matrix::<T, M, N>::zeros();
for j in 0..N {
let h = sqrt_eps * x[j].abs().max(T::one());
let mut x_pert = *x;
x_pert[j] = x_pert[j] + h;
let f_pert = f(&x_pert);
for i in 0..M {
jac[(i, j)] = (f_pert[i] - f0[i]) / h;
}
}
jac
}
pub fn finite_difference_gradient<T: FloatScalar, const N: usize>(
mut f: impl FnMut(&Vector<T, N>) -> T,
x: &Vector<T, N>,
) -> Vector<T, N> {
let sqrt_eps = T::epsilon().sqrt();
let f0 = f(x);
let mut grad = Vector::<T, N>::zeros();
for j in 0..N {
let h = sqrt_eps * x[j].abs().max(T::one());
let mut x_pert = *x;
x_pert[j] = x_pert[j] + h;
grad[j] = (f(&x_pert) - f0) / h;
}
grad
}