orts 0.2.0

//! Command -> actuator bridge.
//!
//! `ActuatorBundle` holds the most recent command emitted by a
//! controller backend, decomposed into per-actuator fields. The host
//! uses this struct to translate the plugin-layer `Command` struct into
//! concrete physical actuator models (`MtqAssembly`, `RwAssembly`,
//! `DynamicThrottle` + `Thruster`) when assembling the ODE system
//! for the next zero-order-hold segment.
//!
//! The bundle does NOT own the actuator model instances themselves.
//! The caller rebuilds the model at each segment boundary using the
//! applied commanded state stored here. This keeps the bundle free of
//! generic parameters on environment-model types and lets different
//! backends / tests pick their own field model independently.
//!
//! ## Multi-command semantics
//!
//! Each `apply()` call updates the actuators for which the `Command`
//! has `Some` fields. Other actuators retain their last value
//! (zero-order hold). If a guest sets both `mtq` and `rw` in a single
//! `Command`, both actuators are updated simultaneously.

use super::command::{Command, MtqCommand, RwCommand, ThrusterCommand};
use super::error::PluginError;

/// Per-actuator applied command state.
#[derive(Debug, Clone, Default)]
pub struct ActuatorBundle {
    /// Per-MTQ commanded command (direct moments or normalized).
    /// `None` until `apply()` receives a `Command` with `mtq`.
    commanded_mtq: Option<MtqCommand>,

    /// Per-wheel commanded RW command (speed or torque).
    /// `None` until `apply()` receives a `Command` with `rw`.
    commanded_rw: Option<RwCommand>,

    /// Per-thruster commanded throttle.
    /// `None` until `apply()` receives a `Command` with `thruster`.
    commanded_thruster: Option<ThrusterCommand>,
}

impl ActuatorBundle {
    /// Create an empty bundle with no actuators armed.
    pub fn new() -> Self {
        Self::default()
    }

    /// Apply a command, updating the corresponding actuator(s)' state.
    ///
    /// Rejects non-finite commands before they can poison downstream
    /// actuator models (NaN guard, see DESIGN.md Phase P 落とし穴リスト).
    ///
    /// Only actuators for which the `Command` has `Some` fields are
    /// updated; other actuators retain their previous value (zero-order
    /// hold).
    pub fn apply(&mut self, cmd: &Command) -> Result<(), PluginError> {
        if !cmd.is_finite() {
            return Err(PluginError::BadCommand(format!("{cmd:?}")));
        }
        if let Some(mtq) = &cmd.mtq {
            self.commanded_mtq = Some(mtq.clone());
        }
        if let Some(rw) = &cmd.rw {
            self.commanded_rw = Some(rw.clone());
        }
        if let Some(thruster) = &cmd.thruster {
            self.commanded_thruster = Some(thruster.clone());
        }
        Ok(())
    }

    /// Returns the currently-commanded MTQ command, if any was ever
    /// applied.
    pub fn mtq_command(&self) -> Option<&MtqCommand> {
        self.commanded_mtq.as_ref()
    }

    /// Returns `true` if an MTQ command has been applied at least once.
    pub fn has_mtq_command(&self) -> bool {
        self.commanded_mtq.is_some()
    }

    /// Returns the currently-commanded RW command.
    pub fn rw_command(&self) -> Option<&RwCommand> {
        self.commanded_rw.as_ref()
    }

    /// Returns `true` if an RW command has been applied at least once.
    pub fn has_rw_command(&self) -> bool {
        self.commanded_rw.is_some()
    }

    /// Returns the currently-commanded thruster command.
    pub fn thruster_command(&self) -> Option<&ThrusterCommand> {
        self.commanded_thruster.as_ref()
    }

    /// Returns `true` if a thruster command has been applied at least once.
    pub fn has_thruster_command(&self) -> bool {
        self.commanded_thruster.is_some()
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn apply_stores_mtq_moments() {
        let mut bundle = ActuatorBundle::new();
        assert!(!bundle.has_mtq_command());
        assert!(bundle.mtq_command().is_none());

        bundle.apply(&Command::mtq(vec![1.0, -2.0, 3.0])).unwrap();
        assert!(bundle.has_mtq_command());
        assert_eq!(
            bundle.mtq_command(),
            Some(&MtqCommand::Moments(vec![1.0, -2.0, 3.0]))
        );
    }

    #[test]
    fn apply_stores_mtq_normalized() {
        let mut bundle = ActuatorBundle::new();
        bundle
            .apply(&Command::mtq_normalized(vec![0.5, -1.0, 0.0]))
            .unwrap();
        assert!(bundle.has_mtq_command());
        assert_eq!(
            bundle.mtq_command(),
            Some(&MtqCommand::NormalizedMoments(vec![0.5, -1.0, 0.0]))
        );
    }

    #[test]
    fn apply_stores_rw_torques() {
        let mut bundle = ActuatorBundle::new();
        assert!(!bundle.has_rw_command());

        bundle
            .apply(&Command::rw_torques(vec![0.01, -0.02, 0.03]))
            .unwrap();
        assert!(bundle.has_rw_command());
        assert_eq!(
            bundle.rw_command(),
            Some(&RwCommand::Torques(vec![0.01, -0.02, 0.03]))
        );
    }

    #[test]
    fn apply_stores_rw_speeds() {
        let mut bundle = ActuatorBundle::new();
        bundle
            .apply(&Command::rw_speeds(vec![10.0, -5.0, 0.0]))
            .unwrap();
        assert!(bundle.has_rw_command());
        assert_eq!(
            bundle.rw_command(),
            Some(&RwCommand::Speeds(vec![10.0, -5.0, 0.0]))
        );
    }

    #[test]
    fn apply_rejects_nan() {
        let mut bundle = ActuatorBundle::new();
        let bad = Command::mtq(vec![1.0, f64::NAN, 0.0]);
        let err = bundle.apply(&bad).unwrap_err();
        match err {
            PluginError::BadCommand(_) => {}
            other => panic!("unexpected error: {other:?}"),
        }
        assert!(!bundle.has_mtq_command());
    }

    #[test]
    fn apply_rw_rejects_nan() {
        let mut bundle = ActuatorBundle::new();
        let bad = Command::rw_torques(vec![0.0, f64::INFINITY, 0.0]);
        assert!(bundle.apply(&bad).is_err());
        assert!(!bundle.has_rw_command());
    }

    #[test]
    fn multi_command_retains_both() {
        let mut bundle = ActuatorBundle::new();
        bundle.apply(&Command::mtq(vec![1.0, 0.0, 0.0])).unwrap();
        bundle
            .apply(&Command::rw_torques(vec![0.0, 0.1, 0.0]))
            .unwrap();
        assert_eq!(
            bundle.mtq_command(),
            Some(&MtqCommand::Moments(vec![1.0, 0.0, 0.0]))
        );
        assert_eq!(
            bundle.rw_command(),
            Some(&RwCommand::Torques(vec![0.0, 0.1, 0.0]))
        );
    }

    #[test]
    fn single_command_with_both_fields() {
        let mut bundle = ActuatorBundle::new();
        let cmd = Command {
            mtq: Some(MtqCommand::Moments(vec![1.0, 0.0, 0.0])),
            rw: Some(RwCommand::Torques(vec![0.0, 0.1, 0.0])),
            thruster: None,
        };
        bundle.apply(&cmd).unwrap();
        assert_eq!(
            bundle.mtq_command(),
            Some(&MtqCommand::Moments(vec![1.0, 0.0, 0.0]))
        );
        assert_eq!(
            bundle.rw_command(),
            Some(&RwCommand::Torques(vec![0.0, 0.1, 0.0]))
        );
    }

    #[test]
    fn apply_stores_thruster() {
        let mut bundle = ActuatorBundle::new();
        assert!(!bundle.has_thruster_command());
        assert!(bundle.thruster_command().is_none());

        bundle
            .apply(&Command::thruster(vec![0.5, 1.0, 0.0]))
            .unwrap();
        assert!(bundle.has_thruster_command());
        assert_eq!(
            bundle.thruster_command(),
            Some(&ThrusterCommand::Throttles(vec![0.5, 1.0, 0.0]))
        );
    }

    #[test]
    fn apply_thruster_rejects_nan() {
        let mut bundle = ActuatorBundle::new();
        let bad = Command::thruster(vec![0.5, f64::NAN, 0.0]);
        assert!(bundle.apply(&bad).is_err());
        assert!(!bundle.has_thruster_command());
    }

    #[test]
    fn multi_command_retains_all_three() {
        let mut bundle = ActuatorBundle::new();
        bundle.apply(&Command::mtq(vec![1.0])).unwrap();
        bundle.apply(&Command::rw_torques(vec![0.1])).unwrap();
        bundle.apply(&Command::thruster(vec![0.5])).unwrap();
        assert!(bundle.has_mtq_command());
        assert!(bundle.has_rw_command());
        assert!(bundle.has_thruster_command());
    }
}