use crate::fem_io::{GetIn, GetOut};
use super::{DiscreteStateSpace, Exponential, ExponentialMatrix, Solver};
use gmt_fem::{Result, FEM};
use interface::TimerMarker;
use nalgebra as na;
use rayon::prelude::*;
use std::{
collections::HashMap,
fmt,
sync::Arc,
thread::{self, JoinHandle},
};
impl<T: Solver + Default> TimerMarker for DiscreteModalSolver<T> {}
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Debug, Default)]
pub struct PsiTimesU {
data: Vec<f64>,
#[cfg_attr(feature = "serde", serde(skip))]
handler: Option<JoinHandle<na::DVector<f64>>>,
}
impl PsiTimesU {
pub fn mul(&mut self, u: &[f64], mat: &Arc<na::DMatrix<f64>>) {
let clone_mat = Arc::clone(mat);
let vec_u = na::DVector::from_column_slice(u);
self.handler = Some(thread::spawn(move || &*clone_mat * vec_u));
}
pub fn join(&mut self) -> &[f64] {
self.handler.take().map(|h| {
let y = h.join().unwrap();
if self.data.is_empty() {
self.data = y.as_slice().to_vec();
} else {
self.data.copy_from_slice(y.as_slice());
}
});
self.data.as_slice()
}
}
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Debug, Default)]
pub struct DiscreteModalSolver<T: Solver + Default> {
pub u: Vec<f64>,
pub y: Vec<f64>,
pub y_sizes: Vec<usize>,
pub state_space: Vec<T>,
pub psi_dcg: Option<Arc<na::DMatrix<f64>>>,
pub psi_times_u: PsiTimesU,
pub ins: Vec<Box<dyn GetIn>>,
pub outs: Vec<Box<dyn GetOut>>,
pub facesheet_nodes: Option<HashMap<u8, Vec<f64>>>,
}
impl<T: Solver + Default> DiscreteModalSolver<T> {
pub fn from_fem(fem: FEM) -> DiscreteStateSpace<'static, T> {
fem.into()
}
pub fn from_env() -> Result<DiscreteStateSpace<'static, T>> {
let fem = FEM::from_env()?;
Ok(DiscreteModalSolver::from_fem(fem))
}
}
impl Iterator for DiscreteModalSolver<Exponential> {
type Item = ();
fn next(&mut self) -> Option<Self::Item> {
let n = self.y.len();
let _u_ = &self.u;
self.y = self
.state_space
.par_iter_mut()
.fold(
|| vec![0f64; n],
|mut a: Vec<f64>, m| {
a.iter_mut().zip(m.solve(_u_)).for_each(|(yc, y)| {
*yc += y;
});
a
},
)
.reduce(
|| vec![0f64; n],
|mut a: Vec<f64>, b: Vec<f64>| {
a.iter_mut().zip(b.iter()).for_each(|(a, b)| {
*a += *b;
});
a
},
);
Some(())
}
}
impl Iterator for DiscreteModalSolver<ExponentialMatrix> {
type Item = ();
fn next(&mut self) -> Option<Self::Item> {
let n = self.y.len();
let _u_ = &self.u;
self.y = self
.state_space
.par_iter_mut()
.fold(
|| vec![0f64; n],
|mut a: Vec<f64>, m| {
a.iter_mut().zip(m.solve(_u_)).for_each(|(yc, y)| {
*yc += y;
});
a
},
)
.reduce(
|| vec![0f64; n],
|mut a: Vec<f64>, b: Vec<f64>| {
a.iter_mut().zip(b.iter()).for_each(|(a, b)| {
*a += *b;
});
a
},
);
if let Some(psi_dcg) = &self.psi_dcg {
let psi_times_u = self.psi_times_u.join();
self.y
.iter_mut()
.zip(psi_times_u)
.for_each(|(v1, v2)| *v1 += *v2);
self.psi_times_u.mul(&self.u, psi_dcg);
}
Some(())
}
}
impl<T: Solver + Default> fmt::Display for DiscreteModalSolver<T> {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(
f,
r##"
DiscreteModalSolver:
- inputs ({}):
{:}
- outputs ({}):
{:}
- {:} 2x2 state space models
"##,
self.u.len(),
self.ins
.iter()
.map(|x| x.fem_type())
.collect::<Vec<String>>()
.join("\n"),
self.y.len(),
self.outs
.iter()
.map(|x| x.fem_type())
.collect::<Vec<String>>()
.join("\n"),
self.state_space.len(),
)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::fem_io::actors_inputs::OSSElDriveTorque;
use crate::fem_io::actors_outputs::OSSElEncoderAngle;
use gmt_fem::FEM;
#[test]
fn serde() {
let state_space = {
let fem = FEM::from_env().unwrap();
DiscreteModalSolver::<ExponentialMatrix>::from_fem(fem)
.sampling(1e3)
.max_eigen_frequency(0.1)
.ins::<OSSElDriveTorque>()
.outs::<OSSElEncoderAngle>()
.build()
.unwrap()
};
dbg!(&state_space);
let json = serde_json::to_string(&state_space).unwrap();
println!("{:#}", &json);
let q: DiscreteModalSolver<ExponentialMatrix> = serde_json::from_str(&json).unwrap();
dbg!(&q);
}
}