autocore-std 3.3.33

//! Command Interface protocol.
//!
//! A handshake-based command protocol for receiving commands from external sources
//! (UI, HMI, other modules) via shared memory. Based on CANopen/EtherCAT state
//! machine conventions.
//!
//! # Protocol
//!
//! The external source (e.g. HMI) and the control program communicate through
//! six shared memory fields, managed via a [`CommandInterfaceView`]:
//!
//! | Field | Direction | Description |
//! |-------|-----------|-------------|
//! | `command` | External → Control | Request code ([`CommandRequest`] values) |
//! | `command_code` | External → Control | Identifies the specific command |
//! | `command_arg_1` | External → Control | First argument (meaning defined per command code) |
//! | `command_arg_2` | External → Control | Second argument |
//! | `command_status` | Control → External | Current status ([`CommandStatus`] values) |
//! | `command_result` | Control → External | Result value from completed command |
//!
//! # Handshake Sequence
//!
//! ```text
//! External                          Control Program
//! ────────                          ───────────────
//! 1. Write command_code, arg_1, arg_2
//! 2. Set command = EXEC (11)
//!                                   3. Sees EXEC → status = EXECUTING (20)
//!                                      call() returns Some(command_code)
//!                                   4. Process the command...
//!                                   5. set_done() → status = DONE (100)
//! 6. Read command_result
//! 7. Set command = ACKDONE (101)
//!                                   8. Sees ACKDONE → status = IDLE (10)
//! ```
//!
//! # Example
//!
//! ```
//! use autocore_std::iface::{CommandInterface, CommandInterfaceView, CommandStatus, CommandRequest};
//!
//! // These would normally be GlobalMemory fields
//! let mut command: u16 = CommandRequest::Idle.as_u16();
//! let mut command_code: u32 = 0;
//! let mut arg_1: f64 = 0.0;
//! let mut arg_2: f64 = 0.0;
//! let mut status: u16 = 0;
//! let mut result: f64 = 0.0;
//!
//! let mut cmd = CommandInterface::new();
//!
//! // First scan — initializes to IDLE
//! let mut view = CommandInterfaceView {
//!     command: &mut command,
//!     command_code: &mut command_code,
//!     command_arg_1: &mut arg_1,
//!     command_arg_2: &mut arg_2,
//!     command_status: &mut status,
//!     command_result: &mut result,
//! };
//! assert_eq!(cmd.call(&mut view), None);
//! assert_eq!(*view.command_status, CommandStatus::Idle.as_u16());
//!
//! // External source sends a command
//! *view.command_code = 42;
//! *view.command_arg_1 = 3.14;
//! *view.command = CommandRequest::Execute.as_u16();
//!
//! // Next scan — command interface picks it up
//! assert_eq!(cmd.call(&mut view), Some(42));
//! assert_eq!(*view.command_status, CommandStatus::Executing.as_u16());
//!
//! // Subsequent scans — still executing, call() returns None
//! assert_eq!(cmd.call(&mut view), None);
//!
//! // Control program finishes the command
//! cmd.set_done(&mut view, 99.0);
//! assert_eq!(*view.command_status, CommandStatus::Done.as_u16());
//! assert_eq!(*view.command_result, 99.0);
//!
//! // External source acknowledges
//! *view.command = CommandRequest::AckDone.as_u16();
//! assert_eq!(cmd.call(&mut view), None);
//! assert_eq!(*view.command_status, CommandStatus::Idle.as_u16());
//! ```

/// Status of the command interface, written by the control program.
///
/// These values are written to `command_status` in [`CommandInterfaceView`].
#[repr(u16)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CommandStatus {
    /// Initial state before first scan.
    Init = 1,
    /// Ready to accept a new command.
    Idle = 10,
    /// A command is currently being processed.
    Executing = 20,
    /// Command completed successfully. Result is available in `command_result`.
    Done = 100,
    /// Command failed. The control program may place an error code in `command_result`.
    Error = 900,
}

impl CommandStatus {
    /// Convert to the underlying u16 wire value.
    pub fn as_u16(self) -> u16 {
        self as u16
    }

    /// Try to convert a raw u16 from shared memory into a `CommandStatus`.
    /// Returns `None` if the value does not match any known variant.
    pub fn from_u16(val: u16) -> Option<Self> {
        match val {
            1 => Some(Self::Init),
            10 => Some(Self::Idle),
            20 => Some(Self::Executing),
            100 => Some(Self::Done),
            900 => Some(Self::Error),
            _ => None,
        }
    }
}

/// Request codes written by the external source (UI/HMI).
///
/// These values are written to `command` in [`CommandInterfaceView`].
#[repr(u16)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CommandRequest {
    /// No request. The external source should set this after the full handshake completes.
    Idle = 10,
    /// Request execution of the command specified by `command_code`.
    Execute = 11,
    /// Acknowledge that the command result has been read. Allows the interface to
    /// return to [`CommandStatus::Idle`].
    AckDone = 101,
}

impl CommandRequest {
    /// Convert to the underlying u16 wire value.
    pub fn as_u16(self) -> u16 {
        self as u16
    }

    /// Try to convert a raw u16 from shared memory into a `CommandRequest`.
    /// Returns `None` if the value does not match any known variant.
    pub fn from_u16(val: u16) -> Option<Self> {
        match val {
            10 => Some(Self::Idle),
            11 => Some(Self::Execute),
            101 => Some(Self::AckDone),
            _ => None,
        }
    }
}

/// View struct for mapping command interface signals to GlobalMemory fields.
///
/// Create one of these each scan cycle by borrowing the appropriate fields
/// from your `GlobalMemory` struct, then pass it to [`CommandInterface::call`].
///
/// # Example
///
/// ```ignore
/// fn cmd_view(gm: &mut GlobalMemory) -> CommandInterfaceView<'_> {
///     CommandInterfaceView {
///         command:        &mut gm.robot_command,
///         command_code:   &mut gm.robot_command_code,
///         command_arg_1:  &mut gm.robot_arg_1,
///         command_arg_2:  &mut gm.robot_arg_2,
///         command_status: &mut gm.robot_status,
///         command_result: &mut gm.robot_result,
///     }
/// }
/// ```
pub struct CommandInterfaceView<'a> {
    /// Request from external source ([`CommandRequest`] values stored as u16).
    pub command: &'a mut u16,
    /// Identifies the specific command to execute.
    pub command_code: &'a mut u32,
    /// First argument to the command. Meaning is defined per command code.
    pub command_arg_1: &'a mut f64,
    /// Second argument to the command. Meaning is defined per command code.
    pub command_arg_2: &'a mut f64,
    /// Current status of the interface ([`CommandStatus`] values stored as u16).
    pub command_status: &'a mut u16,
    /// Result value from the most recently completed command.
    pub command_result: &'a mut f64,
}

/// Command Interface function block.
///
/// Manages the handshake protocol between an external command source and the
/// control program. Call [`call`](Self::call) once per scan cycle. When a new
/// command arrives, `call` returns `Some(command_code)`. The control program
/// then processes the command and calls [`set_done`](Self::set_done) or
/// [`set_error`](Self::set_error) when finished.
///
/// # Usage Pattern
///
/// ```ignore
/// // In your control program struct:
/// cmd: CommandInterface,
///
/// // In process_tick:
/// let mut cmd_view = my_cmd_view(gm);
/// if let Some(code) = self.cmd.call(&mut cmd_view) {
///     match code {
///         1 => { /* start something */ }
///         2 => { /* do something else */ }
///         _ => { self.cmd.set_error(&mut cmd_view, -1.0); }
///     }
/// }
///
/// // When async work completes later:
/// if self.cmd.is_executing() && work_is_done {
///     self.cmd.set_done(&mut cmd_view, result_value);
/// }
/// ```
#[derive(Debug, Clone)]
pub struct CommandInterface {
    initialized: bool,
    active_command: Option<u32>,
}

impl CommandInterface {
    /// Creates a new command interface. Status will be set to [`CommandStatus::Idle`]
    /// on the first call to [`call`](Self::call).
    pub fn new() -> Self {
        Self {
            initialized: false,
            active_command: None,
        }
    }

    /// Call once per scan cycle.
    ///
    /// Returns `Some(command_code)` on the scan where a new `Execute` request is
    /// detected while the interface is `Idle`. Returns `None` on all other scans.
    ///
    /// This method handles the full handshake lifecycle:
    /// - **Idle + Execute** → transition to `Executing`, return the command code
    /// - **Done + AckDone** → transition back to `Idle`
    /// - **Error + AckDone** → transition back to `Idle`
    pub fn call(&mut self, view: &mut CommandInterfaceView) -> Option<u32> {
        if !self.initialized {
            *view.command_status = CommandStatus::Idle.as_u16();
            self.initialized = true;
            return None;
        }

        let status = *view.command_status;
        let command = *view.command;

        if status == CommandStatus::Idle.as_u16() {
            if command == CommandRequest::Execute.as_u16() {
                self.active_command = Some(*view.command_code);
                *view.command_status = CommandStatus::Executing.as_u16();
                return self.active_command;
            }
        } else if status == CommandStatus::Done.as_u16() || status == CommandStatus::Error.as_u16() {
            if command == CommandRequest::AckDone.as_u16() {
                self.active_command = None;
                *view.command_status = CommandStatus::Idle.as_u16();
                *view.command = CommandRequest::Idle.as_u16();
            }
        }

        None
    }

    /// Returns `true` if a command is currently being executed.
    pub fn is_executing(&self) -> bool {
        self.active_command.is_some()
    }

    /// Returns the command code currently being executed, or `None` if idle.
    pub fn active_command(&self) -> Option<u32> {
        self.active_command
    }

    /// Mark the current command as completed successfully.
    ///
    /// Sets `command_status` to [`CommandStatus::Done`] and writes `result` to
    /// `command_result`. The interface will wait for the external source to send
    /// [`CommandRequest::AckDone`] before returning to idle.
    pub fn set_done(&mut self, view: &mut CommandInterfaceView, result: f64) {
        *view.command_result = result;
        *view.command_status = CommandStatus::Done.as_u16();
    }

    /// Mark the current command as failed.
    ///
    /// Sets `command_status` to [`CommandStatus::Error`]. Optionally write an
    /// error code or description to `command_result` before calling this.
    /// The interface will wait for [`CommandRequest::AckDone`] before returning
    /// to idle.
    pub fn set_error(&mut self, view: &mut CommandInterfaceView, error_result: f64) {
        *view.command_result = error_result;
        *view.command_status = CommandStatus::Error.as_u16();
    }
}

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

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

    fn make_view_fields() -> (u16, u32, f64, f64, u16, f64) {
        (CommandRequest::Idle.as_u16(), 0u32, 0.0f64, 0.0f64, 0u16, 0.0f64)
    }

    macro_rules! view {
        ($cmd:expr, $code:expr, $a1:expr, $a2:expr, $status:expr, $result:expr) => {
            CommandInterfaceView {
                command: &mut $cmd,
                command_code: &mut $code,
                command_arg_1: &mut $a1,
                command_arg_2: &mut $a2,
                command_status: &mut $status,
                command_result: &mut $result,
            }
        };
    }

    #[test]
    fn test_initialization() {
        let mut ci = CommandInterface::new();
        let (mut cmd, mut code, mut a1, mut a2, mut status, mut result) = make_view_fields();
        let mut v = view!(cmd, code, a1, a2, status, result);

        assert_eq!(ci.call(&mut v), None);
        assert_eq!(*v.command_status, CommandStatus::Idle.as_u16());
        assert!(!ci.is_executing());
    }

    #[test]
    fn test_full_handshake() {
        let mut ci = CommandInterface::new();
        let (mut cmd, mut code, mut a1, mut a2, mut status, mut result) = make_view_fields();

        // Init scan
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            ci.call(&mut v);
        }
        assert_eq!(status, CommandStatus::Idle.as_u16());

        // External sends command
        code = 42;
        a1 = 3.14;
        a2 = 2.71;
        cmd = CommandRequest::Execute.as_u16();

        // Scan picks it up
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            assert_eq!(ci.call(&mut v), Some(42));
        }
        assert_eq!(status, CommandStatus::Executing.as_u16());
        assert!(ci.is_executing());
        assert_eq!(ci.active_command(), Some(42));

        // Subsequent scan — still executing
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            assert_eq!(ci.call(&mut v), None);
        }

        // Control program completes
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            ci.set_done(&mut v, 99.0);
        }
        assert_eq!(status, CommandStatus::Done.as_u16());
        assert_eq!(result, 99.0);

        // External acknowledges
        cmd = CommandRequest::AckDone.as_u16();
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            assert_eq!(ci.call(&mut v), None);
        }
        assert_eq!(status, CommandStatus::Idle.as_u16());
        assert_eq!(cmd, CommandRequest::Idle.as_u16());
        assert!(!ci.is_executing());
        assert_eq!(ci.active_command(), None);
    }

    #[test]
    fn test_error_handshake() {
        let mut ci = CommandInterface::new();
        let (mut cmd, mut code, mut a1, mut a2, mut status, mut result) = make_view_fields();

        // Init
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            ci.call(&mut v);
        }

        // Send command
        code = 7;
        cmd = CommandRequest::Execute.as_u16();
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            assert_eq!(ci.call(&mut v), Some(7));
        }

        // Command fails
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            ci.set_error(&mut v, -1.0);
        }
        assert_eq!(status, CommandStatus::Error.as_u16());
        assert_eq!(result, -1.0);

        // External acknowledges the error
        cmd = CommandRequest::AckDone.as_u16();
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            ci.call(&mut v);
        }
        assert_eq!(status, CommandStatus::Idle.as_u16());
        assert!(!ci.is_executing());
    }

    #[test]
    fn test_ignores_exec_while_executing() {
        let mut ci = CommandInterface::new();
        let (mut cmd, mut code, mut a1, mut a2, mut status, mut result) = make_view_fields();

        // Init
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            ci.call(&mut v);
        }

        // First command
        code = 1;
        cmd = CommandRequest::Execute.as_u16();
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            assert_eq!(ci.call(&mut v), Some(1));
        }

        // External tries to send another EXEC while still executing — ignored
        code = 2;
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            assert_eq!(ci.call(&mut v), None);
        }
        assert_eq!(status, CommandStatus::Executing.as_u16());
        assert_eq!(ci.active_command(), Some(1));
    }

    #[test]
    fn test_idle_command_ignored_when_idle() {
        let mut ci = CommandInterface::new();
        let (mut cmd, mut code, mut a1, mut a2, mut status, mut result) = make_view_fields();

        // Init
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            ci.call(&mut v);
        }

        // Command stays IDLE — nothing happens
        {
            let mut v = view!(cmd, code, a1, a2, status, result);
            assert_eq!(ci.call(&mut v), None);
        }
        assert_eq!(status, CommandStatus::Idle.as_u16());
    }
}