autocore-std 3.3.34

//! Non-blocking SDO read/write client for EtherCAT devices.
//!
//! [`SdoClient`] wraps [`CommandClient`](crate::CommandClient) to provide an
//! ergonomic, handle-based interface for runtime SDO (Service Data Object)
//! operations over CoE (CANopen over EtherCAT). Create one per device, issue
//! reads/writes from your control loop, and check results by handle on
//! subsequent ticks.
//!
//! # When to use
//!
//! Use `SdoClient` for **runtime** SDO access — reading diagnostic registers,
//! changing operating parameters on the fly, or any CoE transfer that happens
//! after the cyclic loop is running. For SDOs that must be applied **before**
//! the cyclic loop starts (e.g. setting `modes_of_operation`), use the
//! `startup_sdo` array in `project.json` instead.
//!
//! # Topic format
//!
//! Requests are sent as IPC commands through the existing WebSocket channel.
//! Topics are scoped to the device name configured in `project.json`:
//!
//! | Operation | Topic                              | Payload                                          |
//! |-----------|------------------------------------|--------------------------------------------------|
//! | Write     | `ethercat.write_sdo`               | `{"device": "...", "index": "0x6060", "sub": 0, "value": "0x01"}` |
//! | Read      | `ethercat.read_sdo`                | `{"device": "...", "index": "0x6060", "sub": 0}`                 |
//!
//! # Usage with a state machine
//!
//! A typical pattern pairs `SdoClient` with [`StateMachine`](crate::fb::StateMachine)
//! to fire an SDO write in one state, then advance on success:
//!
//! ```ignore
//! use autocore_std::{ControlProgram, TickContext};
//! use autocore_std::ethercat::{SdoClient, SdoResult};
//! use autocore_std::fb::StateMachine;
//! use serde_json::json;
//! use std::time::Duration;
//!
//! pub struct MyProgram {
//!     sm: StateMachine,
//!     sdo: SdoClient,
//!     write_tid: Option<u32>,
//! }
//!
//! impl MyProgram {
//!     pub fn new() -> Self {
//!         Self {
//!             sm: StateMachine::new(),
//!             sdo: SdoClient::new("ClearPath_0"),
//!             write_tid: None,
//!         }
//!     }
//! }
//!
//! impl ControlProgram for MyProgram {
//!     type Memory = GlobalMemory;
//!
//!     fn process_tick(&mut self, ctx: &mut TickContext<Self::Memory>) {
//!         self.sm.call();
//!         match self.sm.index {
//!             // State 10: Send SDO write for modes_of_operation = PP
//!             10 => {
//!                 self.write_tid = Some(
//!                     self.sdo.write(ctx.client, 0x6060, 0, json!(1))
//!                 );
//!                 self.sm.timeout_preset = Duration::from_secs(3);
//!                 self.sm.index = 20;
//!             }
//!             // State 20: Wait for response
//!             20 => {
//!                 let tid = self.write_tid.unwrap();
//!                 match self.sdo.result(ctx.client, tid, Duration::from_secs(3)) {
//!                     SdoResult::Pending => { /* keep waiting */ }
//!                     SdoResult::Ok(_) => {
//!                         log::info!("modes_of_operation set to PP");
//!                         self.sm.index = 30;
//!                     }
//!                     SdoResult::Err(e) => {
//!                         log::error!("SDO write failed: {}", e);
//!                         self.sm.set_error(1);
//!                     }
//!                     SdoResult::Timeout => {
//!                         log::error!("SDO write timed out");
//!                         self.sm.set_error(2);
//!                     }
//!                 }
//!             }
//!             // State 30: Done — continue with normal operation
//!             30 => { /* ... */ }
//!             _ => {}
//!         }
//!     }
//! }
//! ```
//!
//! # Reading an SDO
//!
//! ```ignore
//! // Fire a read request
//! let tid = sdo.read(ctx.client, 0x6064, 0); // Position Actual Value
//!
//! // On a later tick, check the result
//! match sdo.result(ctx.client, tid, Duration::from_secs(3)) {
//!     SdoResult::Ok(data) => {
//!         let position: i32 = serde_json::from_value(data).unwrap();
//!         log::info!("Current position: {}", position);
//!     }
//!     SdoResult::Pending => { /* still waiting */ }
//!     SdoResult::Err(e) => { log::error!("SDO read failed: {}", e); }
//!     SdoResult::Timeout => { log::error!("SDO read timed out"); }
//! }
//! ```

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

use serde_json::{json, Value};

use crate::command_client::CommandClient;

/// Metadata for an in-flight SDO request.
pub struct SdoRequest {
    /// CoE object dictionary index (e.g. 0x6060).
    pub index: u16,
    /// CoE sub-index.
    pub sub_index: u8,
    /// Whether this is a read or write.
    pub kind: SdoRequestKind,
    /// When the request was sent (for timeout detection).
    pub sent_at: Instant,
}

/// Discriminates SDO reads from writes.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum SdoRequestKind {
    /// SDO read (CoE upload).
    Read,
    /// SDO write (CoE download).
    Write,
}

/// Result of checking an in-flight SDO request.
///
/// Returned by [`SdoClient::result()`]. The caller should match on this each
/// tick until the request resolves (i.e. is no longer [`Pending`](SdoResult::Pending)).
///
/// ```ignore
/// match sdo.result(ctx.client, tid, Duration::from_secs(3)) {
///     SdoResult::Pending => { /* check again next tick */ }
///     SdoResult::Ok(data) => { /* use data */ }
///     SdoResult::Err(msg) => { log::error!("{}", msg); }
///     SdoResult::Timeout => { log::error!("timed out"); }
/// }
/// ```
#[derive(Debug, Clone)]
pub enum SdoResult {
    /// No response yet; check again next tick.
    Pending,
    /// Operation succeeded. Contains the response `data` field — the read
    /// value for reads, or an empty/null value for writes.
    Ok(Value),
    /// The server (or EtherCAT master) reported an error. The string contains
    /// the `error_message` from the response (e.g. `"SDO abort: 0x06090011"`).
    Err(String),
    /// No response arrived within the caller-specified deadline.
    Timeout,
}

/// Non-blocking SDO client scoped to a single EtherCAT device.
///
/// Create one `SdoClient` per device in your control program struct. It holds
/// a map of outstanding requests keyed by `transaction_id` (returned by
/// [`CommandClient::send`]). Keep the returned handle and poll
/// [`result()`](Self::result) each tick until it resolves.
///
/// # Example
///
/// ```ignore
/// use autocore_std::ethercat::{SdoClient, SdoResult};
/// use serde_json::json;
/// use std::time::Duration;
///
/// let mut sdo = SdoClient::new("ClearPath_0");
///
/// // Issue an SDO write (from process_tick):
/// let tid = sdo.write(ctx.client, 0x6060, 0, json!(1));
///
/// // Check result on subsequent ticks:
/// match sdo.result(ctx.client, tid, Duration::from_secs(3)) {
///     SdoResult::Pending => {}
///     SdoResult::Ok(_) => { /* success */ }
///     SdoResult::Err(e) => { log::error!("SDO error: {}", e); }
///     SdoResult::Timeout => { log::error!("SDO timed out"); }
/// }
/// ```
pub struct SdoClient {
    device: String,
    requests: HashMap<u32, SdoRequest>,
}

impl SdoClient {
    /// Create a new client for the given device name.
    ///
    /// The `device` string must match the `name` field in the slave's
    /// `project.json` configuration (e.g. `"ClearPath_0"`).
    ///
    /// ```ignore
    /// let sdo = SdoClient::new("ClearPath_0");
    /// ```
    pub fn new(device: &str) -> Self {
        Self {
            device: device.to_string(),
            requests: HashMap::new(),
        }
    }

    /// Issue an SDO write (CoE download).
    ///
    /// Sends a command to topic `ethercat.{device}.sdo_write` with payload:
    /// ```json
    /// {"index": "0x6060", "sub": 0, "value": 1}
    /// ```
    ///
    /// Returns a transaction handle for use with [`result()`](Self::result).
    ///
    /// # Arguments
    ///
    /// * `client` — the [`CommandClient`] from [`TickContext`](crate::TickContext)
    /// * `index` — CoE object dictionary index (e.g. `0x6060`)
    /// * `sub_index` — CoE sub-index (usually `0`)
    /// * `value` — the value to write, as a [`serde_json::Value`]
    ///
    /// # Example
    ///
    /// ```ignore
    /// // Set modes_of_operation to Profile Position (1)
    /// let tid = sdo.write(ctx.client, 0x6060, 0, json!(1));
    /// ```
    pub fn write(
        &mut self,
        client: &mut CommandClient,
        index: u16,
        sub_index: u8,
        value: Value,
    ) -> u32 {
        let topic = "ethercat.write_sdo".to_string();
        let payload = json!({
            "device": self.device,
            "index": format!("0x{:04X}", index),
            "sub": sub_index,
            "value": value,
        });
        let tid = client.send(&topic, payload);

        self.requests.insert(tid, SdoRequest {
            index,
            sub_index,
            kind: SdoRequestKind::Write,
            sent_at: Instant::now(),
        });

        tid
    }

    /// Issue an SDO read (CoE upload).
    ///
    /// Sends a command to topic `ethercat.{device}.sdo_read` with payload:
    /// ```json
    /// {"index": "0x6064", "sub": 0}
    /// ```
    ///
    /// Returns a transaction handle for use with [`result()`](Self::result).
    ///
    /// # Arguments
    ///
    /// * `client` — the [`CommandClient`] from [`TickContext`](crate::TickContext)
    /// * `index` — CoE object dictionary index (e.g. `0x6064`)
    /// * `sub_index` — CoE sub-index (usually `0`)
    ///
    /// # Example
    ///
    /// ```ignore
    /// // Read the actual position value
    /// let tid = sdo.read(ctx.client, 0x6064, 0);
    /// ```
    pub fn read(
        &mut self,
        client: &mut CommandClient,
        index: u16,
        sub_index: u8,
    ) -> u32 {
        let topic = "ethercat.read_sdo".to_string();
        let payload = json!({
            "device": self.device,
            "index": format!("0x{:04X}", index),
            "sub": sub_index,
        });
        let tid = client.send(&topic, payload);

        self.requests.insert(tid, SdoRequest {
            index,
            sub_index,
            kind: SdoRequestKind::Read,
            sent_at: Instant::now(),
        });

        tid
    }

    /// Check the result of a previous SDO request.
    ///
    /// Call this each tick with the handle returned by [`write()`](Self::write)
    /// or [`read()`](Self::read). The result is consumed (removed from the
    /// internal map) once it resolves to [`Ok`](SdoResult::Ok),
    /// [`Err`](SdoResult::Err), or [`Timeout`](SdoResult::Timeout).
    ///
    /// # Arguments
    ///
    /// * `client` — the [`CommandClient`] from [`TickContext`](crate::TickContext)
    /// * `tid` — transaction handle returned by `write()` or `read()`
    /// * `timeout` — maximum time to wait before returning [`SdoResult::Timeout`]
    ///
    /// # Example
    ///
    /// ```ignore
    /// match sdo.result(ctx.client, tid, Duration::from_secs(3)) {
    ///     SdoResult::Pending => { /* keep waiting */ }
    ///     SdoResult::Ok(data) => {
    ///         log::info!("SDO response: {}", data);
    ///         sm.index = next_state;
    ///     }
    ///     SdoResult::Err(e) => {
    ///         log::error!("SDO failed: {}", e);
    ///         sm.set_error(1);
    ///     }
    ///     SdoResult::Timeout => {
    ///         log::error!("SDO timed out");
    ///         sm.set_error(2);
    ///     }
    /// }
    /// ```
    pub fn result(
        &mut self,
        client: &mut CommandClient,
        tid: u32,
        timeout: Duration,
    ) -> SdoResult {
        let req = match self.requests.get(&tid) {
            Some(r) => r,
            None => return SdoResult::Err("unknown transaction id".into()),
        };

        // Check for response from CommandClient
        if let Some(resp) = client.take_response(tid) {
            self.requests.remove(&tid);
            if resp.success {
                return SdoResult::Ok(resp.data);
            } else {
                return SdoResult::Err(resp.error_message);
            }
        }

        // Check timeout
        if req.sent_at.elapsed() > timeout {
            self.requests.remove(&tid);
            return SdoResult::Timeout;
        }

        SdoResult::Pending
    }

    /// Remove all requests that have been pending longer than `timeout`.
    ///
    /// Call periodically (e.g. once per second) to prevent the internal map
    /// from growing unboundedly if callers forget to check results.
    ///
    /// # Example
    ///
    /// ```ignore
    /// // At the end of process_tick, clean up anything older than 10s
    /// self.sdo.drain_stale(ctx.client, Duration::from_secs(10));
    /// ```
    pub fn drain_stale(&mut self, client: &mut CommandClient, timeout: Duration) {
        let stale_tids: Vec<u32> = self
            .requests
            .iter()
            .filter(|(_, req)| req.sent_at.elapsed() > timeout)
            .map(|(&tid, _)| tid)
            .collect();

        for tid in stale_tids {
            self.requests.remove(&tid);
            // Also consume the response from CommandClient if one arrived late
            let _ = client.take_response(tid);
        }
    }

    /// Number of in-flight SDO requests.
    pub fn pending_count(&self) -> usize {
        self.requests.len()
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use mechutil::ipc::CommandMessage;
    use tokio::sync::mpsc;

    /// Helper: create a CommandClient backed by test channels, returning
    /// (client, response_sender, write_receiver).
    fn test_client() -> (
        CommandClient,
        mpsc::UnboundedSender<CommandMessage>,
        mpsc::UnboundedReceiver<String>,
    ) {
        let (write_tx, write_rx) = mpsc::unbounded_channel();
        let (response_tx, response_rx) = mpsc::unbounded_channel();
        let client = CommandClient::new(write_tx, response_rx);
        (client, response_tx, write_rx)
    }

    #[test]
    fn write_sends_correct_topic_and_payload() {
        let (mut client, _resp_tx, mut write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        let tid = sdo.write(&mut client, 0x6060, 0, json!(1));

        let msg_json = write_rx.try_recv().expect("should have sent a message");
        let msg: CommandMessage = serde_json::from_str(&msg_json).unwrap();

        assert_eq!(msg.transaction_id, tid);
        assert_eq!(msg.topic, "ethercat.write_sdo");
        assert_eq!(msg.data["device"], "ClearPath_0");
        assert_eq!(msg.data["index"], "0x6060");
        assert_eq!(msg.data["sub"], 0);
        assert_eq!(msg.data["value"], 1);
        assert_eq!(sdo.pending_count(), 1);
    }

    #[test]
    fn read_sends_correct_topic_and_payload() {
        let (mut client, _resp_tx, mut write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        let tid = sdo.read(&mut client, 0x6064, 0);

        let msg_json = write_rx.try_recv().expect("should have sent a message");
        let msg: CommandMessage = serde_json::from_str(&msg_json).unwrap();

        assert_eq!(msg.transaction_id, tid);
        assert_eq!(msg.topic, "ethercat.read_sdo");
        assert_eq!(msg.data["device"], "ClearPath_0");
        assert_eq!(msg.data["index"], "0x6064");
        assert_eq!(msg.data["sub"], 0);
        assert!(msg.data.get("value").is_none());
    }

    #[test]
    fn result_returns_ok_on_success() {
        let (mut client, resp_tx, _write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        let tid = sdo.write(&mut client, 0x6060, 0, json!(1));

        // Simulate successful response
        resp_tx
            .send(CommandMessage::response(tid, json!(null)))
            .unwrap();
        client.poll();

        match sdo.result(&mut client, tid, Duration::from_secs(3)) {
            SdoResult::Ok(data) => assert_eq!(data, json!(null)),
            other => panic!("expected Ok, got {:?}", other),
        }

        // Consumed — no longer tracked
        assert_eq!(sdo.pending_count(), 0);
    }

    #[test]
    fn result_returns_err_on_failure() {
        let (mut client, resp_tx, _write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        let tid = sdo.write(&mut client, 0x6060, 0, json!(1));

        // Simulate error response
        let mut err_resp = CommandMessage::response(tid, json!(null));
        err_resp.success = false;
        err_resp.error_message = "SDO abort: 0x06090011".into();
        resp_tx.send(err_resp).unwrap();
        client.poll();

        match sdo.result(&mut client, tid, Duration::from_secs(3)) {
            SdoResult::Err(msg) => assert_eq!(msg, "SDO abort: 0x06090011"),
            other => panic!("expected Err, got {:?}", other),
        }
    }

    #[test]
    fn result_returns_pending_while_waiting() {
        let (mut client, _resp_tx, _write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        let tid = sdo.write(&mut client, 0x6060, 0, json!(1));
        client.poll();

        match sdo.result(&mut client, tid, Duration::from_secs(30)) {
            SdoResult::Pending => {}
            other => panic!("expected Pending, got {:?}", other),
        }

        // Still tracked
        assert_eq!(sdo.pending_count(), 1);
    }

    #[test]
    fn result_returns_timeout_when_deadline_exceeded() {
        let (mut client, _resp_tx, _write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        let tid = sdo.write(&mut client, 0x6060, 0, json!(1));
        client.poll();

        // Zero timeout -> immediately expired
        match sdo.result(&mut client, tid, Duration::ZERO) {
            SdoResult::Timeout => {}
            other => panic!("expected Timeout, got {:?}", other),
        }

        assert_eq!(sdo.pending_count(), 0);
    }

    #[test]
    fn drain_stale_removes_old_requests() {
        let (mut client, _resp_tx, _write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        sdo.write(&mut client, 0x6060, 0, json!(1));
        sdo.read(&mut client, 0x6064, 0);
        assert_eq!(sdo.pending_count(), 2);

        // Zero timeout -> everything is stale
        sdo.drain_stale(&mut client, Duration::ZERO);
        assert_eq!(sdo.pending_count(), 0);
    }

    #[test]
    fn multiple_concurrent_requests() {
        let (mut client, resp_tx, _write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        let tid1 = sdo.write(&mut client, 0x6060, 0, json!(1));
        let tid2 = sdo.read(&mut client, 0x6064, 0);
        assert_eq!(sdo.pending_count(), 2);

        // Only respond to the read
        resp_tx
            .send(CommandMessage::response(tid2, json!(12345)))
            .unwrap();
        client.poll();

        // Read resolves, write still pending
        match sdo.result(&mut client, tid2, Duration::from_secs(3)) {
            SdoResult::Ok(v) => assert_eq!(v, json!(12345)),
            other => panic!("expected Ok, got {:?}", other),
        }
        match sdo.result(&mut client, tid1, Duration::from_secs(30)) {
            SdoResult::Pending => {}
            other => panic!("expected Pending, got {:?}", other),
        }
        assert_eq!(sdo.pending_count(), 1);
    }

    #[test]
    fn unknown_tid_returns_err() {
        let (mut client, _resp_tx, _write_rx) = test_client();
        let mut sdo = SdoClient::new("ClearPath_0");

        match sdo.result(&mut client, 99999, Duration::from_secs(3)) {
            SdoResult::Err(msg) => assert!(msg.contains("unknown")),
            other => panic!("expected Err for unknown tid, got {:?}", other),
        }
    }
}