autocore_std/fb/

state_machine.rs

use std::time::{Duration, Instant};

/// State Machine Helper (FB_StateMachine)
///
/// A state machine helper with automatic timer management and error tracking.
/// Provides two timers that automatically reset when the state index changes:
///
/// - **Timer** (`timer_done()`) - General purpose timer for delays and debouncing
/// - **Timeout** (`timed_out()`) - For detecting stuck states
///
/// This is equivalent to the IEC 61131-3 FB_StateMachine function block.
///
/// # Automatic Timer Reset
///
/// Both timers automatically reset when `index` changes. This eliminates a
/// common source of bugs in state machines where timers are not properly reset.
///
/// The pattern is:
/// 1. Set `timer_preset` (and optionally `timeout_preset`) in state N
/// 2. Change `index` to state N+1 (timers reset and start counting)
/// 3. In state N+1, check `timer_done()` or `timed_out()`
///
/// # Example
///
/// ```
/// use autocore_std::fb::StateMachine;
/// use std::time::Duration;
///
/// let mut state = StateMachine::new();
///
/// // Simulate a control loop
/// loop {
///     match state.index {
///         0 => { // Reset
///             state.clear_error();
///             state.index = 10;
///         }
///         10 => { // Idle - wait for start signal
///             // For demo, just proceed
///             state.timer_preset = Duration::from_millis(100);
///             state.index = 20;
///         }
///         20 => { // Debounce
///             if state.timer_done() {
///                 state.timeout_preset = Duration::from_secs(10);
///                 state.index = 30;
///             }
///         }
///         30 => { // Wait for operation (simulated)
///             // In real code: check operation_complete
///             // For demo, check timeout
///             if state.timed_out() {
///                 state.set_error(30, "Operation timeout");
///                 state.index = 0;
///             }
///             // Exit demo loop
///             break;
///         }
///         _ => { state.index = 0; }
///     }
///
///     state.call(); // Call at end of each scan cycle
///     # break; // Exit for doctest
/// }
/// ```
///
/// # Timer Presets Persist
///
/// Timer presets persist until you change them. This allows setting a preset
/// once and using it across multiple states:
///
/// ```ignore
/// 100 => {
///     state.timer_preset = Duration::from_millis(300);
///     state.index = 110;
/// }
/// 110 => {
///     // Uses 300ms preset set in state 100
///     if some_condition && state.timer_done() {
///         state.index = 120;
///     }
/// }
/// 120 => {
///     // Still uses 300ms preset (timer reset on state change)
///     if state.timer_done() {
///         state.index = 10;
///     }
/// }
/// ```
///
/// # Error Handling Pattern
///
/// ```ignore
/// 200 => {
///     state.timeout_preset = Duration::from_secs(7);
///     start_operation();
///     state.index = 210;
/// }
/// 210 => {
///     if operation_complete {
///         state.index = 1000; // Success
///     } else if state.timed_out() {
///         state.set_error(210, "Operation timed out");
///         state.index = 5000; // Error handler
///     }
/// }
/// 5000 => {
///     // Error recovery
///     state.index = 0;
/// }
/// ```
#[derive(Debug, Clone)]
pub struct StateMachine {
    /// Current state index.
    pub index: i32,

    /// Timer preset. `timer_done()` returns true when time in current state >= this value.
    /// Defaults to `Duration::MAX` (timer never triggers unless you set a preset).
    pub timer_preset: Duration,

    /// Timeout preset. `timed_out()` returns true when time in current state >= this value.
    /// Defaults to `Duration::MAX` (timeout never triggers unless you set a preset).
    pub timeout_preset: Duration,

    /// Error code. A value of 0 indicates no error.
    /// When non-zero, `is_error()` returns true.
    pub error_code: i32,

    /// Status message for UI display. Content does not indicate an error.
    pub message: String,

    /// Error message for UI display. Should only have content when `error_code != 0`.
    pub error_message: String,

    // Internal state
    last_index: Option<i32>,
    state_entered_at: Option<Instant>,
}

impl StateMachine {
    /// Creates a new state machine starting at state 0.
    ///
    /// Timer presets default to `Duration::MAX`, meaning timers won't trigger
    /// until you explicitly set a preset.
    ///
    /// # Example
    ///
    /// ```
    /// use autocore_std::fb::StateMachine;
    ///
    /// let state = StateMachine::new();
    /// assert_eq!(state.index, 0);
    /// assert_eq!(state.error_code, 0);
    /// assert!(!state.is_error());
    /// ```
    pub fn new() -> Self {
        Self {
            index: 0,
            timer_preset: Duration::MAX,
            timeout_preset: Duration::MAX,
            error_code: 0,
            message: String::new(),
            error_message: String::new(),
            last_index: None,
            state_entered_at: None,
        }
    }

    /// Call once per scan cycle at the END of your state machine logic.
    ///
    /// This method:
    /// - Detects state changes (when `index` differs from the previous call)
    /// - Resets internal timers on state change
    /// - Updates internal tracking for `elapsed()`, `timer_done()`, and `timed_out()`
    ///
    /// # Example
    ///
    /// ```
    /// use autocore_std::fb::StateMachine;
    ///
    /// let mut state = StateMachine::new();
    ///
    /// // Your state machine logic here...
    /// match state.index {
    ///     0 => { state.index = 10; }
    ///     _ => {}
    /// }
    ///
    /// state.call(); // Always call at the end
    /// ```
    pub fn call(&mut self) {
        if self.last_index != Some(self.index) {
            self.state_entered_at = Some(Instant::now());
        }
        self.last_index = Some(self.index);
    }

    /// Returns true when time in current state >= `timer_preset`.
    ///
    /// The timer automatically resets when the state index changes.
    ///
    /// # Example
    ///
    /// ```
    /// use autocore_std::fb::StateMachine;
    /// use std::time::Duration;
    ///
    /// let mut state = StateMachine::new();
    /// state.timer_preset = Duration::from_millis(50);
    /// state.call(); // Start tracking
    ///
    /// assert!(!state.timer_done()); // Not enough time elapsed
    ///
    /// std::thread::sleep(Duration::from_millis(60));
    /// assert!(state.timer_done()); // Now it's done
    /// ```
    pub fn timer_done(&self) -> bool {
        self.elapsed() >= self.timer_preset
    }

    /// Returns true when time in current state >= `timeout_preset`.
    ///
    /// Use this for detecting stuck states. The timeout automatically
    /// resets when the state index changes.
    ///
    /// # Example
    ///
    /// ```
    /// use autocore_std::fb::StateMachine;
    /// use std::time::Duration;
    ///
    /// let mut state = StateMachine::new();
    /// state.timeout_preset = Duration::from_millis(50);
    /// state.call();
    ///
    /// assert!(!state.timed_out());
    ///
    /// std::thread::sleep(Duration::from_millis(60));
    /// assert!(state.timed_out());
    /// ```
    pub fn timed_out(&self) -> bool {
        self.elapsed() >= self.timeout_preset
    }

    /// Returns elapsed time since entering the current state.
    ///
    /// Returns `Duration::ZERO` if `call()` has never been called.
    ///
    /// # Example
    ///
    /// ```
    /// use autocore_std::fb::StateMachine;
    /// use std::time::Duration;
    ///
    /// let mut state = StateMachine::new();
    /// state.call();
    ///
    /// std::thread::sleep(Duration::from_millis(10));
    /// assert!(state.elapsed() >= Duration::from_millis(10));
    /// ```
    pub fn elapsed(&self) -> Duration {
        self.state_entered_at
            .map(|t| t.elapsed())
            .unwrap_or(Duration::ZERO)
    }

    /// Returns true if `error_code != 0`.
    ///
    /// # Example
    ///
    /// ```
    /// use autocore_std::fb::StateMachine;
    ///
    /// let mut state = StateMachine::new();
    /// assert!(!state.is_error());
    ///
    /// state.error_code = 100;
    /// assert!(state.is_error());
    /// ```
    pub fn is_error(&self) -> bool {
        self.error_code != 0
    }

    /// Set error state with code and message.
    ///
    /// This is a convenience method equivalent to setting `error_code`
    /// and `error_message` directly.
    ///
    /// # Example
    ///
    /// ```
    /// use autocore_std::fb::StateMachine;
    ///
    /// let mut state = StateMachine::new();
    /// state.set_error(110, "Failed to home X axis");
    ///
    /// assert_eq!(state.error_code, 110);
    /// assert_eq!(state.error_message, "Failed to home X axis");
    /// assert!(state.is_error());
    /// ```
    pub fn set_error(&mut self, code: i32, message: impl Into<String>) {
        self.error_code = code;
        self.error_message = message.into();
    }

    /// Clear error state.
    ///
    /// Sets `error_code` to 0 and clears `error_message`.
    ///
    /// # Example
    ///
    /// ```
    /// use autocore_std::fb::StateMachine;
    ///
    /// let mut state = StateMachine::new();
    /// state.set_error(100, "Some error");
    /// assert!(state.is_error());
    ///
    /// state.clear_error();
    /// assert!(!state.is_error());
    /// assert_eq!(state.error_code, 0);
    /// assert!(state.error_message.is_empty());
    /// ```
    pub fn clear_error(&mut self) {
        self.error_code = 0;
        self.error_message.clear();
    }

    /// Returns the current state index.
    ///
    /// This is equivalent to reading `state.index` directly but provided
    /// for API consistency.
    pub fn state(&self) -> i32 {
        self.index
    }
}

impl Default for StateMachine {
    fn default() -> Self {
        Self::new()
    }
}

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

    #[test]
    fn test_state_machine_basic() {
        let state = StateMachine::new();

        assert_eq!(state.index, 0);
        assert_eq!(state.error_code, 0);
        assert!(!state.is_error());
        assert_eq!(state.timer_preset, Duration::MAX);
        assert_eq!(state.timeout_preset, Duration::MAX);
    }

    #[test]
    fn test_state_machine_timer() {
        let mut state = StateMachine::new();
        state.timer_preset = Duration::from_millis(50);
        state.call();

        // Timer shouldn't be done yet
        assert!(!state.timer_done());

        // Wait for timer
        std::thread::sleep(Duration::from_millis(60));
        assert!(state.timer_done());
        assert!(state.elapsed() >= Duration::from_millis(50));
    }

    #[test]
    fn test_state_machine_timeout() {
        let mut state = StateMachine::new();
        state.timeout_preset = Duration::from_millis(50);
        state.call();

        assert!(!state.timed_out());

        std::thread::sleep(Duration::from_millis(60));
        assert!(state.timed_out());
    }

    #[test]
    fn test_state_machine_timer_reset_on_state_change() {
        let mut state = StateMachine::new();
        state.timer_preset = Duration::from_millis(50);
        state.call();

        // Wait a bit
        std::thread::sleep(Duration::from_millis(30));
        let elapsed_before = state.elapsed();
        assert!(elapsed_before >= Duration::from_millis(30));

        // Change state
        state.index = 10;
        state.call();

        // Timer should have reset
        assert!(state.elapsed() < Duration::from_millis(20));
        assert!(!state.timer_done());
    }

    #[test]
    fn test_state_machine_error_handling() {
        let mut state = StateMachine::new();

        assert!(!state.is_error());

        state.set_error(110, "Failed to home axis");
        assert!(state.is_error());
        assert_eq!(state.error_code, 110);
        assert_eq!(state.error_message, "Failed to home axis");

        state.clear_error();
        assert!(!state.is_error());
        assert_eq!(state.error_code, 0);
        assert!(state.error_message.is_empty());
    }

    #[test]
    fn test_state_machine_preset_persists() {
        let mut state = StateMachine::new();

        // Set preset in state 0
        state.timer_preset = Duration::from_millis(50);
        state.index = 10;
        state.call();

        // Preset should still be 50ms
        assert_eq!(state.timer_preset, Duration::from_millis(50));

        // Change to state 20 without changing preset
        state.index = 20;
        state.call();

        // Preset still 50ms
        assert_eq!(state.timer_preset, Duration::from_millis(50));
    }

    #[test]
    fn test_state_machine_default_presets_never_trigger() {
        let mut state = StateMachine::new();
        state.call();

        // Default presets are Duration::MAX, so timers should never trigger
        std::thread::sleep(Duration::from_millis(10));
        assert!(!state.timer_done());
        assert!(!state.timed_out());
    }
}