use dos_actors::clients::mount::{Mount, MountEncoders, MountSetPoint, MountTorques};
use dos_actors::{clients::arrow_client::Arrow, prelude::*};
use fem::{
dos::{DiscreteModalSolver, ExponentialMatrix},
fem_io::*,
FEM,
};
use lom::{Stats, LOM};
#[tokio::test]
async fn zero_mount_at() -> anyhow::Result<()> {
let sim_sampling_frequency = 1000;
let sim_duration = 4_usize;
let n_step = sim_sampling_frequency * sim_duration;
let state_space = {
let fem = FEM::from_env()?.static_from_env()?;
let n_io = (fem.n_inputs(), fem.n_outputs());
DiscreteModalSolver::<ExponentialMatrix>::from_fem(fem)
.sampling(sim_sampling_frequency as f64)
.proportional_damping(2. / 100.)
.ins::<OSSElDriveTorque>()
.ins::<OSSAzDriveTorque>()
.ins::<OSSRotDriveTorque>()
.outs::<OSSAzEncoderAngle>()
.outs::<OSSElEncoderAngle>()
.outs::<OSSRotEncoderAngle>()
.outs::<OSSM1Lcl>()
.outs::<MCM2Lcl6D>()
.use_static_gain_compensation(n_io)
.build()?
};
let mut source: Initiator<_> = Signals::new(3, n_step).into();
let mut fem: Actor<_> = state_space.into();
let mut mount: Actor<_> = Mount::new().into();
let logging = Arrow::builder(n_step).build().into_arcx();
let mut sink = Terminator::<_>::new(logging.clone());
source
.add_output()
.build::<MountSetPoint>()
.into_input(&mut mount);
mount
.add_output()
.build::<MountTorques>()
.into_input(&mut fem);
fem.add_output()
.bootstrap()
.multiplex(2)
.build::<MountEncoders>()
.into_input(&mut mount)
.logn(&mut sink, 14)
.await;
fem.add_output()
.unbounded()
.build::<OSSM1Lcl>()
.log(&mut sink)
.await;
fem.add_output()
.unbounded()
.build::<MCM2Lcl6D>()
.log(&mut sink)
.await;
Model::new(vec![
Box::new(source),
Box::new(mount),
Box::new(fem),
Box::new(sink),
])
.name("mount")
.flowchart()
.check()?
.run()
.wait()
.await?;
Ok(())
}