autocore_std/motion/

axis_wait.rs

//! Common wait-for-axis-done function block.
//!
//! Most state machines that command a motion axis end up writing the same
//! 15-line wait handler over and over: poll `is_busy`, then check `is_error`,
//! then handle `state.timed_out`, then route to the next state. [`AxisWait`]
//! collapses that into a single call returning a [`WaitStatus`] the caller
//! matches on.
//!
//! It is stateless — all state lives in the [`AxisHandle`] (the drive) and
//! the caller's [`StateMachine`] (the timeout). Because it takes any
//! `&impl AxisHandle`, the same call works for `AxisX` / `AxisY` / `AxisZ`
//! / `AxisC` or any custom handle.
//!
//! # Example
//!
//! ```ignore
//! use autocore_std::motion::axis_wait::{AxisWait, WaitStatus};
//!
//! Some(MyState::WaitMoveDone) => {
//!     match AxisWait::poll(axis_z, &self.state) {
//!         WaitStatus::Pending => {}
//!         WaitStatus::Done => {
//!             self.state.index = MyState::NextStep as i32;
//!         }
//!         WaitStatus::Error(msg) => {
//!             log::error!("Z axis error: {}", msg);
//!             self.state.index = MyState::Reset as i32;
//!         }
//!         WaitStatus::Timeout => {
//!             log::error!("Timeout waiting for Z motion");
//!             self.state.index = MyState::Reset as i32;
//!         }
//!     }
//! }
//! ```
//!
//! # Pairing with a `follow_up_state` enum
//!
//! State machines that re-use one wait state across many transitions
//! typically pair this with an `Option<MyState>` field set by the
//! command-issuing state. The `Done` arm reads + clears it:
//!
//! ```ignore
//! WaitStatus::Done => {
//!     self.state.index = self.follow_up_state
//!         .take()
//!         .map(|s| s as i32)
//!         .unwrap_or(MyState::Idle as i32);
//! }
//! ```

use crate::fb::StateMachine;
use super::axis_view::AxisHandle;

/// Result of a single [`AxisWait::poll`] call.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum WaitStatus {
    /// Axis is still executing the command and the timeout has not
    /// fired. Caller should stay in the wait state.
    Pending,
    /// Axis finished cleanly. Caller should advance to its follow-up state.
    Done,
    /// Axis reported an error before completion. Payload is the drive's
    /// human-readable error message (empty if the handle's `error_message`
    /// override hasn't been wired). Caller should log and recover.
    Error(String),
    /// `StateMachine.timed_out()` fired while the axis was still busy.
    /// Caller should log and recover.
    Timeout,
}

/// Wait-for-axis-done function block. Stateless — all state lives in
/// the drive and the caller's [`StateMachine`].
pub struct AxisWait;

impl AxisWait {
    /// Poll the axis exactly once. Call this from inside your wait state.
    ///
    /// Decision order:
    ///
    /// 1. **`!is_busy && is_error`** → [`WaitStatus::Error`] with the
    ///    drive's `error_message()`. Checked first because the busy bit
    ///    typically clears before the error bit on a faulted move.
    /// 2. **`!is_busy`** → [`WaitStatus::Done`].
    /// 3. **`state.timed_out()`** → [`WaitStatus::Timeout`].
    /// 4. Otherwise → [`WaitStatus::Pending`].
    ///
    /// `error_message` defaults to `""` if the handle hasn't overridden
    /// it (see the [`AxisHandle`] trait); regenerated `Axis*` handles
    /// override it to return the drive's actual message.
    pub fn poll<A: AxisHandle + ?Sized>(axis: &A, state: &StateMachine) -> WaitStatus {
        if !axis.is_busy() {
            if axis.is_error() {
                WaitStatus::Error(axis.error_message())
            } else {
                WaitStatus::Done
            }
        } else if state.timed_out() {
            WaitStatus::Timeout
        } else {
            WaitStatus::Pending
        }
    }

    /// Convenience wrapper for the common pattern: on `Done`, jump to
    /// the follow-up state (or `default_next` if no follow-up was set);
    /// on `Error` or `Timeout`, log and jump to `error_next`. Returns
    /// the new state index, or `None` if the caller should stay put
    /// (the `Pending` case).
    ///
    /// # Example
    ///
    /// ```ignore
    /// Some(MyState::WaitMoveDone) => {
    ///     if let Some(next) = AxisWait::resolve(
    ///         axis_z,
    ///         &self.state,
    ///         &mut self.follow_up_state,
    ///         MyState::Idle as i32,
    ///         MyState::Reset as i32,
    ///         "Z axis",
    ///     ) {
    ///         self.state.index = next;
    ///     }
    /// }
    /// ```
    pub fn resolve<A: AxisHandle + ?Sized>(
        axis:         &A,
        state:        &StateMachine,
        follow_up:    &mut Option<i32>,
        default_next: i32,
        error_next:   i32,
        label:        &str,
    ) -> Option<i32> {
        match Self::poll(axis, state) {
            WaitStatus::Pending => None,
            WaitStatus::Done => {
                let next = follow_up.take().unwrap_or(default_next);
                log::info!("{label} command complete → state {next}");
                Some(next)
            }
            WaitStatus::Error(msg) => {
                log::error!("{label} error: {msg}");
                Some(error_next)
            }
            WaitStatus::Timeout => {
                log::error!("Timeout waiting for {label}");
                Some(error_next)
            }
        }
    }
}

// -------------------------------------------------------------------------
// Tests
// -------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;
    use crate::motion::axis_config::AxisConfig;

    /// Mock implementation of AxisHandle for unit tests.
    struct MockAxis {
        busy:    bool,
        error:   bool,
        message: String,
        config:  AxisConfig,
    }

    impl Default for MockAxis {
        fn default() -> Self {
            Self {
                busy:    false,
                error:   false,
                message: String::new(),
                config:  AxisConfig::new(1000),
            }
        }
    }

    impl AxisHandle for MockAxis {
        fn position(&self) -> f64 { 0.0 }
        fn config(&self) -> &AxisConfig { &self.config }
        fn move_relative(&mut self, _: f64, _: f64, _: f64, _: f64) {}
        fn move_absolute(&mut self, _: f64, _: f64, _: f64, _: f64) {}
        fn halt(&mut self) {}
        fn is_busy(&self) -> bool { self.busy }
        fn is_error(&self) -> bool { self.error }
        fn error_message(&self) -> String { self.message.clone() }
        fn motor_on(&self) -> bool { true }
    }

    #[test]
    fn poll_pending_when_busy_and_not_timed_out() {
        let axis = MockAxis { busy: true, ..Default::default() };
        let state = StateMachine::new();
        assert_eq!(AxisWait::poll(&axis, &state), WaitStatus::Pending);
    }

    #[test]
    fn poll_done_when_idle_and_no_error() {
        let axis = MockAxis { busy: false, error: false, ..Default::default() };
        let state = StateMachine::new();
        assert_eq!(AxisWait::poll(&axis, &state), WaitStatus::Done);
    }

    #[test]
    fn poll_error_takes_priority_over_done_when_idle() {
        let axis = MockAxis {
            busy: false, error: true,
            message: "fault: overcurrent".into(),
            ..Default::default()
        };
        let state = StateMachine::new();
        assert_eq!(
            AxisWait::poll(&axis, &state),
            WaitStatus::Error("fault: overcurrent".to_string()),
        );
    }

    #[test]
    fn poll_timeout_only_when_still_busy() {
        // Build a state that is timed out.
        let mut state = StateMachine::new();
        state.timeout_preset = std::time::Duration::from_millis(0);
        // timed_out() compares against an internal timer — sleeping a
        // millisecond is enough to ensure the elapsed > preset.
        std::thread::sleep(std::time::Duration::from_millis(2));

        let busy_axis = MockAxis { busy: true, ..Default::default() };
        assert_eq!(AxisWait::poll(&busy_axis, &state), WaitStatus::Timeout);

        // Same state, but not busy → Done wins over Timeout.
        let idle_axis = MockAxis { busy: false, ..Default::default() };
        assert_eq!(AxisWait::poll(&idle_axis, &state), WaitStatus::Done);
    }

    #[test]
    fn resolve_returns_none_when_pending() {
        let axis = MockAxis { busy: true, ..Default::default() };
        let state = StateMachine::new();
        let mut follow_up: Option<i32> = Some(42);
        let next = AxisWait::resolve(&axis, &state, &mut follow_up, 0, 99, "test");
        assert_eq!(next, None);
        assert_eq!(follow_up, Some(42), "follow_up must not be consumed on Pending");
    }

    #[test]
    fn resolve_consumes_follow_up_on_done() {
        let axis = MockAxis { busy: false, ..Default::default() };
        let state = StateMachine::new();
        let mut follow_up: Option<i32> = Some(42);
        let next = AxisWait::resolve(&axis, &state, &mut follow_up, 0, 99, "test");
        assert_eq!(next, Some(42));
        assert_eq!(follow_up, None);
    }

    #[test]
    fn resolve_uses_default_when_no_follow_up_on_done() {
        let axis = MockAxis { busy: false, ..Default::default() };
        let state = StateMachine::new();
        let mut follow_up: Option<i32> = None;
        let next = AxisWait::resolve(&axis, &state, &mut follow_up, 7, 99, "test");
        assert_eq!(next, Some(7));
    }

    #[test]
    fn resolve_routes_to_error_next_on_error() {
        let axis = MockAxis {
            busy: false, error: true,
            message: "x".into(), ..Default::default()
        };
        let state = StateMachine::new();
        let mut follow_up: Option<i32> = Some(42);
        let next = AxisWait::resolve(&axis, &state, &mut follow_up, 0, 99, "test");
        assert_eq!(next, Some(99));
        assert_eq!(follow_up, Some(42), "follow_up retained on Error so caller can re-attempt");
    }
}