autd3-protobuf 25.1.0

use autd3_gain_holo::{LinAlgBackend, NalgebraBackend};

use crate::{
    pb::*,
    to_holo,
    traits::{FromMessage, ToMessage},
};

impl ToMessage
    for autd3_gain_holo::LM<
        autd3_driver::acoustics::directivity::Sphere,
        NalgebraBackend<autd3_driver::acoustics::directivity::Sphere>,
    >
{
    type Message = DatagramLightweight;

    #[allow(clippy::unnecessary_cast)]
    fn to_msg(&self, _: Option<&autd3_driver::geometry::Geometry>) -> Self::Message {
        Self::Message {
            datagram: Some(datagram_lightweight::Datagram::Gain(Gain {
                gain: Some(gain::Gain::Lm(Lm {
                    holo: to_holo!(self),
                    eps_1: self.eps_1() as _,
                    eps_2: self.eps_2() as _,
                    tau: self.tau() as _,
                    k_max: self.k_max() as _,
                    initial: self.initial().iter().map(|&v| v as _).collect(),
                    constraint: Some(self.constraint().to_msg(None)),
                })),
                segment: Segment::S0 as _,
                transition: true,
            })),
        }
    }
}

impl ToMessage
    for autd3_driver::datagram::DatagramWithSegment<
        autd3_gain_holo::LM<
            autd3_driver::acoustics::directivity::Sphere,
            NalgebraBackend<autd3_driver::acoustics::directivity::Sphere>,
        >,
    >
{
    type Message = DatagramLightweight;

    #[allow(clippy::unnecessary_cast)]
    fn to_msg(&self, _: Option<&autd3_driver::geometry::Geometry>) -> Self::Message {
        Self::Message {
            datagram: Some(datagram_lightweight::Datagram::Gain(Gain {
                gain: Some(gain::Gain::Lm(Lm {
                    holo: to_holo!(self),
                    eps_1: self.eps_1() as _,
                    eps_2: self.eps_2() as _,
                    tau: self.tau() as _,
                    k_max: self.k_max() as _,
                    initial: self.initial().iter().map(|&v| v as _).collect(),
                    constraint: Some(self.constraint().to_msg(None)),
                })),
                segment: self.segment() as _,
                transition: self.transition(),
            })),
        }
    }
}

impl FromMessage<Lm>
    for autd3_gain_holo::LM<
        autd3_driver::acoustics::directivity::Sphere,
        NalgebraBackend<autd3_driver::acoustics::directivity::Sphere>,
    >
{
    #[allow(clippy::unnecessary_cast)]
    fn from_msg(msg: &Lm) -> Option<Self> {
        Some(
            Self::new(
                NalgebraBackend::new().ok()?,
                msg.holo
                    .iter()
                    .map(|h| {
                        Some((
                            autd3_driver::geometry::Vector3::from_msg(h.pos.as_ref()?)?,
                            h.amp.as_ref()?.value as f32 * autd3_gain_holo::Pa,
                        ))
                    })
                    .collect::<Option<Vec<_>>>()?,
            )
            .with_eps_1(msg.eps_1 as _)
            .with_eps_2(msg.eps_2 as _)
            .with_tau(msg.tau as _)
            .with_k_max(msg.k_max as _)
            .with_initial(msg.initial.iter().map(|&v| v as _).collect())
            .with_constraint(autd3_gain_holo::EmissionConstraint::from_msg(
                msg.constraint.as_ref()?,
            )?),
        )
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use autd3_driver::geometry::Vector3;
    use rand::Rng;

    #[test]
    fn test_holo_sdp() {
        let mut rng = rand::thread_rng();

        let holo = autd3_gain_holo::LM::new(
            NalgebraBackend::new().unwrap(),
            [
                (
                    Vector3::new(rng.gen(), rng.gen(), rng.gen()),
                    rng.gen::<f32>() * autd3_gain_holo::Pa,
                ),
                (
                    Vector3::new(rng.gen(), rng.gen(), rng.gen()),
                    rng.gen::<f32>() * autd3_gain_holo::Pa,
                ),
            ],
        )
        .with_eps_1(rng.gen())
        .with_eps_2(rng.gen())
        .with_tau(rng.gen())
        .with_k_max(rng.gen())
        .with_initial(vec![rng.gen(), rng.gen(), rng.gen()]);
        let msg = holo.to_msg(None);

        match msg.datagram {
            Some(datagram_lightweight::Datagram::Gain(Gain {
                gain: Some(gain::Gain::Lm(g)),
                ..
            })) => {
                let holo2 = autd3_gain_holo::LM::from_msg(&g).unwrap();
                assert_approx_eq::assert_approx_eq!(holo.eps_1(), holo2.eps_1());
                assert_approx_eq::assert_approx_eq!(holo.eps_2(), holo2.eps_2());
                assert_approx_eq::assert_approx_eq!(holo.tau(), holo2.tau());
                assert_eq!(holo.k_max(), holo2.k_max());
                holo.initial()
                    .iter()
                    .zip(holo2.initial().iter())
                    .for_each(|(v1, v2)| {
                        assert_approx_eq::assert_approx_eq!(v1, v2);
                    });
                assert_eq!(holo.constraint(), holo2.constraint());
                holo.foci()
                    .iter()
                    .zip(holo2.foci().iter())
                    .for_each(|(f1, f2)| {
                        assert_approx_eq::assert_approx_eq!(f1.x, f2.x);
                        assert_approx_eq::assert_approx_eq!(f1.y, f2.y);
                        assert_approx_eq::assert_approx_eq!(f1.z, f2.z);
                    });
                holo.amps()
                    .iter()
                    .zip(holo2.amps().iter())
                    .for_each(|(f1, f2)| {
                        assert_approx_eq::assert_approx_eq!(f1.pascal(), f2.pascal());
                    });
            }
            _ => panic!("unexpected datagram type"),
        }
    }
}