use std::marker::PhantomData;
use nalgebra::{
allocator::Allocator, convert, storage::StorageMut, ComplexField, DefaultAllocator, DimMin,
DimName, IsContiguous, OVector, Vector, U1,
};
use thiserror::Error;
use crate::{
core::{Domain, Problem, ProblemError, System},
derivatives::{Jacobian, JacobianError, EPSILON_SQRT},
};
#[derive(Debug, Error)]
pub enum InitialGuessAnalysisError {
#[error("{0}")]
System(#[from] ProblemError),
#[error("{0}")]
Jacobian(#[from] JacobianError),
}
pub struct InitialGuessAnalysis<F: Problem> {
nonlinear: Vec<usize>,
ty: PhantomData<F>,
}
impl<F: System> InitialGuessAnalysis<F>
where
DefaultAllocator: Allocator<F::Scalar, F::Dim>,
DefaultAllocator: Allocator<F::Scalar, F::Dim, F::Dim>,
F::Dim: DimMin<F::Dim, Output = F::Dim>,
DefaultAllocator: Allocator<F::Scalar, <F::Dim as DimMin<F::Dim>>::Output>,
{
pub fn analyze<Sx, Sfx>(
f: &F,
dom: &Domain<F::Scalar>,
x: &mut Vector<F::Scalar, F::Dim, Sx>,
fx: &mut Vector<F::Scalar, F::Dim, Sfx>,
) -> Result<Self, InitialGuessAnalysisError>
where
Sx: StorageMut<F::Scalar, F::Dim> + IsContiguous,
Sfx: StorageMut<F::Scalar, F::Dim>,
{
let scale_iter = dom.vars().iter().map(|var| var.scale());
let scale = OVector::from_iterator_generic(f.dim(), U1::name(), scale_iter);
f.eval(x, fx)?;
let jac1 = Jacobian::new(f, x, &scale, fx)?;
let mut p = fx.clone_owned();
p.neg_mut();
let qr = jac1.clone_owned().qr();
qr.solve_mut(&mut p);
p *= convert::<_, F::Scalar>(0.001);
*x += p;
f.eval(x, fx)?;
let jac2 = Jacobian::new(f, x, &scale, fx)?;
let nonlinear = jac1
.column_iter()
.zip(jac2.column_iter())
.enumerate()
.filter(|(_, (c1, c2))| {
c1.iter()
.zip(c2.iter())
.any(|(a, b)| (*a - *b).abs() > convert(EPSILON_SQRT))
})
.map(|(col, _)| col)
.collect();
Ok(Self {
nonlinear,
ty: PhantomData,
})
}
pub fn nonlinear(&self) -> &[usize] {
&self.nonlinear
}
}