autocore-std 3.3.35

//! CiA 402 (CANopen drive profile) base types and traits.
//!
//! This module provides the standard CiA 402 state machine definitions,
//! mode-of-operation enum, and trait-based abstractions for control word
//! and status word bit manipulation.
//!
//! Layers:
//! 1. **Raw newtypes** — [`RawControlWord`], [`RawStatusWord`]
//! 2. **Base traits** — [`Cia402Control`], [`Cia402Status`], [`Cia402State`]
//! 3. **Mode-specific traits** — [`PpControl`]/[`PpStatus`],
//!    [`PvControl`]/[`PvStatus`], [`HomingControl`]/[`HomingStatus`]
//!
//! Vendor-specific extensions (e.g. Teknic, Yaskawa) build on top of these
//! traits in their own crates.

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

use serde_json::json;

use crate::fb::StateMachine;
use super::axis_view::AxisView;
use crate::command_client::CommandClient;
use crate::ethercat::{SdoClient, SdoResult};


/// Raw u16 newtype — this IS the PDO data, no conversion needed
#[derive(Clone, Copy, Debug, Default)]
pub struct RawControlWord(pub u16);

/// Raw u16 newtype for the status word.
#[derive(Clone, Copy, Debug, Default)]
pub struct RawStatusWord(pub u16);

// ──────────────────────────────────────────────
// Layer 1: CiA 402 State Machine (all modes)
// ──────────────────────────────────────────────

/// State machine commands common to every CiA 402 mode.
///
/// Bits 0–3 and 7 are the state-machine control bits.
/// Bits 4–6, 8 are mode-specific (see `PpControl`, `PvControl`, etc.).
/// Bits 9–15 are manufacturer-specific.
pub trait Cia402Control {
    /// Read the raw control word.
    fn raw(&self) -> u16;
    /// Get a mutable reference to the raw control word.
    fn raw_mut(&mut self) -> &mut u16;

    // ── Individual bit setters ──

    /// Set bit 0 — Switch On.
    fn set_switch_on(&mut self, v: bool) {
        self.set_bit(0, v);
    }
    /// Set bit 1 — Enable Voltage.
    fn set_enable_voltage(&mut self, v: bool) {
        self.set_bit(1, v);
    }
    /// Set bit 2 — Quick Stop.
    fn set_quick_stop(&mut self, v: bool) {
        self.set_bit(2, v);
    }
    /// Set bit 3 — Enable Operation.
    fn set_enable_operation(&mut self, v: bool) {
        self.set_bit(3, v);
    }
    /// Set bit 7 — Fault Reset.
    fn set_fault_reset(&mut self, v: bool) {
        self.set_bit(7, v);
    }

    // ── State-machine transition commands ──
    //
    // Each command sets bits 0–3,7 to the pattern required by CiA 402
    // while preserving mode-specific and vendor bits (4–6, 8–15).
    //
    // The mask 0x008F covers bits 0,1,2,3,7.

    /// Shutdown command (transitions 2, 6, 8).
    /// Target state: Ready to Switch On.
    fn cmd_shutdown(&mut self) {
        let w = self.raw_mut();
        *w = (*w & !0x008F) | 0x0006; // bits 1,2 set; 0,3,7 clear
    }

    /// Switch On command (transition 3).
    /// Target state: Switched On.
    fn cmd_switch_on(&mut self) {
        let w = self.raw_mut();
        *w = (*w & !0x008F) | 0x0007; // bits 0,1,2 set; 3,7 clear
    }

    /// Enable Operation command (transition 4, or combined 2+3+4).
    /// Target state: Operation Enabled.
    fn cmd_enable_operation(&mut self) {
        let w = self.raw_mut();
        *w = (*w & !0x008F) | 0x000F; // bits 0-3 set; 7 clear
    }

    /// Disable Operation command (transition 5).
    /// Target state: Switched On.
    fn cmd_disable_operation(&mut self) {
        let w = self.raw_mut();
        *w = (*w & !0x008F) | 0x0007; // bits 0,1,2 set; 3,7 clear
    }

    /// Disable Voltage command (transitions 7, 9, 10, 12).
    /// Target state: Switch On Disabled.
    fn cmd_disable_voltage(&mut self) {
        let w = self.raw_mut();
        *w &= !0x0082; // clear bits 1 and 7
    }

    /// Quick Stop command (transition 11).
    /// Target state: Quick Stop Active.
    fn cmd_quick_stop(&mut self) {
        let w = self.raw_mut();
        *w = (*w & !0x0086) | 0x0002; // bit 1 set; bits 2,7 clear
    }

    /// Fault Reset command (transition 15, rising edge on bit 7).
    /// Drive must be in Fault state. Transitions to Switch On Disabled.
    fn cmd_fault_reset(&mut self) {
        let w = self.raw_mut();
        *w |= 0x0080; // set bit 7
    }


    /// Clear the Fault Reset command (transition 15, rising edge on bit 7).
    fn cmd_clear_fault_reset(&mut self) {
        self.set_bit(7, false);
    }

    /// Set or clear a single bit in the control word.
    fn set_bit(&mut self, bit: u8, v: bool) {
        let w = self.raw_mut();
        if v {
            *w |= 1 << bit;
        } else {
            *w &= !(1 << bit);
        }
    }
}

/// CiA 402 status word decoding, common to every mode.
///
/// Bits 0–6: state machine state.
/// Bit 7:    warning.
/// Bit 9:    remote (drive is controlled via fieldbus).
/// Bit 10:   target reached (mode-dependent meaning).
/// Bits 8, 11–15: mode-specific or manufacturer-specific.
pub trait Cia402Status {
    /// Read the raw status word.
    fn raw(&self) -> u16;

    /// Bit 0 — Ready to Switch On.
    fn ready_to_switch_on(&self) -> bool {
        self.raw() & (1 << 0) != 0
    }
    /// Bit 1 — Switched On.
    fn switched_on(&self) -> bool {
        self.raw() & (1 << 1) != 0
    }
    /// Bit 2 — Operation Enabled.
    fn operation_enabled(&self) -> bool {
        self.raw() & (1 << 2) != 0
    }
    /// Bit 3 — Fault.
    fn fault(&self) -> bool {
        self.raw() & (1 << 3) != 0
    }
    /// Bit 4 — Voltage Enabled.
    fn voltage_enabled(&self) -> bool {
        self.raw() & (1 << 4) != 0
    }
    /// Bit 5 — Quick Stop Active.
    fn quick_stop_active(&self) -> bool {
        self.raw() & (1 << 5) != 0
    }
    /// Bit 6 — Switch On Disabled.
    fn switch_on_disabled(&self) -> bool {
        self.raw() & (1 << 6) != 0
    }
    /// Bit 7 — Warning.
    fn warning(&self) -> bool {
        self.raw() & (1 << 7) != 0
    }
    /// Bit 9 — Remote.
    fn remote(&self) -> bool {
        self.raw() & (1 << 9) != 0
    }
    /// Bit 10 — Target Reached.
    fn target_reached(&self) -> bool {
        self.raw() & (1 << 10) != 0
    }

    /// Decode the CiA 402 state machine state from status word bits.
    ///
    /// The state is encoded in bits 0–3, 5, 6. Some states have bit 5
    /// as "don't care", so we use different masks:
    ///  - 0x006F (bits 0,1,2,3,5,6) for states where bit 5 is defined
    ///  - 0x004F (bits 0,1,2,3,6)   for states where bit 5 is "x"
    fn state(&self) -> Cia402State {
        let w = self.raw();
        // Check most-specific patterns first (bit 5 defined → mask 0x006F)
        if w & 0x006F == 0x0027 { return Cia402State::OperationEnabled; }
        if w & 0x006F == 0x0023 { return Cia402State::SwitchedOn; }
        if w & 0x006F == 0x0021 { return Cia402State::ReadyToSwitchOn; }
        if w & 0x006F == 0x0007 { return Cia402State::QuickStopActive; }
        // Less-specific patterns (bit 5 is don't-care → mask 0x004F)
        if w & 0x004F == 0x000F { return Cia402State::FaultReactionActive; }
        if w & 0x004F == 0x0008 { return Cia402State::Fault; }
        if w & 0x004F == 0x0040 { return Cia402State::SwitchOnDisabled; }
        if w & 0x004F == 0x0000 { return Cia402State::NotReadyToSwitchOn; }
        Cia402State::Unknown
    }
}

/// CiA 402 drive state machine states.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Cia402State {
    /// Not Ready to Switch On — drive is initializing.
    NotReadyToSwitchOn,
    /// Switch On Disabled — drive power stage is disabled.
    SwitchOnDisabled,
    /// Ready to Switch On — waiting for Switch On command.
    ReadyToSwitchOn,
    /// Switched On — power stage is energized but not enabled.
    SwitchedOn,
    /// Operation Enabled — drive is active and accepting motion commands.
    OperationEnabled,
    /// Quick Stop Active — drive is decelerating to stop.
    QuickStopActive,
    /// Fault Reaction Active — drive is executing fault reaction.
    FaultReactionActive,
    /// Fault — drive has faulted and requires a fault reset.
    Fault,
    /// Unknown — status word pattern did not match any defined state.
    Unknown,
}

impl std::fmt::Display for Cia402State {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            Self::NotReadyToSwitchOn => write!(f, "Not Ready to Switch On"),
            Self::SwitchOnDisabled   => write!(f, "Switch On Disabled"),
            Self::ReadyToSwitchOn    => write!(f, "Ready to Switch On"),
            Self::SwitchedOn         => write!(f, "Switched On"),
            Self::OperationEnabled   => write!(f, "Operation Enabled"),
            Self::QuickStopActive    => write!(f, "Quick Stop Active"),
            Self::FaultReactionActive => write!(f, "Fault Reaction Active"),
            Self::Fault              => write!(f, "Fault"),
            Self::Unknown            => write!(f, "Unknown"),
        }
    }
}

// ── Modes of Operation (0x6060 / 0x6061) ──

/// CiA 402 Modes of Operation.
///
/// Written to RxPDO object 0x6060; read back from TxPDO object 0x6061.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[repr(i8)]
pub enum ModesOfOperation {
    /// Profile Position mode (PP).
    ProfilePosition          =  1,
    /// Profile Velocity mode (PV).
    ProfileVelocity          =  3,
    /// Homing mode.
    Homing                   =  6,
    /// Interpolated Position mode (IP).
    InterpolatedPosition     =  7,
    /// Cyclic Synchronous Position mode (CSP).
    CyclicSynchronousPosition = 8,
    /// Cyclic Synchronous Velocity mode (CSV).
    CyclicSynchronousVelocity = 9,
    /// Cyclic Synchronous Torque mode (CST).
    CyclicSynchronousTorque  = 10,
}

impl ModesOfOperation {
    /// Convert an i8 to a `ModesOfOperation` variant, if valid.
    pub fn from_i8(v: i8) -> Option<Self> {
        match v {
            1  => Some(Self::ProfilePosition),
            3  => Some(Self::ProfileVelocity),
            6  => Some(Self::Homing),
            7  => Some(Self::InterpolatedPosition),
            8  => Some(Self::CyclicSynchronousPosition),
            9  => Some(Self::CyclicSynchronousVelocity),
            10 => Some(Self::CyclicSynchronousTorque),
            _  => None,
        }
    }

    /// Convert to the underlying i8 value.
    pub fn as_i8(self) -> i8 {
        self as i8
    }
}

impl std::fmt::Display for ModesOfOperation {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            Self::ProfilePosition           => write!(f, "Profile Position (PP)"),
            Self::ProfileVelocity           => write!(f, "Profile Velocity (PV)"),
            Self::Homing                    => write!(f, "Homing"),
            Self::InterpolatedPosition      => write!(f, "Interpolated Position (IP)"),
            Self::CyclicSynchronousPosition => write!(f, "Cyclic Synchronous Position (CSP)"),
            Self::CyclicSynchronousVelocity => write!(f, "Cyclic Synchronous Velocity (CSV)"),
            Self::CyclicSynchronousTorque   => write!(f, "Cyclic Synchronous Torque (CST)"),
        }
    }
}

// Implement base traits for the raw newtypes
impl Cia402Control for RawControlWord {
    fn raw(&self) -> u16 {
        self.0
    }
    fn raw_mut(&mut self) -> &mut u16 {
        &mut self.0
    }
}
impl Cia402Status for RawStatusWord {
    fn raw(&self) -> u16 {
        self.0
    }
}

// ──────────────────────────────────────────────
// Layer 2: Profile Position mode bits (PP)
// ──────────────────────────────────────────────

/// Profile Position (PP) mode control word bits.
pub trait PpControl: Cia402Control {
    /// Bit 4 — New Set-Point: rising edge starts a new positioning move.
    fn set_new_set_point(&mut self, v: bool) {
        self.set_bit(4, v);
    }
    /// Bit 5 — Change Set Immediately: if true, interrupt current move.
    fn set_change_set_immediately(&mut self, v: bool) {
        self.set_bit(5, v);
    }
    /// Bit 6 — Relative: target position is relative to current.
    fn set_relative(&mut self, v: bool) {
        self.set_bit(6, v);
    }
    /// Bit 8 — Halt: decelerate to stop.
    fn set_halt(&mut self, v: bool) {
        self.set_bit(8, v);
    }
}

/// Profile Position (PP) mode status word bits.
pub trait PpStatus: Cia402Status {
    /// Bit 10 — Target Reached: positioning move completed.
    fn pp_target_reached(&self) -> bool {
        self.raw() & (1 << 10) != 0
    }
    /// Bit 11 — Internal Limit Active.
    fn internal_limit(&self) -> bool {
        self.raw() & (1 << 11) != 0
    }
    /// Bit 12 — Set-Point Acknowledge: drive accepted the new set-point.
    fn set_point_acknowledge(&self) -> bool {
        self.raw() & (1 << 12) != 0
    }
    /// Bit 13 — Following Error: position tracking error exceeded limit.
    fn following_error(&self) -> bool {
        self.raw() & (1 << 13) != 0
    }
}

// ──────────────────────────────────────────────
// Layer 2: Profile Velocity mode bits (PV)
// ──────────────────────────────────────────────

/// Profile Velocity (PV) mode control word bits.
pub trait PvControl: Cia402Control {
    /// Bit 8 — Halt: decelerate to zero velocity.
    fn set_halt(&mut self, v: bool) {
        self.set_bit(8, v);
    }
}

/// Profile Velocity (PV) mode status word bits.
pub trait PvStatus: Cia402Status {
    /// Bit 10 — Target Reached: actual velocity equals target velocity.
    fn pv_target_reached(&self) -> bool {
        self.raw() & (1 << 10) != 0
    }
    /// Bit 11 — Internal Limit Active.
    fn pv_internal_limit(&self) -> bool {
        self.raw() & (1 << 11) != 0
    }
    /// Bit 12 — Speed: 0 = velocity != 0, 1 = velocity = 0.
    fn speed_is_zero(&self) -> bool {
        self.raw() & (1 << 12) != 0
    }
    /// Bit 13 — Max Slippage Error (AC motors; not used by ClearPath-EC).
    fn max_slippage_error(&self) -> bool {
        self.raw() & (1 << 13) != 0
    }
}

// ──────────────────────────────────────────────
// Layer 2: Homing mode bits
// ──────────────────────────────────────────────

/// Homing mode control word bits.
pub trait HomingControl: Cia402Control {
    /// Bit 4 — Homing Operation Start: rising edge starts homing.
    fn set_homing_start(&mut self, v: bool) {
        self.set_bit(4, v);
    }
    /// Bit 8 — Halt: interrupt homing and decelerate.
    fn set_halt(&mut self, v: bool) {
        self.set_bit(8, v);
    }
}

/// Homing mode status word bits.
pub trait HomingStatus: Cia402Status {
    /// Bit 10 — Target Reached: homing target position reached.
    fn homing_target_reached(&self) -> bool {
        self.raw() & (1 << 10) != 0
    }
    /// Bit 12 — Homing Attained: homing procedure completed successfully.
    fn homing_attained(&self) -> bool {
        self.raw() & (1 << 12) != 0
    }
    /// Bit 13 — Homing Error: homing procedure failed.
    fn homing_error(&self) -> bool {
        self.raw() & (1 << 13) != 0
    }
}

// Implement PP traits for RawControlWord / RawStatusWord
impl PpControl for RawControlWord {}
impl PpStatus for RawStatusWord {}
impl PvControl for RawControlWord {}
impl PvStatus for RawStatusWord {}
impl HomingControl for RawControlWord {}
impl HomingStatus for RawStatusWord {}

// ──────────────────────────────────────────────
// Tests
// ──────────────────────────────────────────────

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

    #[test]
    fn test_state_decoding() {
        // Standard patterns
        assert_eq!(RawStatusWord(0x0000).state(), Cia402State::NotReadyToSwitchOn);
        assert_eq!(RawStatusWord(0x0040).state(), Cia402State::SwitchOnDisabled);
        assert_eq!(RawStatusWord(0x0021).state(), Cia402State::ReadyToSwitchOn);
        assert_eq!(RawStatusWord(0x0023).state(), Cia402State::SwitchedOn);
        assert_eq!(RawStatusWord(0x0027).state(), Cia402State::OperationEnabled);
        assert_eq!(RawStatusWord(0x0007).state(), Cia402State::QuickStopActive);
        assert_eq!(RawStatusWord(0x000F).state(), Cia402State::FaultReactionActive);
        assert_eq!(RawStatusWord(0x0008).state(), Cia402State::Fault);
    }

    #[test]
    fn test_state_decoding_bit5_dont_care() {
        // States where bit 5 is "don't care" must decode correctly
        // regardless of whether bit 5 is 0 or 1.

        // Not Ready to Switch On: bit 5 = 1 → still NRTSO
        assert_eq!(RawStatusWord(0x0020).state(), Cia402State::NotReadyToSwitchOn);
        // Switch On Disabled: bit 5 = 1
        assert_eq!(RawStatusWord(0x0060).state(), Cia402State::SwitchOnDisabled);
        // Fault Reaction Active: bit 5 = 1
        assert_eq!(RawStatusWord(0x002F).state(), Cia402State::FaultReactionActive);
        // Fault: bit 5 = 1
        assert_eq!(RawStatusWord(0x0028).state(), Cia402State::Fault);
    }

    #[test]
    fn test_state_decoding_ignores_high_bits() {
        // Bits 8+ should not affect state decoding
        assert_eq!(RawStatusWord(0xFF27).state(), Cia402State::OperationEnabled);
        assert_eq!(RawStatusWord(0x8040).state(), Cia402State::SwitchOnDisabled);
    }

    #[test]
    fn test_cmd_shutdown() {
        let mut cw = RawControlWord(0xFF00);
        cw.cmd_shutdown();
        // Bits 1,2 set; bits 0,3,7 clear; bits 4-6,8-15 preserved
        assert_eq!(cw.0, 0xFF06);
    }

    #[test]
    fn test_cmd_enable_operation() {
        let mut cw = RawControlWord(0x0000);
        cw.cmd_enable_operation();
        assert_eq!(cw.0, 0x000F);
    }

    #[test]
    fn test_cmd_fault_reset() {
        let mut cw = RawControlWord(0x0000);
        cw.cmd_fault_reset();
        assert!(cw.0 & 0x0080 != 0); // bit 7 set
    }

    #[test]
    fn test_modes_of_operation_roundtrip() {
        for mode in [
            ModesOfOperation::ProfilePosition,
            ModesOfOperation::ProfileVelocity,
            ModesOfOperation::Homing,
        ] {
            assert_eq!(ModesOfOperation::from_i8(mode.as_i8()), Some(mode));
        }
        assert_eq!(ModesOfOperation::from_i8(99), None);
    }
}



#[repr(i32)]
#[derive(Copy, Clone, Debug, FromRepr)]
enum ModeOfOperationStates {
    Reset = 0,
    WriteModeOp,
    WaitWriteModeOp,
    ReadModeOp,
    WaitReadModeOp
}

/// Used to set the mode of operation 0x6060 of a Ci402 drive using
/// SDO writes. The mode is confirmed using 0x6061 mode of operation
/// display.
#[derive(Clone, Debug)]
pub struct FbSetModeOfOperation {
    /// state machine fb
    state : StateMachine,
    /// target mode to write to the drive
    target_mode : i8,
    tid : u32,
    retry_count : u16
}

impl FbSetModeOfOperation {
    /// Constructor
    pub fn new() -> Self {
        Self {
            state : StateMachine::new(),
            target_mode : 0,
            tid : 0,
            retry_count : 0
        }
    }

    pub fn reset(&mut self) {
        self.state.error_code = 0;
        self.state.error_message.clear();
        self.state.index = ModeOfOperationStates::Reset as i32;
    }

    pub fn start(&mut self, target_mode : i8) {
        self.target_mode = target_mode;
        self.retry_count = 0;
        self.state.error_code = 0;
        self.state.error_message.clear();
        self.state.index = ModeOfOperationStates::WriteModeOp as i32;
    }

    pub fn is_busy(&self) -> bool {
        return self.state.index > ModeOfOperationStates::Reset as i32;
    }

    pub fn is_error(&self) -> bool {
        return self.state.is_error();
    }

    pub fn error_code(&self) -> i32 {
        return self.state.error_code;
    }

    pub fn error_message(&self) -> String {
        return self.state.error_message.clone();
    }

    pub fn tick(&mut self, client: &mut CommandClient, sdo: &mut SdoClient) {
        match ModeOfOperationStates::from_repr(self.state.index) {
            Some(ModeOfOperationStates::Reset) => {
                // do nothing
            },
            Some(ModeOfOperationStates::WriteModeOp) => {                
                // Switch the mode of operation into Homing Mode so that we can execute
                // the homing command.
                self.tid = sdo.write(
                    client, 0x6060, 0, json!(self.target_mode),
                );
                self.state.index = ModeOfOperationStates::WaitWriteModeOp as i32;
                self.state.timeout_preset = Duration::from_secs(7);
                log::info!("FbSetModeOfOperation: Waiting write complete target_mode {}", self.target_mode);
            },       
            Some(ModeOfOperationStates::WaitWriteModeOp) => {
                // Wait for method SDO ack
                match sdo.result(client, self.tid, Duration::from_secs(5)) {
                    SdoResult::Ok(_) => { 
                        log::info!("FbSetModeOfOperation: write complete.");
                        self.state.index = ModeOfOperationStates::ReadModeOp as i32;
                        self.state.timer_preset = Duration::from_millis(100);
                    }
                    SdoResult::Pending => {
                        if self.state.timed_out() {
                            self.state.error_code = ModeOfOperationStates::WaitWriteModeOp as i32 * 10;
                            self.state.error_message = "Timeout waiting for SDO write to complete".to_string();
                            self.state.index = ModeOfOperationStates::Reset as i32;
                        }
                    }
                    SdoResult::Err(e) => {
                        self.state.error_code = ModeOfOperationStates::WaitWriteModeOp as i32 * 10 + 1;
                        self.state.error_message = format!("SDO write error {}", e);
                        self.state.index = ModeOfOperationStates::Reset as i32;
                    }
                    SdoResult::Timeout => {
                        self.state.error_code = ModeOfOperationStates::WaitWriteModeOp as i32 * 10 + 2;
                        self.state.error_message = "SDO write resulted in timeout".to_string();
                        self.state.index = ModeOfOperationStates::Reset as i32;
                    }
                }
            },
            Some(ModeOfOperationStates::ReadModeOp) => {
                if self.state.timer_done() {
                    // Setting a requested operating mode doesn't mean we're in it.
                    // Let's read back the mode of operation to ensure
                    log::info!("FbSetModeOfOperation: Starting read to validate operation mode.");
                    self.tid = sdo.read(
                        client, 0x6061, 0
                    );
                    self.state.index = ModeOfOperationStates::WaitReadModeOp as i32;
                    self.state.timeout_preset = Duration::from_secs(7);
                }
            },            
            Some(ModeOfOperationStates::WaitReadModeOp) => {
                // Wait for method SDO ack
                match sdo.result(client, self.tid, Duration::from_secs(5)) {
                    SdoResult::Ok(val) => { 
                        log::info!("FbSetModeOfOperation: read complete.");
                        if let Some(val_num) = val["value"].as_i64() {
                            if val_num as i8 == self.target_mode {
                                log::info!("Successfully changed motor mode of operation to {}", self.target_mode);
                                self.state.index = ModeOfOperationStates::Reset as i32;
                            }
                            else {
                                self.retry_count += 1;
                                if self.retry_count > 3 {                                    
                                    self.state.error_code = ModeOfOperationStates::WaitReadModeOp as i32 * 10 + 3;
                                    self.state.error_message = format!("Drive did not transition to operation mode {} from {}", 
                                        self.target_mode, val_num as i8
                                    );
                                    self.state.index = ModeOfOperationStates::Reset as i32;
                                }
                                else {
                                    log::info!("FbSetModeOfOperation: Operation mode does not match. Retrying...");
                                    self.state.timer_preset = Duration::from_millis(50);
                                    self.state.index = ModeOfOperationStates::ReadModeOp as i32;
                                }
                            }
                        }
                        else {
                            self.state.error_code = ModeOfOperationStates::WaitReadModeOp as i32 * 10 + 4;
                            self.state.error_message = format!("Invalid value received from drive SDO read 0x6061:01  [{}]", val );
                            self.state.index = ModeOfOperationStates::Reset as i32;                            
                        }
                    }
                    SdoResult::Pending => {
                        if self.state.timed_out() {
                            self.state.error_code = ModeOfOperationStates::WaitReadModeOp as i32 * 10;
                            self.state.error_message = "Timeout waiting for SDO write to complete".to_string();
                            self.state.index = ModeOfOperationStates::Reset as i32;
                        }
                    }
                    SdoResult::Err(e) => {
                        self.state.error_code = ModeOfOperationStates::WaitReadModeOp as i32 * 10 + 1;
                        self.state.error_message = format!("SDO write error {}", e);
                        self.state.index = ModeOfOperationStates::Reset as i32;
                    }
                    SdoResult::Timeout => {
                        self.state.error_code = ModeOfOperationStates::WaitReadModeOp as i32 * 10 + 2;
                        self.state.error_message = "SDO write resulted in timeout".to_string();
                        self.state.index = ModeOfOperationStates::Reset as i32;
                    }
                }
            },
            None => {
                self.state.index = ModeOfOperationStates::Reset as i32;
            }
        }


        // tick this state machine
        self.state.call();
    }


}