autocore_std/motion/

axis.rs

//! Stateful motion controller for CiA 402 servo drives.
//!
//! [`Axis`] manages the CiA 402 protocol state machine internally,
//! providing a clean high-level motion API. It owns an [`SdoClient`]
//! for SDO operations during homing.
//!
//! # Usage
//!
//! ```ignore
//! use autocore_std::motion::{Axis, AxisConfig};
//!
//! let config = AxisConfig::new(12_800).with_user_scale(360.0);
//! let mut axis = Axis::new(config, "ClearPath_0");
//!
//! // In your control loop:
//! axis.tick(&mut view, ctx.client);
//!
//! // Command the axis:
//! axis.enable(&mut view);              // start enable sequence
//! axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);
//! axis.home(&mut view, HomingMethod::CurrentPosition);
//! ```

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

use serde_json::json;
use strum_macros::FromRepr;

use crate::command_client::CommandClient;
use crate::ethercat::{SdoClient, SdoResult};
use crate::fb::Ton;
use crate::motion::FbSetModeOfOperation;
use super::axis_config::AxisConfig;
use super::axis_view::AxisView;
use super::homing::HomingMethod;
use super::cia402::{
    Cia402Control, Cia402Status, Cia402State,
    ModesOfOperation, RawControlWord, RawStatusWord,
};

// ──────────────────────────────────────────────
// Internal state machine
// ──────────────────────────────────────────────

#[derive(Debug, Clone, PartialEq)]
enum AxisOp {
    Idle,
    Enabling(u8),
    Disabling(u8),
    Moving(MoveKind, u8, bool, bool),
    Homing(u8),
    SoftHoming(u8),
    Halting(u8),
    FaultRecovery(u8),
}

/// Sub-steps of `AxisOp::Halting`. Mirrors the multi-stage close-out used
/// by soft-homing: wait for motion to stop, issue cancel_move, wait for
/// setpoint_ack, clear new_setpoint, wait for setpoint_ack to drop.
/// Without this sequence the PP handshake is left half-finished and the
/// next `move_absolute` gets rejected with "set-point not acknowledged."
#[repr(u8)]
#[derive(Debug, Clone, PartialEq, FromRepr)]
enum HaltState {
    /// Halt bit set + new_setpoint cleared. Polling position for
    /// stability before issuing cancel_move.
    WaitStopped        = 0,
    /// cancel_move issued. Waiting for SW bit 12 (setpoint_ack) AND
    /// bit 10 (target_reached) — drive has accepted the cancel.
    WaitCancelAck      = 10,
    /// Ack received, new_setpoint cleared, single-setpoint flush bit
    /// asserted. Waiting for SW bit 12 to drop so the next move's
    /// rising edge is clean.
    WaitCancelAckClear = 20,
}

/// How long each halt sub-stage may take before we error out.
const HALT_STAGE_TIMEOUT: Duration = Duration::from_secs(3);

/// Raw-encoder-count window within which the axis is considered "stopped."
/// Sized to tolerate servo micro-oscillation during closed-loop hold.
/// On a 10 000 cnt/mm drive this is 0.005 mm — below any meaningful
/// motion but well above typical ±5 count hold jitter.
const HALT_STABLE_WINDOW: i32 = 50;

/// Velocity magnitude (in raw drive units, typically counts/s) at or
/// below which the axis is considered "not moving" for the purposes of
/// completing a halt. Used alongside position stability so we don't
/// require *both* perfect position and zero velocity — either reliable
/// indicator counts.
const HALT_STOPPED_VELOCITY: i32 = 100;

/// Consecutive ticks of stability required before issuing cancel_move.
/// At a 10 ms scan period, 5 ticks = ~50 ms dwell — long enough for the
/// drive to have actually settled, short enough not to stall the cycle.
const HALT_STABLE_TICKS_REQUIRED: u8 = 5;

#[repr(u8)]
#[derive(Debug, Clone, PartialEq, FromRepr)]
enum HomeState {
    EnsurePpMode = 0,
    WaitPpMode = 1,
    Search = 5,
    WaitSearching = 10,
    WaitFoundSensor = 20,
    WaitStoppedFoundSensor = 30,
    WaitFoundSensorAck = 40,
    WaitFoundSensorAckClear = 45,
    DebounceFoundSensor = 50,
    BackOff = 60,
    WaitBackingOff = 70,
    WaitLostSensor = 80,
    WaitStoppedLostSensor = 90,
    WaitLostSensorAck = 100,
    WaitLostSensorAckClear = 120,
    WaitHomeOffsetDone = 125,

    WriteHomingModeOp = 160,
    WaitWriteHomingModeOp = 165,
    
    WriteHomingMethod = 205,
    WaitWriteHomingMethodDone = 210,
    ClearHomingTrigger = 215,
    TriggerHoming = 217,
    WaitHomingStarted = 218,
    WaitHomingDone = 220,
    ResetHomingTrigger = 222,
    WaitHomingTriggerCleared = 223,
    WriteMotionModeOfOperation = 230,
    WaitWriteMotionModeOfOperation = 235,
    SendCurrentPositionTarget = 240,
    WaitCurrentPositionTargetSent = 245

}

#[derive(Debug, Clone, PartialEq)]
enum MoveKind {
    Absolute,
    Relative,
}

#[derive(Debug, Clone, Copy, PartialEq)]
enum SoftHomeSensor {
    PositiveLimit,
    NegativeLimit,
    HomeSensor,
}

#[derive(Debug, Clone, Copy, PartialEq)]
enum SoftHomeSensorType {
    /// PNP: sensor reads true when object detected (normally open)
    Pnp,
    /// NPN: sensor reads false when object detected (normally closed)
    Npn,
}

// ──────────────────────────────────────────────
// Axis
// ──────────────────────────────────────────────

/// Stateful motion controller for a CiA 402 servo drive.
///
/// Manages the CiA 402 protocol (state machine, PP handshake, homing)
/// internally. Call [`tick()`](Self::tick) every control cycle to progress
/// operations and update output fields.
pub struct Axis {
    config: AxisConfig,
    sdo: SdoClient,

    // ── Internal state ──
    op: AxisOp,
    home_offset: i32,
    last_raw_position: i32,
    op_started: Option<Instant>,
    op_timeout: Duration,
    homing_timeout: Duration,
    move_start_timeout: Duration,
    pending_move_target: i32,
    pending_move_vel: u32,
    pending_move_accel: u32,
    pending_move_decel: u32,
    homing_method: i8,
    homing_sdo_tid: u32,
    soft_home_sensor: SoftHomeSensor,
    soft_home_sensor_type: SoftHomeSensorType,
    soft_home_direction: f64,
    halt_stable_count: u8,
    prev_positive_limit: bool,
    prev_negative_limit: bool,
    prev_home_sensor: bool,



    fb_mode_of_operation : FbSetModeOfOperation,

    // ── Outputs (updated every tick) ──

    /// True if a drive fault or operation timeout has occurred.
    pub is_error: bool,
    /// Drive error code (from status word or view error_code).
    pub error_code: u32,
    /// Human-readable error description.
    pub error_message: String,
    /// True when the drive is in Operation Enabled state.
    pub motor_on: bool,
    /// True when any operation is in progress (enable, move, home, fault recovery, etc.).
    ///
    /// Derived from the internal state machine — immediately true when a command
    /// is issued, false when the operation completes or a fault cancels it.
    /// Use this (not [`in_motion`](Self::in_motion)) to wait for operations to finish.
    pub is_busy: bool,
    /// True while a move operation specifically is active (subset of [`is_busy`](Self::is_busy)).
    pub in_motion: bool,
    /// True when velocity is positive.
    pub moving_positive: bool,
    /// True when velocity is negative.
    pub moving_negative: bool,
    /// Current position in user units (relative to home).
    pub position: f64,
    /// Current position in raw encoder counts (widened from i32).
    pub raw_position: i64,
    /// Current speed in user units/s (absolute value).
    pub speed: f64,
    /// True when position is at or beyond the maximum software limit.
    pub at_max_limit: bool,
    /// True when position is at or beyond the minimum software limit.
    pub at_min_limit: bool,
    /// True when the positive-direction hardware limit switch is active.
    pub at_positive_limit_switch: bool,
    /// True when the negative-direction hardware limit switch is active.
    pub at_negative_limit_switch: bool,
    /// True when the home reference sensor is active.
    pub home_sensor: bool,


    /// Timer used for delays between states.
    ton : Ton
}

impl Axis {
    /// Create a new Axis with the given configuration.
    ///
    /// `device_name` must match the device name in `project.json`
    /// (used for SDO operations during homing).
    pub fn new(config: AxisConfig, device_name: &str) -> Self {
        let op_timeout = Duration::from_secs_f64(config.operation_timeout_secs);
        let homing_timeout = Duration::from_secs_f64(config.homing_timeout_secs);
        let move_start_timeout = op_timeout; // reuse operation timeout for move handshake
        Self {
            config,
            sdo: SdoClient::new(device_name),
            op: AxisOp::Idle,
            home_offset: 0,
            last_raw_position: 0,
            op_started: None,
            op_timeout,
            homing_timeout,
            move_start_timeout,
            pending_move_target: 0,
            pending_move_vel: 0,
            pending_move_accel: 0,
            pending_move_decel: 0,
            homing_method: 37,
            homing_sdo_tid: 0,
            soft_home_sensor: SoftHomeSensor::HomeSensor,
            soft_home_sensor_type: SoftHomeSensorType::Pnp,
            soft_home_direction: 1.0,
            halt_stable_count: 0,
            prev_positive_limit: false,
            prev_negative_limit: false,
            prev_home_sensor: false,
            is_error: false,
            error_code: 0,
            error_message: String::new(),
            motor_on: false,
            is_busy: false,
            in_motion: false,
            moving_positive: false,
            moving_negative: false,
            position: 0.0,
            raw_position: 0,
            speed: 0.0,
            at_max_limit: false,
            at_min_limit: false,
            at_positive_limit_switch: false,
            at_negative_limit_switch: false,
            home_sensor: false,
            ton: Ton::new(),
            fb_mode_of_operation : FbSetModeOfOperation::new()
        }
    }

    /// Get a reference to the axis configuration.
    pub fn config(&self) -> &AxisConfig {
        &self.config
    }

    // ═══════════════════════════════════════════
    // Motion commands
    // ═══════════════════════════════════════════

    /// Start an absolute move to `target` in user units.
    ///
    /// The axis must be enabled (Operation Enabled) before calling this.
    /// If the target exceeds a software position limit, the move is rejected
    /// and [`is_error`](Self::is_error) is set.
    pub fn move_absolute(
        &mut self,
        view: &mut impl AxisView,
        target: f64,
        vel: f64,
        accel: f64,
        decel: f64,
    ) {
        if let Some(msg) = self.check_target_limit(target, view) {
            self.set_op_error(&msg);
            return;
        }

        let cpu = self.config.counts_per_user();
        let raw_target = self.config.to_counts(target).round() as i32 + self.home_offset;
        let raw_vel = (vel * cpu).round() as u32;
        let raw_accel = (accel * cpu).round() as u32;
        let raw_decel = (decel * cpu).round() as u32;

        // User-frame direction (consistent with sw_max/sw_min and hw_pos/hw_neg).
        let user_pos = target > self.position;
        let user_neg = target < self.position;

        self.start_move(view, raw_target, raw_vel, raw_accel, raw_decel, MoveKind::Absolute, user_pos, user_neg);
    }

    /// Start a relative move by `distance` user units from the current position.
    ///
    /// The axis must be enabled (Operation Enabled) before calling this.
    /// If the resulting position would exceed a software position limit,
    /// the move is rejected and [`is_error`](Self::is_error) is set.
    pub fn move_relative(
        &mut self,
        view: &mut impl AxisView,
        distance: f64,
        vel: f64,
        accel: f64,
        decel: f64,
    ) {
        log::info!("Axis: request to move relative dist {} vel {} accel {} decel {}",
            distance, vel, accel, decel
        );

        if let Some(msg) = self.check_target_limit(self.position + distance, view) {
            self.set_op_error(&msg);
            return;
        }

        let cpu = self.config.counts_per_user();
        let raw_distance = self.config.to_counts(distance).round() as i32;
        let raw_vel = (vel * cpu).round() as u32;
        let raw_accel = (accel * cpu).round() as u32;
        let raw_decel = (decel * cpu).round() as u32;

        log::info!("Axis starting relative move: request to move relative raw dist {} raw vel {} raw accel {} raw decel {}",
            raw_distance, raw_vel, raw_accel, raw_decel
        );

        // Make sure bit 4 is off so that a new move can be commanded.
        let mut cw = RawControlWord(view.control_word());
        cw.set_bit(4, false); // new set-point
        view.set_control_word(cw.raw());

        // User-frame direction is the sign of `distance` (which is in user units).
        let user_pos = distance > 0.0;
        let user_neg = distance < 0.0;

        self.start_move(view, raw_distance, raw_vel, raw_accel, raw_decel, MoveKind::Relative, user_pos, user_neg);
    }

    fn start_move(
        &mut self,
        view: &mut impl AxisView,
        raw_target: i32,
        raw_vel: u32,
        raw_accel: u32,
        raw_decel: u32,
        kind: MoveKind,
        user_pos: bool,
        user_neg: bool,
    ) {
        self.pending_move_target = raw_target;
        self.pending_move_vel = raw_vel;
        self.pending_move_accel = raw_accel;
        self.pending_move_decel = raw_decel;

        // Set parameters on view
        view.set_target_position(raw_target);
        view.set_profile_velocity(raw_vel);
        view.set_profile_acceleration(raw_accel);
        view.set_profile_deceleration(raw_decel);

        // Set control word: relative bit + trigger (new set-point).
        // We also clear halt as belt-and-suspenders — limit-switch halts and
        // explicit halt() both flow through the Halting state machine, which
        // ends with halt already cleared, so this is normally a no-op.
        let mut cw = RawControlWord(view.control_word());
        cw.set_bit(6, kind == MoveKind::Relative);
        cw.set_bit(8, false); // clear halt
        cw.set_bit(4, true);  // new set-point
        view.set_control_word(cw.raw());

        // pos/neg stored in AxisOp::Moving are USER-frame so that check_limits
        // can compare them directly against user-frame software limits and
        // the integrator's user-frame limit-switch wiring without an
        // axis-inversion swap.
        self.op = AxisOp::Moving(kind, 1, user_pos, user_neg);
        self.op_started = Some(Instant::now());
    }

    /// Halt the current move (decelerate to stop).
    ///
    /// This is a **multi-tick** operation. `halt()` starts the sequence:
    ///
    /// 1. Halt bit (CW 8) set, new_setpoint (CW 4) cleared.
    /// 2. Wait for motor position to stabilize for ~100 ms.
    /// 3. Issue cancel_move with current_position as target.
    /// 4. Wait for setpoint_ack (SW 12) + target_reached (SW 10).
    /// 5. Clear new_setpoint, set single_setpoint (CW 5).
    /// 6. Wait for setpoint_ack to drop.
    /// 7. Return to Idle.
    ///
    /// [`is_busy`](Self::is_busy) stays `true` for the whole sequence.
    /// Callers that wait on `!is_busy()` after `halt()` (e.g.
    /// [`super::move_to_load::MoveToLoad`]) will correctly block until
    /// the drive's PP handshake is fully cleaned up, preventing a
    /// "set-point not acknowledged" timeout on the *next* move.
    pub fn halt(&mut self, view: &mut impl AxisView) {
        self.command_halt(view);
        self.halt_stable_count = 0;
        self.last_raw_position = view.position_actual();
        self.op_started = Some(Instant::now());
        self.op = AxisOp::Halting(HaltState::WaitStopped as u8);
    }

    // ═══════════════════════════════════════════
    // Drive control
    // ═══════════════════════════════════════════

    /// Start the enable sequence (Shutdown → ReadyToSwitchOn → OperationEnabled).
    ///
    /// The sequence is multi-tick. Check [`motor_on`](Self::motor_on) for completion.
    pub fn enable(&mut self, view: &mut impl AxisView) {
        // Step 0: set PP mode + cmd_shutdown
        view.set_modes_of_operation(ModesOfOperation::ProfilePosition.as_i8());
        let mut cw = RawControlWord(view.control_word());
        cw.cmd_shutdown();
        view.set_control_word(cw.raw());

        self.op = AxisOp::Enabling(1);
        self.op_started = Some(Instant::now());
    }

    /// Start the disable sequence (OperationEnabled → SwitchedOn).
    pub fn disable(&mut self, view: &mut impl AxisView) {
        let mut cw = RawControlWord(view.control_word());
        cw.cmd_disable_operation();
        cw.set_bit(4, false);
        cw.set_bit(8, false);
        cw.set_bit(7, false);
        cw.set_bit(2, true);
        view.set_control_word(cw.raw());

        self.op = AxisOp::Disabling(1);
        self.op_started = Some(Instant::now());
    }

    /// Clear the axis error state.
    ///
    /// Two paths depending on whether the drive itself is faulted:
    ///
    /// - **Drive in CiA 402 Fault / FaultReactionActive**: runs the full
    ///   reset sequence — clear bit 7, then assert it next tick (rising
    ///   edge on the drive's fault-reset bit), then wait for the drive to
    ///   leave the Fault state. `is_busy` stays true through this sequence.
    ///
    /// - **Drive healthy** (e.g. the `is_error` was set by a software
    ///   software-limit overshoot, an operation timeout, or a limit-switch
    ///   close-out timeout — the drive itself is still in OperationEnabled):
    ///   no CiA 402 handshake is needed. Just clears the software error
    ///   flags and returns the axis to Idle synchronously. `is_busy` goes
    ///   to false on the same call.
    pub fn reset_faults(&mut self, view: &mut impl AxisView) {
        let sw = RawStatusWord(view.status_word());
        let drive_in_fault = matches!(
            sw.state(),
            Cia402State::Fault | Cia402State::FaultReactionActive
        );

        self.is_error = false;
        self.error_code = 0;
        self.error_message.clear();

        if drive_in_fault {
            // Step 0: clear bit 7 first (so next step produces a rising edge).
            let mut cw = RawControlWord(view.control_word());
            cw.cmd_clear_fault_reset();
            view.set_control_word(cw.raw());
            self.op = AxisOp::FaultRecovery(1);
            self.op_started = Some(Instant::now());
        } else {
            // Software-only error — no drive handshake needed.
            self.op = AxisOp::Idle;
            self.op_started = None;
            self.is_busy = false;
            self.in_motion = false;
        }
    }

    /// Start a homing sequence with the given homing method.
    ///
    /// **Integrated** methods delegate to the drive's built-in CiA 402
    /// homing mode (SDO writes + homing trigger).
    ///
    /// **Software** methods are implemented by the Axis, which monitors
    /// [`AxisView`] sensor signals for edge triggers and captures home.
    pub fn home(&mut self, view: &mut impl AxisView, method: HomingMethod) {
        if method.is_integrated() {
            self.homing_method = match method {
                HomingMethod::CurrentPosition => self.config.soft_home_method,
                _ => method.cia402_code(),
            };
            self.op = AxisOp::Homing(0);
            self.op_started = Some(Instant::now());
            let _ = view;
        } else {
            self.configure_soft_homing(method);
            self.start_soft_homing(view);
        }
    }

    // ═══════════════════════════════════════════
    // Position management
    // ═══════════════════════════════════════════

    /// Set the current position to the given user-unit value.
    ///
    /// Adjusts the internal home offset so that the current raw position
    /// maps to `user_units`. Does not move the motor.
    pub fn set_position(&mut self, view: &impl AxisView, user_units: f64) {
        self.home_offset = view.position_actual() - self.config.to_counts(user_units).round() as i32;
    }

    /// Set the current position to `user_units` and persist it to the drive.
    ///
    /// Writes `0x607C` (home offset) to the drive in encoder counts, then runs
    /// a CiA 402 homing cycle with method [`AxisConfig::soft_home_method`] so
    /// the drive's reported position (`0x6064`) reads `user_units` afterward.
    /// No motor motion occurs.
    ///
    /// For drives with absolute encoders configured in linear mode (e.g.
    /// Inovance SV660N with `0x2002:02 = 1`), the resulting encoder offset
    /// (`0x2005:2F`/`0x2005:31`) is auto-saved to EEPROM, so the new reference
    /// survives a power cycle.
    ///
    /// **Preconditions:** drive must be enabled (OperationEnabled) and at rest
    /// (no move in progress). The operation switches to HM mode internally and
    /// back to PP on completion. Poll [`is_busy`](Self::is_busy) to know when
    /// it finishes.
    pub fn set_position_persistent(
        &mut self,
        client: &mut CommandClient,
        user_units: f64,
    ) {
        // Reuse the SoftHoming state machine starting from the post-607C-write
        // step (WaitHomeOffsetDone). We issue the 607C SDO write here; the
        // state machine handles the rest: wait for ack, switch to HM, write
        // 6098 = soft_home_method, trigger homing, wait for completion, switch
        // back to PP, and zero the software home_offset since the drive owns
        // it now.
        self.config.home_position = user_units;
        let desired_counts = self.config.to_counts(user_units).round() as i32;
        self.homing_sdo_tid = self.sdo.write(client, 0x607C, 0, json!(desired_counts));
        self.op = AxisOp::SoftHoming(HomeState::WaitHomeOffsetDone as u8);
        self.op_started = Some(Instant::now());
        log::info!(
            "set_position_persistent: writing 0x607C = {} counts ({} user units)",
            desired_counts, user_units
        );
    }

    /// Set the home position in user units. This value is used by the next
    /// `home()` call to set the axis position at the reference point.
    /// Can be called at any time before homing.
    pub fn set_home_position(&mut self, user_units: f64) {
        self.config.home_position = user_units;
    }

    /// Set the maximum (positive) software position limit.
    pub fn set_software_max_limit(&mut self, user_units: f64) {
        self.config.max_position_limit = user_units;
        self.config.enable_max_position_limit = true;
    }

    /// Set the minimum (negative) software position limit.
    pub fn set_software_min_limit(&mut self, user_units: f64) {
        self.config.min_position_limit = user_units;
        self.config.enable_min_position_limit = true;
    }

    // ═══════════════════════════════════════════
    // SDO pass-through
    // ═══════════════════════════════════════════

    /// Write an SDO value to the drive.
    pub fn sdo_write(
        &mut self,
        client: &mut CommandClient,
        index: u16,
        sub_index: u8,
        value: serde_json::Value,
    ) {
        self.sdo.write(client, index, sub_index, value);
    }

    /// Start an SDO read from the drive. Returns a transaction ID.
    pub fn sdo_read(
        &mut self,
        client: &mut CommandClient,
        index: u16,
        sub_index: u8,
    ) -> u32 {
        self.sdo.read(client, index, sub_index)
    }

    /// Check the result of a previous SDO read.
    pub fn sdo_result(
        &mut self,
        client: &mut CommandClient,
        tid: u32,
    ) -> SdoResult {
        self.sdo.result(client, tid, Duration::from_secs(5))
    }

    // ═══════════════════════════════════════════
    // Tick — call every control cycle
    // ═══════════════════════════════════════════

    /// Update outputs and progress the current operation.
    ///
    /// Must be called every control cycle. Does three things:
    /// 1. Checks for drive faults
    /// 2. Progresses the current multi-tick operation
    /// 3. Updates output fields (position, velocity, status)
    ///
    /// Outputs are updated last so they reflect the final state after
    /// all processing for this tick.
    pub fn tick(&mut self, view: &mut impl AxisView, client: &mut CommandClient) {
        self.check_faults(view);
        self.progress_op(view, client);
        self.update_outputs(view);
        self.check_limits(view);
    }

    // ═══════════════════════════════════════════
    // Internal: output update
    // ═══════════════════════════════════════════

    fn update_outputs(&mut self, view: &impl AxisView) {
        let raw = view.position_actual();
        self.raw_position = raw as i64;
        self.position = self.config.to_user((raw - self.home_offset) as f64);

        let vel = view.velocity_actual();
        let user_vel = self.config.to_user(vel as f64);
        self.speed = user_vel.abs();
        self.moving_positive = user_vel > 0.0;
        self.moving_negative = user_vel < 0.0;
        self.is_busy = self.op != AxisOp::Idle;
        self.in_motion = matches!(self.op, AxisOp::Moving(_, _, _, _) | AxisOp::SoftHoming(_));

        let sw = RawStatusWord(view.status_word());
        self.motor_on = sw.state() == Cia402State::OperationEnabled;

        self.last_raw_position = raw;
    }

    // ═══════════════════════════════════════════
    // Internal: fault check
    // ═══════════════════════════════════════════

    fn check_faults(&mut self, view: &impl AxisView) {
        let sw = RawStatusWord(view.status_word());
        let state = sw.state();

        if matches!(state, Cia402State::Fault | Cia402State::FaultReactionActive) {
            if !matches!(self.op, AxisOp::FaultRecovery(_)) {
                self.is_error = true;
                let ec = view.error_code();
                if ec != 0 {
                    self.error_code = ec as u32;
                }
                self.error_message = format!("Drive fault (state: {})", state);
                // Cancel the current operation so is_busy goes false
                self.op = AxisOp::Idle;
                self.op_started = None;
            }
        }
    }

    // ═══════════════════════════════════════════
    // Internal: operation timeout helper
    // ═══════════════════════════════════════════

    fn op_timed_out(&self) -> bool {
        self.op_started
            .map_or(false, |t| t.elapsed() > self.op_timeout)
    }

    fn homing_timed_out(&self) -> bool {
        self.op_started
            .map_or(false, |t| t.elapsed() > self.homing_timeout)
    }

    fn move_start_timed_out(&self) -> bool {
        self.op_started
            .map_or(false, |t| t.elapsed() > self.move_start_timeout)
    }

    /// Has the current operation exceeded the supplied stage timeout?
    /// Used by the halt state machine so each sub-stage gets its own
    /// budget rather than sharing the general `op_timeout`.
    fn op_stage_timed_out(&self, limit: Duration) -> bool {
        self.op_started
            .map_or(false, |t| t.elapsed() > limit)
    }

    fn set_op_error(&mut self, msg: &str) {
        self.is_error = true;
        self.error_message = msg.to_string();
        self.op = AxisOp::Idle;
        self.op_started = None;
        self.is_busy = false;
        self.in_motion = false;
        log::error!("Axis error: {}", msg);
    }

    fn restore_pp_after_error(&mut self, msg: &str) {
        self.is_error = true;
        self.error_message = msg.to_string();
        self.op = AxisOp::SoftHoming(HomeState::WriteMotionModeOfOperation as u8);;
        log::error!("Axis error: {}", msg);
    }

    fn finish_op_error(&mut self) {
        self.op = AxisOp::Idle;
        self.op_started = None;
        self.is_busy = false;
        self.in_motion = false;
    }

    fn complete_op(&mut self) {
        self.op = AxisOp::Idle;
        self.op_started = None;
        // Clear busy/motion flags directly so callers that observe state on
        // the same tick (e.g. check_limits, which runs after update_outputs)
        // see Idle immediately. For complete_op calls inside progress_op,
        // update_outputs would recompute these to the same values anyway.
        self.is_busy = false;
        self.in_motion = false;
    }

    // ═══════════════════════════════════════════
    // Internal: position limits and limit switches
    // ═══════════════════════════════════════════

    /// Resolve the effective maximum software limit for this tick, combining
    /// the static [`AxisConfig`] value (if enabled) with any dynamic limit
    /// supplied by the [`AxisView`] (e.g. a GM-linked variable). The most
    /// restrictive (smallest) value wins.
    fn effective_max_limit(&self, view: &impl AxisView) -> Option<f64> {
        let static_limit = if self.config.enable_max_position_limit {
            Some(self.config.max_position_limit)
        } else {
            None
        };
        match (static_limit, view.dynamic_max_position_limit()) {
            (Some(s), Some(d)) => Some(s.min(d)),
            (Some(v), None) | (None, Some(v)) => Some(v),
            (None, None) => None,
        }
    }

    /// Resolve the effective minimum software limit for this tick. See
    /// [`effective_max_limit`](Self::effective_max_limit) — the most
    /// restrictive (largest) value wins.
    fn effective_min_limit(&self, view: &impl AxisView) -> Option<f64> {
        let static_limit = if self.config.enable_min_position_limit {
            Some(self.config.min_position_limit)
        } else {
            None
        };
        match (static_limit, view.dynamic_min_position_limit()) {
            (Some(s), Some(d)) => Some(s.max(d)),
            (Some(v), None) | (None, Some(v)) => Some(v),
            (None, None) => None,
        }
    }

    /// Check whether a target position (in user units) exceeds a software limit.
    /// Returns `Some(error_message)` if the target is out of range, `None` if OK.
    /// Consults both the static [`AxisConfig`] limits and any dynamic limits
    /// exposed by the view, taking whichever is most restrictive.
    fn check_target_limit(&self, target: f64, view: &impl AxisView) -> Option<String> {
        if let Some(max) = self.effective_max_limit(view) {
            if target > max {
                return Some(format!(
                    "Target {:.3} exceeds max software limit {:.3}",
                    target, max
                ));
            }
        }
        if let Some(min) = self.effective_min_limit(view) {
            if target < min {
                return Some(format!(
                    "Target {:.3} exceeds min software limit {:.3}",
                    target, min
                ));
            }
        }
        None
    }

    /// Check software position limits, hardware limit switches, and home sensor.
    /// If a limit is violated and a move is in progress in that direction,
    /// halt the drive and set an error. Moving in the opposite direction is
    /// always allowed so the axis can be recovered.
    ///
    /// During software homing on a limit switch (`SoftHoming` + `SoftHomeSensor::PositiveLimit`
    /// or `NegativeLimit`), the homed-on switch is suppressed so it triggers a home
    /// event rather than an error halt. The opposite switch still protects.
    fn check_limits(&mut self, view: &mut impl AxisView) {
        // ── Software position limits (static config + dynamic GM-linked) ──
        let eff_max = self.effective_max_limit(view);
        let eff_min = self.effective_min_limit(view);
        let sw_max = eff_max.map_or(false, |m| self.position >= m);
        let sw_min = eff_min.map_or(false, |m| self.position <= m);

        self.at_max_limit = sw_max;
        self.at_min_limit = sw_min;

        // ── Hardware limit switches ──
        let hw_pos = view.positive_limit_active();
        let hw_neg = view.negative_limit_active();

        self.at_positive_limit_switch = hw_pos;
        self.at_negative_limit_switch = hw_neg;

        // ── Home sensor ──
        self.home_sensor = view.home_sensor_active();

        // ── Save previous sensor state for next tick's edge detection ──
        self.prev_positive_limit = hw_pos;
        self.prev_negative_limit = hw_neg;
        self.prev_home_sensor = view.home_sensor_active();

        // ── Halt logic (only while moving or soft-homing) ──
        let mut commanded_positive = false;
        let mut commanded_negative = false;

        let is_moving = matches!(self.op, AxisOp::Moving(_, _, _, _));
        let is_soft_homing = matches!(self.op, AxisOp::SoftHoming(_));

        if !is_moving && !is_soft_homing {
            return; // Only halt actively if we are currently driving into the limit
        }

        match &self.op {
            AxisOp::Moving(_, _, pos, neg) => {
                // Already user-frame (set by move_absolute / move_relative).
                commanded_positive = *pos;
                commanded_negative = *neg;
            }
            AxisOp::SoftHoming(_) => {
                match self.soft_home_sensor {
                    SoftHomeSensor::PositiveLimit => commanded_positive = true,
                    SoftHomeSensor::NegativeLimit => commanded_negative = true,
                    SoftHomeSensor::HomeSensor => {
                        commanded_positive = self.moving_positive;
                        commanded_negative = self.moving_negative;
                    }
                }
            }
            _ => {}
        }

        // During software homing, suppress the limit switch being homed on
        let suppress_pos = is_soft_homing && self.soft_home_sensor == SoftHomeSensor::PositiveLimit;
        let suppress_neg = is_soft_homing && self.soft_home_sensor == SoftHomeSensor::NegativeLimit;

        let effective_hw_pos = hw_pos && !suppress_pos;
        let effective_hw_neg = hw_neg && !suppress_neg;

        // During soft homing, suppress software limits too (we need to move freely)
        let effective_sw_max = sw_max && !is_soft_homing;
        let effective_sw_min = sw_min && !is_soft_homing;

        let positive_blocked = (effective_sw_max || effective_hw_pos) && commanded_positive;
        let negative_blocked = (effective_sw_min || effective_hw_neg) && commanded_negative;


        if positive_blocked || negative_blocked {
            let mut cw = RawControlWord(view.control_word());
            cw.set_bit(8, true); // halt
            view.set_control_word(cw.raw());

            let msg = if effective_hw_pos && commanded_positive {
                "Positive limit switch active".to_string()
            } else if effective_hw_neg && commanded_negative {
                "Negative limit switch active".to_string()
            } else if effective_sw_max && commanded_positive {
                format!(
                    "Software position limit: position {:.3} >= max {:.3}",
                    self.position, eff_max.unwrap_or(self.position)
                )
            } else {
                format!(
                    "Software position limit: position {:.3} <= min {:.3}",
                    self.position, eff_min.unwrap_or(self.position)
                )
            };

            if is_soft_homing {
                // Hitting the *opposite* limit during soft homing is a real
                // fault — the axis went the wrong way and must be reset.
                self.set_op_error(&msg);
            } else {
                // Regular move into a limit: enter the multi-stage Halting
                // close-out (wait for motor stop → cancel queued setpoint →
                // clear halt) so the drive ends in a clean state. Just
                // clearing halt isn't enough — the previous move's target is
                // still queued in the drive's PP buffer, and on the next
                // start_move some drives ignore the new setpoint until that
                // queued one is canceled.
                //
                // is_busy stays true through the close-out. Callers detect
                // why the move ended by inspecting
                // `at_positive_limit_switch` / `at_negative_limit_switch` /
                // `at_max_limit` / `at_min_limit` once `!is_busy()`.
                log::info!("Axis move halted by limit: {}", msg);
                self.command_halt(view);
                self.halt_stable_count = 0;
                self.last_raw_position = view.position_actual();
                self.op_started = Some(Instant::now());
                self.op = AxisOp::Halting(HaltState::WaitStopped as u8);
            }
        }
    }

    // ═══════════════════════════════════════════
    // Internal: operation progress
    // ═══════════════════════════════════════════

    fn progress_op(&mut self, view: &mut impl AxisView, client: &mut CommandClient) {
        match self.op.clone() {
            AxisOp::Idle => {}
            AxisOp::Enabling(step) => self.tick_enabling(view, step),
            AxisOp::Disabling(step) => self.tick_disabling(view, step),
            AxisOp::Moving(kind, step, pos, neg) => self.tick_moving(view, kind, step, pos, neg),
            AxisOp::Homing(step) => self.tick_homing(view, client, step),
            AxisOp::SoftHoming(step) => self.tick_soft_homing(view, client, step),
            AxisOp::Halting(step) => self.tick_halting(view, step),
            AxisOp::FaultRecovery(step) => self.tick_fault_recovery(view, step),
        }
    }

    // ── Enabling ──
    // Step 0: (done in enable()) ensure PP + cmd_shutdown
    // Step 1: wait ReadyToSwitchOn → cmd_enable_operation
    // Step 2: wait OperationEnabled → capture home → Idle
    fn tick_enabling(&mut self, view: &mut impl AxisView, step: u8) {
        match step {
            1 => {
                let sw = RawStatusWord(view.status_word());
                if sw.state() == Cia402State::ReadyToSwitchOn {
                    let mut cw = RawControlWord(view.control_word());
                    cw.cmd_enable_operation();
                    view.set_control_word(cw.raw());
                    self.op = AxisOp::Enabling(2);
                } else if self.op_timed_out() {
                    self.set_op_error("Enable timeout: waiting for ReadyToSwitchOn");
                }
            }
            2 => {
                let sw = RawStatusWord(view.status_word());
                if sw.state() == Cia402State::OperationEnabled {
                    // NO - We do not do software-based encoder. That would break absolute encoders.
                    // self.home_offset = view.position_actual();
                    // log::info!("Axis enabled — home captured at {}", self.home_offset);

                    // Possible TODO: Read the home_offset in the drive? 

                    self.complete_op();
                } else if self.op_timed_out() {
                    self.set_op_error("Enable timeout: waiting for OperationEnabled");
                }
            }
            _ => self.complete_op(),
        }
    }

    // ── Disabling ──
    // Step 0: (done in disable()) cmd_disable_operation
    // Step 1: wait not OperationEnabled → Idle
    fn tick_disabling(&mut self, view: &mut impl AxisView, step: u8) {
        match step {
            1 => {
                let sw = RawStatusWord(view.status_word());
                if sw.state() != Cia402State::OperationEnabled {
                    self.complete_op();
                } else if self.op_timed_out() {
                    self.set_op_error("Disable timeout: drive still in OperationEnabled");
                }
            }
            _ => self.complete_op(),
        }
    }

    // ── Moving ──
    // Step 0: (done in move_absolute/relative()) set params + trigger
    // Step 1: wait set_point_acknowledge → ack
    // Step 2: wait ack cleared (one tick)
    // Step 3: wait target_reached → Idle
    fn tick_moving(&mut self, view: &mut impl AxisView, kind: MoveKind, step: u8, pos: bool, neg: bool) {
        match step {
            1 => {
                // Wait for set-point acknowledge (bit 12)
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) != 0 {
                    // Ack: clear new set-point (bit 4)
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);
                    view.set_control_word(cw.raw());
                    self.op = AxisOp::Moving(kind, 2, pos, neg);
                } else if self.move_start_timed_out() {
                    self.set_op_error("Move timeout: set-point not acknowledged");
                }
            },
            2 => {
                // Wait for the drive to confirm it saw Bit 4 go low
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) == 0 {
                    // Handshake is officially reset. Now wait for physics.
                    self.op = AxisOp::Moving(kind, 3, pos, neg);
                }
            },
            3 => {
                // Wait for target reached (bit 10) — no timeout, moves can take arbitrarily long
                let sw = RawStatusWord(view.status_word());
                if sw.target_reached() {
                    self.complete_op();
                }
            },
            _ => self.complete_op(),
        }
    }

    // ── Homing (hardware-delegated) ──
    // Step 0:  write homing method SDO (0x6098:0)
    // Step 1:  wait SDO ack
    // Step 2:  write homing speed SDO (0x6099:1 — search for switch)
    // Step 3:  wait SDO ack
    // Step 4:  write homing speed SDO (0x6099:2 — search for zero)
    // Step 5:  wait SDO ack
    // Step 6:  write homing accel SDO (0x609A:0)
    // Step 7:  wait SDO ack
    // Step 8:  switch to Homing mode (PDO field + SDO write 0x6060)
    // Step 9:  wait for Homing mode written + verified via SDO read-back (0x6061)
    // Step 10: trigger homing (rising edge on CW bit 4)
    // Step 11: wait for drive to acknowledge start (bit 12 clears)
    // Step 12: wait homing complete (bits 10+12 set, 13 clear)
    // Step 13: capture home offset, clear CW bit 4
    // Step 14: switch back to PP mode (PDO field + SDO write 0x6060)
    // Step 15: wait for PP mode written + verified, then → Idle
    //
    // If homing_speed and homing_accel are both 0, steps 2-7 are skipped
    // (preserves backward compatibility for users who pre-configure via SDO).
    //
    // Mode is switched AND verified over SDO (via FbSetModeOfOperation) rather
    // than trusting modes_of_operation_display(): on drives that don't PDO-map
    // 0x6060/0x6061 (e.g. Kollmorgen AKD) the cyclic display field is a codegen
    // stub mirrored from the commanded value, so it would falsely confirm.
    fn tick_homing(
        &mut self,
        view: &mut impl AxisView,
        client: &mut CommandClient,
        step: u8,
    ) {
        match step {
            0 => {
                // Write homing method via SDO (0x6098:0)
                self.homing_sdo_tid = self.sdo.write(
                    client,
                    0x6098,
                    0,
                    json!(self.homing_method),
                );
                self.op = AxisOp::Homing(1);
            }
            1 => {
                // Wait for SDO write ack
                match self.sdo.result(client, self.homing_sdo_tid, Duration::from_secs(5)) {
                    SdoResult::Ok(_) => {
                        // Skip speed/accel SDOs if both are zero
                        if self.config.homing_speed == 0.0 && self.config.homing_accel == 0.0 {
                            self.op = AxisOp::Homing(8);
                        } else {
                            self.op = AxisOp::Homing(2);
                        }
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Homing timeout: SDO write for homing method");
                        }
                    }
                    SdoResult::Err(e) => {
                        self.set_op_error(&format!("Homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.set_op_error("Homing timeout: SDO write timed out");
                    }
                }
            }
            2 => {
                // Write homing speed (0x6099:1 — search for switch)
                let speed_counts = self.config.to_counts(self.config.homing_speed).round() as u32;
                self.homing_sdo_tid = self.sdo.write(
                    client,
                    0x6099,
                    1,
                    json!(speed_counts),
                );
                self.op = AxisOp::Homing(3);
            }
            3 => {
                match self.sdo.result(client, self.homing_sdo_tid, Duration::from_secs(5)) {
                    SdoResult::Ok(_) => { self.op = AxisOp::Homing(4); }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Homing timeout: SDO write for homing speed (switch)");
                        }
                    }
                    SdoResult::Err(e) => { self.set_op_error(&format!("Homing SDO error: {}", e)); }
                    SdoResult::Timeout => { self.set_op_error("Homing timeout: SDO write timed out"); }
                }
            }
            4 => {
                // Write homing speed (0x6099:2 — search for zero, same value)
                let speed_counts = self.config.to_counts(self.config.homing_speed).round() as u32;
                self.homing_sdo_tid = self.sdo.write(
                    client,
                    0x6099,
                    2,
                    json!(speed_counts),
                );
                self.op = AxisOp::Homing(5);
            }
            5 => {
                match self.sdo.result(client, self.homing_sdo_tid, Duration::from_secs(5)) {
                    SdoResult::Ok(_) => { self.op = AxisOp::Homing(6); }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Homing timeout: SDO write for homing speed (zero)");
                        }
                    }
                    SdoResult::Err(e) => { self.set_op_error(&format!("Homing SDO error: {}", e)); }
                    SdoResult::Timeout => { self.set_op_error("Homing timeout: SDO write timed out"); }
                }
            }
            6 => {
                // Write homing acceleration (0x609A:0)
                let accel_counts = self.config.to_counts(self.config.homing_accel).round() as u32;
                self.homing_sdo_tid = self.sdo.write(
                    client,
                    0x609A,
                    0,
                    json!(accel_counts),
                );
                self.op = AxisOp::Homing(7);
            }
            7 => {
                match self.sdo.result(client, self.homing_sdo_tid, Duration::from_secs(5)) {
                    SdoResult::Ok(_) => { self.op = AxisOp::Homing(8); }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Homing timeout: SDO write for homing acceleration");
                        }
                    }
                    SdoResult::Err(e) => { self.set_op_error(&format!("Homing SDO error: {}", e)); }
                    SdoResult::Timeout => { self.set_op_error("Homing timeout: SDO write timed out"); }
                }
            }
            8 => {
                // Switch into Homing mode on BOTH channels:
                //  - the cyclic PDO field, so drives that PDO-map 0x6060 carry the
                //    correct mode on the wire, and
                //  - an SDO write + read-back of 0x6060/0x6061 (FbSetModeOfOperation),
                //    so drives that pin mode by SDO (e.g. Kollmorgen AKD, which does
                //    not PDO-map mode) actually transition AND are verified.
                // The SDO read-back replaces the old modes_of_operation_display()
                // check, which silently passed on PDO-unmapped drives because codegen
                // mirrors the commanded value into the display stub.
                view.set_modes_of_operation(ModesOfOperation::Homing.as_i8());
                // Ensure CW bit 4 starts LOW so the next step issues a clean rising edge.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                self.fb_mode_of_operation.reset();
                self.fb_mode_of_operation.start(ModesOfOperation::Homing as i8);
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                self.op = AxisOp::Homing(9);
            }
            9 => {
                // Wait for the mode switch to be written and verified over SDO.
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!(
                            "Homing: failed to enter Homing mode: {} {}",
                            self.fb_mode_of_operation.error_code(),
                            self.fb_mode_of_operation.error_message()
                        ));
                    } else {
                        self.op = AxisOp::Homing(10);
                    }
                }
            }
            10 => {
                // Trigger homing: rising edge on bit 4
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, true);
                view.set_control_word(cw.raw());
                self.op = AxisOp::Homing(11);
            }
            11 => {
                // Wait for the drive to clear bit 12 to acknowledge the start
                // of homing. Without this, stale bit 12 from the previous mode
                // (e.g. PP set-point acknowledge) would let the next step pass
                // instantly even though the drive never ran the method.
                let sw = view.status_word();
                let error = sw & (1 << 13) != 0;
                if error {
                    self.set_op_error("Homing error: drive reported homing failure");
                } else if sw & (1 << 12) == 0 {
                    self.op = AxisOp::Homing(12);
                } else if self.homing_timed_out() {
                    self.set_op_error(&format!("Homing timeout: drive did not acknowledge homing start (sw=0x{:04X})", sw));
                }
            }
            12 => {
                // Wait for homing complete
                // Bit 13 = error, Bit 12 = attained, Bit 10 = reached
                let sw = view.status_word();
                let error    = sw & (1 << 13) != 0;
                let attained = sw & (1 << 12) != 0;
                let reached  = sw & (1 << 10) != 0;

                if error {
                    self.set_op_error("Homing error: drive reported homing failure");
                } else if attained && reached {
                    self.op = AxisOp::Homing(13);
                } else if self.homing_timed_out() {
                    self.set_op_error("Homing timeout: procedure did not complete");
                }
            }
            13 => {
                // Capture home offset, applying home_position so the reference
                // point reads as config.home_position in user units.
                self.home_offset = view.position_actual()
                    - self.config.to_counts(self.config.home_position).round() as i32;
                // Clear homing start bit in its own cycle before switching modes
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                self.op = AxisOp::Homing(14);
            }
            14 => {
                // One tick later (bit 4 already cleared in step 13, so the drive
                // sees the falling edge before the mode change), switch back to PP
                // mode. Same dual-channel approach as the switch into Homing: cyclic
                // PDO field plus an SDO write + read-back so it lands and is verified
                // on drives that don't PDO-map mode.
                view.set_modes_of_operation(ModesOfOperation::ProfilePosition.as_i8());
                self.fb_mode_of_operation.reset();
                self.fb_mode_of_operation.start(ModesOfOperation::ProfilePosition as i8);
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                self.op = AxisOp::Homing(15);
            }
            15 => {
                // Wait for the switch back to PP mode to be written and verified.
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!(
                            "Homing: failed to restore PP mode: {} {}",
                            self.fb_mode_of_operation.error_code(),
                            self.fb_mode_of_operation.error_message()
                        ));
                    } else {
                        log::info!("Homing complete — home offset: {}", self.home_offset);
                        self.complete_op();
                    }
                }
            }
            _ => self.complete_op(),
        }
    }

    // ── Software homing helpers ──

    fn configure_soft_homing(&mut self, method: HomingMethod) {
        match method {
            HomingMethod::LimitSwitchPosPnp => {
                self.soft_home_sensor = SoftHomeSensor::PositiveLimit;
                self.soft_home_sensor_type = SoftHomeSensorType::Pnp;
                self.soft_home_direction = 1.0;
            }
            HomingMethod::LimitSwitchNegPnp => {
                self.soft_home_sensor = SoftHomeSensor::NegativeLimit;
                self.soft_home_sensor_type = SoftHomeSensorType::Pnp;
                self.soft_home_direction = -1.0;
            }
            HomingMethod::LimitSwitchPosNpn => {
                self.soft_home_sensor = SoftHomeSensor::PositiveLimit;
                self.soft_home_sensor_type = SoftHomeSensorType::Npn;
                self.soft_home_direction = 1.0;
            }
            HomingMethod::LimitSwitchNegNpn => {
                self.soft_home_sensor = SoftHomeSensor::NegativeLimit;
                self.soft_home_sensor_type = SoftHomeSensorType::Npn;
                self.soft_home_direction = -1.0;
            }
            HomingMethod::HomeSensorPosPnp => {
                self.soft_home_sensor = SoftHomeSensor::HomeSensor;
                self.soft_home_sensor_type = SoftHomeSensorType::Pnp;
                self.soft_home_direction = 1.0;
            }
            HomingMethod::HomeSensorNegPnp => {
                self.soft_home_sensor = SoftHomeSensor::HomeSensor;
                self.soft_home_sensor_type = SoftHomeSensorType::Pnp;
                self.soft_home_direction = -1.0;
            }
            HomingMethod::HomeSensorPosNpn => {
                self.soft_home_sensor = SoftHomeSensor::HomeSensor;
                self.soft_home_sensor_type = SoftHomeSensorType::Npn;
                self.soft_home_direction = 1.0;
            }
            HomingMethod::HomeSensorNegNpn => {
                self.soft_home_sensor = SoftHomeSensor::HomeSensor;
                self.soft_home_sensor_type = SoftHomeSensorType::Npn;
                self.soft_home_direction = -1.0;
            }
            _ => {} // integrated methods handled elsewhere
        }
    }

    fn start_soft_homing(&mut self, view: &mut impl AxisView) {
        self.op = AxisOp::SoftHoming(HomeState::EnsurePpMode as u8);
        self.op_started = Some(Instant::now());
    }

    fn check_soft_home_trigger(&self, view: &impl AxisView) -> bool {
        let raw = match self.soft_home_sensor {
            SoftHomeSensor::PositiveLimit => view.positive_limit_active(),
            SoftHomeSensor::NegativeLimit => view.negative_limit_active(),
            SoftHomeSensor::HomeSensor    => view.home_sensor_active(),
        };
        match self.soft_home_sensor_type {
            SoftHomeSensorType::Pnp => raw,    // PNP: true = detected
            SoftHomeSensorType::Npn => !raw,   // NPN: false = detected
        }
    }


    /// Calculate the maximum relative target for the specified direction.
    /// The result is adjusted for whether the motor direction has been inverted.
    fn calculate_max_relative_target(&self, direction : f64) -> i32 {
        let dir = if !self.config.invert_direction  {
            direction
        } 
        else {
            -direction
        };

        let target = if dir > 0.0 {
            i32::MAX 
        }
        else {
            i32::MIN
        };

        return target;
    }


    /// Convenient macro
    /// Configure the command word for an immediate halt
    /// and reset the new setpoint bit, which should cause
    /// status word bit 12 to clear
    pub fn command_halt(&self, view: &mut impl AxisView) {
        let mut cw = RawControlWord(view.control_word());
        cw.set_bit(8, true);  // halt
        cw.set_bit(4, false);  // reset new setpoint bit
        cw.set_bit(5, true);  // single set-point. If false, the new move will be queued!
        cw.set_bit(6, false); // absolute move
        view.set_control_word(cw.raw());        
    }

    /// Convenient macro
    /// Configure the command word to clear the halt bit
    pub fn command_clear_halt(&self, view: &mut impl AxisView) {
        let mut cw = RawControlWord(view.control_word());
        cw.set_bit(8, false);  // reset halt
        view.set_control_word(cw.raw());        
    }


    /// Convenient macro.
    /// Configures command bits and targets to cancel a previous move.
    /// Bit 4 should be off before calling this function.
    /// The current absolute position will be used as the target, so there 
    /// should be no motion
    /// Halt will be turned on, if not already.]
    /// After this, wait for bit 12 to be true before clearing the halt bit.
    pub fn command_cancel_move(&self, view: &mut impl AxisView) {

        let mut cw = RawControlWord(view.control_word());

        cw.set_bit(4, true);  // new set-point
        cw.set_bit(5, true);  // single set-point. If false, the new move will be queued!
        cw.set_bit(6, false); // absolute move

        if self.config().clear_halt_during_cancel {
            // Doesn't work with Teknic!
            cw.set_bit(8, false); // clear halt
        }
        
        view.set_control_word(cw.raw());        

        let current_pos = view.position_actual();
        view.set_target_position(current_pos);
        view.set_profile_velocity(0);
    }


    /// Writes out the scaled homing speed into the bus.
    fn command_homing_speed(&self, view: &mut impl AxisView) {
        let cpu = self.config.counts_per_user();
        let vel = (self.config.homing_speed * cpu).round() as u32;
        let accel = (self.config.homing_accel * cpu).round() as u32;
        let decel = (self.config.homing_decel * cpu).round() as u32;
        view.set_profile_velocity(vel);
        view.set_profile_acceleration(accel);
        view.set_profile_deceleration(decel);        
    }

    // ── Software homing state machine ──
    //
    // Phase 1: SEARCH (steps 0-3)
    //   Relative move in search direction until sensor triggers.
    //
    // Phase 2: HALT (steps 4-6)
    //   Stop the motor, cancel the old target.
    //
    // Phase 3: BACK-OFF (steps 7-11)
    //   Move opposite direction until sensor clears, then stop.
    //
    // Phase 4: SET HOME (steps 12-18)
    //   Write home offset to drive via SDO, trigger CurrentPosition homing,
    //   send hold set-point, complete.
    //
    fn tick_soft_homing(&mut self, view: &mut impl AxisView, client: &mut CommandClient, step: u8) {        
        match HomeState::from_repr(step) {

            Some(HomeState::EnsurePpMode) => {
                //
                // If the drive crapped out in a previous mode, it might still be in homing mode.
                // Make sure we're in Profile Position mode.
                //
                log::info!("SoftHome: Ensuring PP mode..");
                self.fb_mode_of_operation.start(ModesOfOperation::ProfilePosition as i8);
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                self.op = AxisOp::SoftHoming(HomeState::WaitPpMode as u8);
            },
            Some(HomeState::WaitPpMode) => {

                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!("Software homing SDO error writing homing mode of operation: {} {}", 
                            self.fb_mode_of_operation.error_code(), self.fb_mode_of_operation.error_message()
                        ));
                    }
                    else {
                        log::info!("SoftHome: Drive is in PP mode!");

                        // If sensor is NOT triggered, search for it (issue a move).
                        // If sensor IS already triggered, skip search and go straight
                        // to the found-sensor halt/back-off sequence.
                        if !self.check_soft_home_trigger(view) {
                            log::info!("SoftHome: Not on home switch; seek out.");
                            self.op = AxisOp::SoftHoming(HomeState::Search as u8);
                        } else {
                            log::info!("SoftHome: Already on home switch, skipping ahead to back-off stage.");
                            self.op = AxisOp::SoftHoming(HomeState::WaitFoundSensor as u8);
                        }
                    }
                }


            },

            // ── Phase 1: SEARCH ──
            Some(HomeState::Search) => {
                view.set_modes_of_operation(ModesOfOperation::ProfilePosition.as_i8());

                // // Absolute move to a far-away position in the search direction.
                // // Use raw counts directly to avoid overflow with invert_direction.                
                // let far_counts = (self.soft_home_direction * 999_999.0 * cpu).round() as i32;
                // let target = if self.config.invert_direction { -far_counts } else { far_counts };
                // let target = target + view.position_actual(); // offset from current


                // move in a relative direction as far as possible
                // we will stop when we reach the switch
                let target = self.calculate_max_relative_target(self.soft_home_direction);
                view.set_target_position(target);

                // let cpu = self.config.counts_per_user();
                // let vel = (self.config.homing_speed * cpu).round() as u32;
                // let accel = (self.config.homing_accel * cpu).round() as u32;
                // let decel = (self.config.homing_decel * cpu).round() as u32;
                // view.set_profile_velocity(vel);
                // view.set_profile_acceleration(accel);
                // view.set_profile_deceleration(decel);

                self.command_homing_speed(view);

                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, true);  // new set-point
                cw.set_bit(6, true); // sets true for relative move
                cw.set_bit(8, false); // clear halt
                cw.set_bit(13, true); // move relative to the actual current motor position
                view.set_control_word(cw.raw());

                log::info!("SoftHome[0]: SEARCH relative target={} vel={} dir={} pos={}",
                    target, self.config.homing_speed, self.soft_home_direction, view.position_actual());
                self.op = AxisOp::SoftHoming(HomeState::WaitSearching as u8);
            }
            Some(HomeState::WaitSearching) => {
                if self.check_soft_home_trigger(view) {
                    log::debug!("SoftHome[1]: sensor triggered during ack wait");
                    self.op = AxisOp::SoftHoming(HomeState::WaitFoundSensor as u8);
                    return;
                }
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) != 0 {
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);
                    view.set_control_word(cw.raw());
                    log::debug!("SoftHome[1]: set-point ack received, clearing bit 4");
                    self.op = AxisOp::SoftHoming(HomeState::WaitFoundSensor as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: set-point not acknowledged");
                }
            }
            // Some(HomeState::WaitSensor) => {
            //     if self.check_soft_home_trigger(view) {
            //         log::debug!("SoftHome[2]: sensor triggered during transition");
            //         self.op = AxisOp::SoftHoming(4);
            //         return;
            //     }
            //     log::debug!("SoftHome[2]: transition → monitoring");
            //     self.op = AxisOp::SoftHoming(3);
            // }
            Some(HomeState::WaitFoundSensor) => {
                if self.check_soft_home_trigger(view) {
                    log::info!("SoftHome[3]: sensor triggered at pos={}. HALTING", view.position_actual());
                    log::info!("ControlWord is : {} ", view.control_word());

                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(8, true);  // halt
                    cw.set_bit(4, false);  // reset new setpoint bit
                    view.set_control_word(cw.raw());


                    self.halt_stable_count = 0;
                    self.op = AxisOp::SoftHoming(HomeState::WaitStoppedFoundSensor as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: sensor not detected");
                }
            }


            Some(HomeState::WaitStoppedFoundSensor) => {
                const STABLE_WINDOW: i32 = 1;
                const STABLE_TICKS_REQUIRED: u8 = 10;

                // let mut cw = RawControlWord(view.control_word());
                // cw.set_bit(8, true);
                // view.set_control_word(cw.raw());

                let pos = view.position_actual();
                if (pos - self.last_raw_position).abs() <= STABLE_WINDOW {
                    self.halt_stable_count = self.halt_stable_count.saturating_add(1);
                } else {
                    self.halt_stable_count = 0;
                }

                if self.halt_stable_count >= STABLE_TICKS_REQUIRED {

                    log::info!("SoftHome[5] motor is stopped. Cancel move and wait for bit 12 go true.");
                    self.command_cancel_move(view);
                    self.op = AxisOp::SoftHoming(HomeState::WaitFoundSensorAck as u8);

                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: motor did not stop after sensor trigger");
                }
            }
            Some(HomeState::WaitFoundSensorAck) => {
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) != 0 &&  sw.raw() & (1 << 10) != 0 {

                    log::info!("SoftHome[6]: relative move cancel ack received. Waiting before back-off...");

                    // reset bit 4 so we're clear for the next move
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);  // reset new setpoint bit
                    cw.set_bit(5, true); // single setpoint -- flush out any previous
                    view.set_control_word(cw.raw());

                    self.op = AxisOp::SoftHoming(HomeState::WaitFoundSensorAckClear as u8);

                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: cancel not acknowledged");
                }
            },
            Some(HomeState::WaitFoundSensorAckClear) => {
                let sw = RawStatusWord(view.status_word());
                // CRITICAL: Wait for the drive to acknowledge that the setpoint is gone
                if sw.raw() & (1 << 12) == 0 { 

                    // turn off halt and it still shouldn't move
                    self.command_clear_halt(view);

                    log::info!("SoftHome[6]: Handshake cleared (Bit 12 is LOW). Proceeding to delay.");
                    self.op = AxisOp::SoftHoming(HomeState::DebounceFoundSensor as u8);
                    self.ton.call(false, Duration::from_secs(3));
                }                   
            },
            // Delay before back-off (60 = wait ~1 second for drive to settle)
            Some(HomeState::DebounceFoundSensor) => {
                self.ton.call(true, Duration::from_secs(3));

                let sw = RawStatusWord(view.status_word());
                if self.ton.q && sw.raw() & (1 << 12) == 0 { 
                    self.ton.call(false, Duration::from_secs(3));
                    log::info!("SoftHome[6.a.]: delay complete, starting back-off from pos={} cw=0x{:04X} sw={:04x}",
                    view.position_actual(), view.control_word(), view.status_word());
                    self.op = AxisOp::SoftHoming(HomeState::BackOff as u8);
                }
            }

            // ── Phase 3: BACK-OFF until sensor clears ──
            Some(HomeState::BackOff) => {

                let target = (self.calculate_max_relative_target(-self.soft_home_direction)) / 2;
                view.set_target_position(target);


                self.command_homing_speed(view);            

                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, true);  // new set-point                
                cw.set_bit(6, true); // relative move                
                cw.set_bit(13, true); // relative from current, actualy position
                view.set_control_word(cw.raw());
                log::info!("SoftHome[7]: BACK-OFF absolute target={} vel={} pos={} cw=0x{:04X}",
                    target, self.config.homing_speed, view.position_actual(), cw.raw());
                self.op = AxisOp::SoftHoming(HomeState::WaitBackingOff as u8);
            }
            Some(HomeState::WaitBackingOff) => {
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) != 0 {
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);
                    view.set_control_word(cw.raw());
                    log::info!("SoftHome[WaitBackingOff]: back-off ack received, pos={}", view.position_actual());
                    self.op = AxisOp::SoftHoming(HomeState::WaitLostSensor as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: back-off not acknowledged");
                }
            }
            Some(HomeState::WaitLostSensor) => {
                if !self.check_soft_home_trigger(view) {
                    log::info!("SoftHome[WaitLostSensor]: sensor lost at pos={}. Halting...", view.position_actual());

                    self.command_halt(view);
                    self.op = AxisOp::SoftHoming(HomeState::WaitStoppedLostSensor as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: sensor did not clear during back-off");
                }
            }
            Some(HomeState::WaitStoppedLostSensor)  => {
                const STABLE_WINDOW: i32 = 1;
                const STABLE_TICKS_REQUIRED: u8 = 10;

                // let mut cw = RawControlWord(view.control_word());
                // cw.set_bit(8, true);
                // view.set_control_word(cw.raw());

                let pos = view.position_actual();
                if (pos - self.last_raw_position).abs() <= STABLE_WINDOW {
                    self.halt_stable_count = self.halt_stable_count.saturating_add(1);
                } else {
                    self.halt_stable_count = 0;
                }

                if self.halt_stable_count >= STABLE_TICKS_REQUIRED {
                    log::info!("SoftHome[WaitStoppedLostSensor] motor is stopped. Cancel move and wait for bit 12 go true.");
                    self.command_cancel_move(view);
                    self.op = AxisOp::SoftHoming(HomeState::WaitLostSensorAck as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: motor did not stop after back-off");
                }
            }
            Some(HomeState::WaitLostSensorAck) => {
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) != 0 &&  sw.raw() & (1 << 10) != 0 {

                    log::info!("SoftHome[WaitLostSensorAck]: relative move cancel ack received. Waiting before back-off...");

                    // reset bit 4 so we're clear for the next move
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);  // reset new setpoint bit
                    view.set_control_word(cw.raw());

                     self.op = AxisOp::SoftHoming(HomeState::WaitLostSensorAckClear as u8);


                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: cancel not acknowledged");
                }
            }
            Some(HomeState::WaitLostSensorAckClear) => {
                // CRITICAL: Wait for the drive to acknowledge that the setpoint is gone
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) == 0 { 

                    self.command_clear_halt(view);

                    let desired_counts = self.config.to_counts(self.config.home_position).round() as i32;
                    // let current_pos = view.position_actual();
                    // let offset = desired_counts - current_pos;
                    self.homing_sdo_tid = self.sdo.write(
                        client, 0x607C, 0, json!(desired_counts),
                    );

                    log::info!("SoftHome[WaitLostSensorAckClear]: Handshake cleared (Bit 12 is LOW). Writing home offset {} [{} counts].",
                        self.config.home_position, desired_counts
                    );

                    self.op = AxisOp::SoftHoming(HomeState::WaitHomeOffsetDone as u8);

                }                
            },

            Some(HomeState::WaitHomeOffsetDone) => {
                // Wait for home offset SDO ack
                match self.sdo.result(client, self.homing_sdo_tid, Duration::from_secs(5)) {
                    SdoResult::Ok(_) => { self.op = AxisOp::SoftHoming(HomeState::WriteHomingModeOp as u8); }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Software homing timeout: home offset SDO write");
                        }
                    }
                    SdoResult::Err(e) => {
                        self.set_op_error(&format!("Software homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.set_op_error("Software homing: home offset SDO timed out");
                    }
                }
            },            
            Some(HomeState::WriteHomingModeOp) => {

                // Switch the mode of operation into Homing Mode so that we can execute
                // the homing command.

                self.fb_mode_of_operation.reset();
                self.fb_mode_of_operation.start(ModesOfOperation::Homing as i8);
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                self.op = AxisOp::SoftHoming(HomeState::WaitWriteHomingModeOp as u8);

                
            },       
            Some(HomeState::WaitWriteHomingModeOp) => {
                // Wait for method SDO ack
                self.fb_mode_of_operation.tick(client, &mut self.sdo);

                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!("Software homing SDO error writing homing mode of operation: {} {}", 
                            self.fb_mode_of_operation.error_code(), self.fb_mode_of_operation.error_message()
                        ));
                    }
                    else {
                        log::info!("SoftHome: Drive is now in Homing Mode.");
                        self.op = AxisOp::SoftHoming(HomeState::WriteHomingMethod as u8);
                    }
                }
            },
            Some(HomeState::WriteHomingMethod) => {
                // Write homing method = CurrentPosition (default 37, configurable
                // via AxisConfig::soft_home_method — Inovance SV660N needs 35).
                self.homing_sdo_tid = self.sdo.write(
                    client, 0x6098, 0, json!(self.config.soft_home_method),
                );
                self.op = AxisOp::SoftHoming(HomeState::WaitWriteHomingMethodDone as u8);
            }
            Some(HomeState::WaitWriteHomingMethodDone) => {
                // Wait for method SDO ack
                match self.sdo.result(client, self.homing_sdo_tid, Duration::from_secs(5)) {
                    SdoResult::Ok(_) => { 
                        log::info!("SoftHome: Successfully wrote homing method.");
                        self.op = AxisOp::SoftHoming(HomeState::ClearHomingTrigger as u8); 
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.restore_pp_after_error("Software homing timeout: homing method SDO write");
                        }
                    }
                    SdoResult::Err(e) => {
                        self.restore_pp_after_error(&format!("Software homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.restore_pp_after_error("Software homing: homing method SDO timed out");
                    }
                }
            }
            Some(HomeState::ClearHomingTrigger) => {
                // Switch to homing mode and ensure CW bit 4 starts LOW, so the
                // next state can issue a clean rising edge the drive will see.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                self.op = AxisOp::SoftHoming(HomeState::TriggerHoming as u8);
            }
            Some(HomeState::TriggerHoming) => {
                // Rising edge on CW bit 4 to start homing.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, true);
                view.set_control_word(cw.raw());
                log::info!("SoftHome[TriggerHoming]: start homing");
                self.op = AxisOp::SoftHoming(HomeState::WaitHomingStarted as u8);
            }
            Some(HomeState::WaitHomingStarted) => {
                // Wait for the drive to clear bit 12 (Homing attained) to acknowledge
                // the start of homing. Without this handshake, stale bit 12 carried
                // over from the previous mode (e.g. PP set-point acknowledge) would
                // cause WaitHomingDone to pass instantly, and the drive would never
                // actually perform the homing method.
                let sw = view.status_word();
                let error = sw & (1 << 13) != 0;
                if error {
                    self.restore_pp_after_error("Software homing: drive reported homing error");
                } else if sw & (1 << 12) == 0 {
                    self.op = AxisOp::SoftHoming(HomeState::WaitHomingDone as u8);
                } else if self.homing_timed_out() {
                    self.restore_pp_after_error(&format!("Software homing timeout: drive did not acknowledge homing start (sw=0x{:04X})", sw));
                }
            }
            Some(HomeState::WaitHomingDone) => {
                // Wait for homing complete (bit 12 attained + bit 10 reached).
                let sw = view.status_word();
                let error    = sw & (1 << 13) != 0;
                let attained = sw & (1 << 12) != 0;
                let reached  = sw & (1 << 10) != 0;

                if error {
                    self.restore_pp_after_error("Software homing: drive reported homing error");
                } else if attained && reached {
                    log::info!("SoftHome[WaitHomingDone]: homing complete (sw=0x{:04X})", sw);
                    self.op = AxisOp::SoftHoming(HomeState::ResetHomingTrigger as u8);
                } else if self.homing_timed_out() {
                    self.restore_pp_after_error(&format!("Software homing timeout: drive homing did not complete (sw=0x{:04X} attained={} reached={})", sw, attained, reached));
                }
            }
            Some(HomeState::ResetHomingTrigger) => {
                // Clear CW bit 4 first, in its own RxPDO cycle, so the drive sees
                // the falling edge *before* we change modes_of_operation away from
                // Homing. Changing both at once can leave the drive committing
                // ambiguous state.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                self.op = AxisOp::SoftHoming(HomeState::WaitHomingTriggerCleared as u8);
            }
            Some(HomeState::WaitHomingTriggerCleared) => {
                // One tick later, switch back to PP mode and record that the drive
                // now owns the offset so our software-side offset is zero.
                self.home_offset = 0; // drive handles it now
                self.op = AxisOp::SoftHoming(HomeState::WriteMotionModeOfOperation as u8);
            }


            Some(HomeState::WriteMotionModeOfOperation) => {

                // Switch back to PP motion mode

                self.fb_mode_of_operation.reset();
                self.fb_mode_of_operation.start(ModesOfOperation::ProfilePosition as i8);
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                self.op = AxisOp::SoftHoming(HomeState::WaitWriteMotionModeOfOperation  as u8);
                
            },       
            Some(HomeState::WaitWriteMotionModeOfOperation) => {
                // Wait for method SDO ack
                self.fb_mode_of_operation.tick(client, &mut self.sdo);

                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!("Software homing SDO error writing homing mode of operation: {} {}", 
                            self.fb_mode_of_operation.error_code(), self.fb_mode_of_operation.error_message()
                        ));
                    }
                    else {
                        if self.is_error {
                            log::error!("Drive back in PP mode after error. Homing sequence did not complete!");
                            self.finish_op_error();
                        }
                        else {
                            // Set the target position so this drive doesn't go wandering off after homing
                            // changed the position
                            self.op = AxisOp::SoftHoming(HomeState::SendCurrentPositionTarget as u8);
                        }
                        
                    }
                }
            },

            Some(HomeState::SendCurrentPositionTarget) => {
                // Hold position: send set-point to current position
                let current_pos = view.position_actual();
                view.set_target_position(current_pos);
                view.set_profile_velocity(0);
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, true);
                cw.set_bit(5, true);
                cw.set_bit(6, false); // absolute
                view.set_control_word(cw.raw());
                self.op = AxisOp::SoftHoming(HomeState::WaitCurrentPositionTargetSent as u8);
            }
            Some(HomeState::WaitCurrentPositionTargetSent) => {
                // Wait for hold ack
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) != 0 {
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);
                    view.set_control_word(cw.raw());
                    log::info!("Software homing complete — position set to {} user units",
                        self.config.home_position);
                    self.complete_op();
                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: hold position not acknowledged");
                }
            }
            _ => self.complete_op(),
        }
    }

    // ── Halting ──
    //
    // Three-stage close-out of the PP handshake, mirroring the soft-home
    // stop sequence (WaitStoppedFoundSensor → WaitFoundSensorAck →
    // WaitFoundSensorAckClear). Leaving any stage out results in a dirty
    // handshake that makes the next `move_absolute` time out at
    // "set-point not acknowledged."
    //
    // Step 0: (done in halt()) command_halt — bit 8 set, bit 4 cleared.
    // Step 1: wait for position to be stable → command_cancel_move.
    // Step 2: wait for SW bit 12 AND bit 10 → clear bit 4, set bit 5.
    // Step 3: wait for SW bit 12 to drop → Idle.
    fn tick_halting(&mut self, view: &mut impl AxisView, step: u8) {
        match HaltState::from_repr(step) {
            Some(HaltState::WaitStopped) => {
                // `update_outputs` writes `last_raw_position` at the end
                // of the previous tick, so this compares delta across
                // exactly one scan period.
                let pos = view.position_actual();
                let pos_stable = (pos - self.last_raw_position).abs() <= HALT_STABLE_WINDOW;

                let vel = view.velocity_actual().abs();
                let vel_stopped = vel <= HALT_STOPPED_VELOCITY;

                // Either signal is sufficient — position jitter during
                // servo hold can exceed the window even when the drive
                // reports ~0 velocity, and vice versa on slow drives
                // that report stale velocity briefly.
                if pos_stable || vel_stopped {
                    self.halt_stable_count = self.halt_stable_count.saturating_add(1);
                } else {
                    self.halt_stable_count = 0;
                }

                if self.halt_stable_count >= HALT_STABLE_TICKS_REQUIRED {
                    self.command_cancel_move(view);
                    self.op_started = Some(Instant::now());
                    self.op = AxisOp::Halting(HaltState::WaitCancelAck as u8);
                } else if self.op_stage_timed_out(HALT_STAGE_TIMEOUT) {
                    self.set_op_error("Halt timeout: motor did not stop");
                }
            }
            Some(HaltState::WaitCancelAck) => {
                let sw = RawStatusWord(view.status_word());
                let setpoint_ack   = sw.raw() & (1 << 12) != 0;
                // let target_reached = sw.raw() & (1 << 10) != 0;
                if setpoint_ack /* && target_reached */  {
                    // Reset the rising edge for the next move; bit 5 flushes
                    // any queued setpoint so we start clean.
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);
                    cw.set_bit(5, true);
                    view.set_control_word(cw.raw());
                    self.op_started = Some(Instant::now());
                    self.op = AxisOp::Halting(HaltState::WaitCancelAckClear as u8);
                } else if self.op_stage_timed_out(HALT_STAGE_TIMEOUT) {
                    self.set_op_error("Halt timeout: cancel not acknowledged");
                }
            }
            Some(HaltState::WaitCancelAckClear) => {
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) == 0 {
                    // setpoint_ack dropped — drive is ready for the next move.
                    self.command_clear_halt(view);                    
                    self.complete_op();
                } else if self.op_stage_timed_out(HALT_STAGE_TIMEOUT) {
                    self.set_op_error("Halt timeout: ack did not clear");
                }
            }
            None => {
                log::warn!("Axis halt: unknown sub-step {}, forcing idle", step);
                self.complete_op();
            }
        }
    }

    // ── Fault Recovery ──
    // Step 0: (done in reset_faults()) clear bit 7
    // Step 1: assert bit 7 (fault reset rising edge)
    // Step 2: wait fault cleared → Idle
    fn tick_fault_recovery(&mut self, view: &mut impl AxisView, step: u8) {
        match step {
            1 => {
                // Assert fault reset (rising edge on bit 7)
                let mut cw = RawControlWord(view.control_word());
                cw.cmd_fault_reset();
                view.set_control_word(cw.raw());
                self.op = AxisOp::FaultRecovery(2);
            }
            2 => {
                // Wait for fault to clear
                let sw = RawStatusWord(view.status_word());
                let state = sw.state();
                if !matches!(state, Cia402State::Fault | Cia402State::FaultReactionActive) {
                    log::info!("Fault cleared (drive state: {})", state);
                    self.complete_op();
                } else if self.op_timed_out() {
                    self.set_op_error("Fault reset timeout: drive still faulted");
                }
            }
            _ => self.complete_op(),
        }
    }
}

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

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

    /// Mock AxisView for testing.
    struct MockView {
        control_word: u16,
        status_word: u16,
        target_position: i32,
        profile_velocity: u32,
        profile_acceleration: u32,
        profile_deceleration: u32,
        modes_of_operation: i8,
        modes_of_operation_display: i8,
        position_actual: i32,
        velocity_actual: i32,
        error_code: u16,
        positive_limit: bool,
        negative_limit: bool,
        home_sensor: bool,
    }

    impl MockView {
        fn new() -> Self {
            Self {
                control_word: 0,
                status_word: 0x0040, // SwitchOnDisabled
                target_position: 0,
                profile_velocity: 0,
                profile_acceleration: 0,
                profile_deceleration: 0,
                modes_of_operation: 0,
                modes_of_operation_display: 1, // PP
                position_actual: 0,
                velocity_actual: 0,
                error_code: 0,
                positive_limit: false,
                negative_limit: false,
                home_sensor: false,
            }
        }

        fn set_state(&mut self, state: u16) {
            self.status_word = state;
        }
    }

    impl AxisView for MockView {
        fn control_word(&self) -> u16 { self.control_word }
        fn set_control_word(&mut self, word: u16) { self.control_word = word; }
        fn set_target_position(&mut self, pos: i32) { self.target_position = pos; }
        fn set_profile_velocity(&mut self, vel: u32) { self.profile_velocity = vel; }
        fn set_profile_acceleration(&mut self, accel: u32) { self.profile_acceleration = accel; }
        fn set_profile_deceleration(&mut self, decel: u32) { self.profile_deceleration = decel; }
        fn set_modes_of_operation(&mut self, mode: i8) { self.modes_of_operation = mode; }
        fn modes_of_operation_display(&self) -> i8 { self.modes_of_operation_display }
        fn status_word(&self) -> u16 { self.status_word }
        fn position_actual(&self) -> i32 { self.position_actual }
        fn velocity_actual(&self) -> i32 { self.velocity_actual }
        fn error_code(&self) -> u16 { self.error_code }
        fn positive_limit_active(&self) -> bool { self.positive_limit }
        fn negative_limit_active(&self) -> bool { self.negative_limit }
        fn home_sensor_active(&self) -> bool { self.home_sensor }
    }

    fn test_config() -> AxisConfig {
        AxisConfig::new(12_800).with_user_scale(360.0)
    }

    /// Helper: create axis + mock client channels.
    fn test_axis() -> (Axis, CommandClient, tokio::sync::mpsc::UnboundedSender<mechutil::ipc::CommandMessage>, tokio::sync::mpsc::UnboundedReceiver<String>) {
        use tokio::sync::mpsc;
        let (write_tx, write_rx) = mpsc::unbounded_channel();
        let (response_tx, response_rx) = mpsc::unbounded_channel();
        let client = CommandClient::new(write_tx, response_rx);
        let axis = Axis::new(test_config(), "TestDrive");
        (axis, client, response_tx, write_rx)
    }

    #[test]
    fn axis_config_conversion() {
        let cfg = test_config();
        // 45 degrees = 1600 counts
        assert!((cfg.to_counts(45.0) - 1600.0).abs() < 0.01);
    }

    #[test]
    fn enable_sequence_sets_pp_mode_and_shutdown() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();

        axis.enable(&mut view);

        // Should have set PP mode
        assert_eq!(view.modes_of_operation, ModesOfOperation::ProfilePosition.as_i8());
        // Should have issued shutdown command (bits 1,2 set; 0,3,7 clear)
        assert_eq!(view.control_word & 0x008F, 0x0006);
        // Should be in Enabling state
        assert_eq!(axis.op, AxisOp::Enabling(1));

        // Simulate drive reaching ReadyToSwitchOn
        view.set_state(0x0021); // ReadyToSwitchOn
        axis.tick(&mut view, &mut client);

        // Should have issued enable_operation (bits 0-3 set; 7 clear)
        assert_eq!(view.control_word & 0x008F, 0x000F);
        assert_eq!(axis.op, AxisOp::Enabling(2));

        // Simulate drive reaching OperationEnabled
        view.set_state(0x0027); // OperationEnabled
        axis.tick(&mut view, &mut client);

        // Should be idle now, motor_on = true
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(axis.motor_on);
    }

    #[test]
    fn move_absolute_sets_target() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027); // OperationEnabled
        axis.tick(&mut view, &mut client); // update outputs

        // Move to 45 degrees at 90 deg/s, 180 deg/s² accel/decel
        axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);

        // Target should be ~1600 counts (45° at 12800 cpr / 360°)
        assert_eq!(view.target_position, 1600);
        // Velocity: 90 deg/s * (12800/360) ≈ 3200 counts/s
        assert_eq!(view.profile_velocity, 3200);
        // Accel: 180 deg/s² * (12800/360) ≈ 6400 counts/s²
        assert_eq!(view.profile_acceleration, 6400);
        assert_eq!(view.profile_deceleration, 6400);
        // Bit 4 (new set-point) should be set
        assert!(view.control_word & (1 << 4) != 0);
        // Bit 6 (relative) should be clear for absolute move
        assert!(view.control_word & (1 << 6) == 0);
        // Should be in Moving state
        assert!(matches!(axis.op, AxisOp::Moving(MoveKind::Absolute, 1, _, _)));
    }

    #[test]
    fn move_relative_sets_relative_bit() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        axis.move_relative(&mut view, 10.0, 90.0, 180.0, 180.0);

        // Bit 6 (relative) should be set
        assert!(view.control_word & (1 << 6) != 0);
        assert!(matches!(axis.op, AxisOp::Moving(MoveKind::Relative, 1, _, _)));
    }

    #[test]
    fn move_completes_on_target_reached() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027); // OperationEnabled
        axis.tick(&mut view, &mut client);

        axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);

        // Step 1: simulate set-point acknowledge (bit 12)
        view.status_word = 0x1027; // OperationEnabled + bit 12
        axis.tick(&mut view, &mut client);
        // Should have cleared bit 4
        assert!(view.control_word & (1 << 4) == 0);

        // Step 2: simulate target reached (bit 10)
        view.status_word = 0x0427; // OperationEnabled + bit 10
        axis.tick(&mut view, &mut client);
        // Should be idle now
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(!axis.in_motion);
    }

    #[test]
    fn fault_detected_sets_error() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0008); // Fault
        view.error_code = 0x1234;

        axis.tick(&mut view, &mut client);

        assert!(axis.is_error);
        assert_eq!(axis.error_code, 0x1234);
        assert!(axis.error_message.contains("fault"));
    }

    #[test]
    fn fault_recovery_sequence() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0008); // Fault

        axis.reset_faults(&mut view);
        // Step 0: bit 7 should be cleared
        assert!(view.control_word & 0x0080 == 0);

        // Step 1: tick should assert bit 7
        axis.tick(&mut view, &mut client);
        assert!(view.control_word & 0x0080 != 0);

        // Step 2: simulate fault cleared → SwitchOnDisabled
        view.set_state(0x0040);
        axis.tick(&mut view, &mut client);
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(!axis.is_error);
    }

    #[test]
    fn reset_faults_software_only_is_synchronous() {
        // When the axis hits a software error (e.g. target outside the
        // software limits) the drive stays in OperationEnabled — there is
        // no CiA 402 fault to clear. reset_faults() must return the axis
        // to Idle on the same call without engaging the drive's bit-7
        // fault-reset handshake.
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027); // OperationEnabled
        axis.tick(&mut view, &mut client);

        // Trigger a software-only error via target outside software limit.
        axis.set_software_min_limit(207.0);
        axis.move_absolute(&mut view, 200.0, 90.0, 180.0, 180.0);
        assert!(axis.is_error, "move past software limit must set is_error");
        assert!(!axis.is_busy);

        // Capture cw before reset. reset_faults must not modify it for
        // a software-only error (drive is healthy, no handshake needed).
        let cw_before = view.control_word;

        axis.reset_faults(&mut view);

        // Synchronous: error cleared, axis already Idle, not busy.
        assert!(!axis.is_error);
        assert_eq!(axis.error_code, 0);
        assert!(axis.error_message.is_empty());
        assert!(!axis.is_busy);
        assert_eq!(axis.op, AxisOp::Idle);
        // No drive handshake — control word untouched.
        assert_eq!(view.control_word, cw_before);
    }

    #[test]
    fn disable_sequence() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027); // OperationEnabled

        axis.disable(&mut view);
        // Should have sent disable_operation command
        assert_eq!(view.control_word & 0x008F, 0x0007);

        // Simulate drive leaving OperationEnabled
        view.set_state(0x0023); // SwitchedOn
        axis.tick(&mut view, &mut client);
        assert_eq!(axis.op, AxisOp::Idle);
    }

    #[test]
    fn position_tracks_with_home_offset() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        view.position_actual = 5000;

        // Enable to capture home offset
        axis.enable(&mut view);
        view.set_state(0x0021);
        axis.tick(&mut view, &mut client);
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        // Home offset should be 5000
        assert_eq!(axis.home_offset, 5000);

        // Position should be 0 (at home)
        assert!((axis.position - 0.0).abs() < 0.01);

        // Move actual position to 5000 + 1600 = 6600
        view.position_actual = 6600;
        axis.tick(&mut view, &mut client);

        // Should read as 45 degrees
        assert!((axis.position - 45.0).abs() < 0.1);
    }

    #[test]
    fn set_position_adjusts_home_offset() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.position_actual = 3200;

        axis.set_position(&view, 90.0);
        axis.tick(&mut view, &mut client);

        // home_offset = 3200 - to_counts(90.0) = 3200 - 3200 = 0
        assert_eq!(axis.home_offset, 0);
        assert!((axis.position - 90.0).abs() < 0.01);
    }

    #[test]
    fn halt_runs_multi_stage_close_out() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);

        axis.halt(&mut view);

        // halt() sets CW bit 8 (halt) and clears CW bit 4 (new setpoint).
        assert!(view.control_word & (1 << 8) != 0, "halt bit must be set");
        assert!(view.control_word & (1 << 4) == 0, "new_setpoint must be cleared");

        // Halt is a multi-stage process. The first stage is WaitStopped.
        assert!(matches!(axis.op, AxisOp::Halting(_)),
                "halt should enter Halting state, not Idle");
        let AxisOp::Halting(step) = axis.op.clone() else { unreachable!() };
        assert_eq!(step, HaltState::WaitStopped as u8);

        // Tick alone does NOT immediately return to Idle anymore —
        // WaitStopped polls position stability (5 consecutive ticks
        // with position stable or velocity near zero). With a static
        // MockView, position stays at 0 and velocity stays at 0, so
        // vel_stopped is true every tick → the counter accumulates
        // and the state progresses after HALT_STABLE_TICKS_REQUIRED.
        for _ in 0..HALT_STABLE_TICKS_REQUIRED {
            axis.tick(&mut view, &mut client);
        }
        // After enough stable ticks we advance to WaitCancelAck.
        assert!(matches!(axis.op, AxisOp::Halting(_)));
        let AxisOp::Halting(step) = axis.op.clone() else { unreachable!() };
        assert_eq!(step, HaltState::WaitCancelAck as u8,
                   "should advance past WaitStopped once position/velocity is stable");

        // axis.is_busy must remain true through the whole halt sequence so
        // callers like MoveToLoad don't see "stopped" until the PP state
        // is fully cleaned up.
        assert!(axis.is_busy, "is_busy must stay true across Halting stages");
    }

    #[test]
    fn is_busy_tracks_operations() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();

        // Idle — not busy
        axis.tick(&mut view, &mut client);
        assert!(!axis.is_busy);

        // Enable — busy
        axis.enable(&mut view);
        axis.tick(&mut view, &mut client);
        assert!(axis.is_busy);

        // Complete enable
        view.set_state(0x0021);
        axis.tick(&mut view, &mut client);
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);
        assert!(!axis.is_busy);

        // Move — busy
        axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);
        axis.tick(&mut view, &mut client);
        assert!(axis.is_busy);
        assert!(axis.in_motion);
    }

    #[test]
    fn fault_during_move_cancels_op() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027); // OperationEnabled
        axis.tick(&mut view, &mut client);

        // Start a move
        axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);
        axis.tick(&mut view, &mut client);
        assert!(axis.is_busy);
        assert!(!axis.is_error);

        // Fault occurs mid-move
        view.set_state(0x0008); // Fault
        axis.tick(&mut view, &mut client);

        // is_busy should be false, is_error should be true
        assert!(!axis.is_busy);
        assert!(axis.is_error);
        assert_eq!(axis.op, AxisOp::Idle);
    }

    #[test]
    fn move_absolute_rejected_by_max_limit() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        axis.set_software_max_limit(90.0);
        axis.move_absolute(&mut view, 100.0, 90.0, 180.0, 180.0);

        // Should not have started a move — error instead
        assert!(axis.is_error);
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(axis.error_message.contains("max software limit"));
    }

    #[test]
    fn move_absolute_rejected_by_min_limit() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        axis.set_software_min_limit(-10.0);
        axis.move_absolute(&mut view, -20.0, 90.0, 180.0, 180.0);

        assert!(axis.is_error);
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(axis.error_message.contains("min software limit"));
    }

    #[test]
    fn move_relative_rejected_by_max_limit() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        // Position is 0, max limit 50 — relative move of +60 should be rejected
        axis.set_software_max_limit(50.0);
        axis.move_relative(&mut view, 60.0, 90.0, 180.0, 180.0);

        assert!(axis.is_error);
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(axis.error_message.contains("max software limit"));
    }

    #[test]
    fn move_within_limits_allowed() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        axis.set_software_max_limit(90.0);
        axis.set_software_min_limit(-90.0);
        axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);

        // Should have started normally
        assert!(!axis.is_error);
        assert!(matches!(axis.op, AxisOp::Moving(MoveKind::Absolute, 1, _, _)));
    }

    #[test]
    fn runtime_limit_halts_move_in_violated_direction() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        axis.set_software_max_limit(45.0);
        // Start a move to exactly the limit (allowed)
        axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);

        // Simulate the drive overshooting past 45° (position_actual in counts)
        // home_offset is 0, so 1650 counts = 46.4°
        view.position_actual = 1650;
        view.velocity_actual = 100; // moving positive

        // Simulate set-point ack so we're in Moving step 2
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client);

        // Should have halted cleanly without error. The axis enters the
        // Halting close-out (caller waits for !is_busy then checks
        // at_max_limit).
        assert!(!axis.is_error);
        assert!(axis.at_max_limit);
        assert!(axis.is_busy);
        assert!(matches!(axis.op, AxisOp::Halting(_)));
        // Halt bit (bit 8) should be set
        assert!(view.control_word & (1 << 8) != 0);
    }

    #[test]
    fn runtime_limit_allows_move_in_opposite_direction() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        // Start at 50° (past max limit)
        view.position_actual = 1778; // ~50°
        axis.set_software_max_limit(45.0);
        axis.tick(&mut view, &mut client);
        assert!(axis.at_max_limit);

        // Move back toward 0 — should be allowed even though at max limit
        axis.move_absolute(&mut view, 0.0, 90.0, 180.0, 180.0);
        assert!(!axis.is_error);
        assert!(matches!(axis.op, AxisOp::Moving(MoveKind::Absolute, 1, _, _)));

        // Simulate moving negative — limit check should not halt
        view.velocity_actual = -100;
        view.status_word = 0x1027; // ack
        axis.tick(&mut view, &mut client);
        // Still moving, no error from limit
        assert!(!axis.is_error);
    }

    #[test]
    fn positive_limit_switch_halts_positive_move() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        // Start a move in the positive direction
        axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);
        view.velocity_actual = 100; // moving positive
        // Simulate set-point ack so we're in Moving step 2
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        view.status_word = 0x0027;

        // Now the positive limit switch trips
        view.positive_limit = true;
        axis.tick(&mut view, &mut client);

        // Limit hit during a regular move enters the Halting close-out:
        // no error, halt bit asserted, drive's queued setpoint will be
        // canceled and halt cleared before the axis returns to Idle.
        // Caller waits for !is_busy then inspects at_positive_limit_switch.
        assert!(!axis.is_error);
        assert!(axis.at_positive_limit_switch);
        assert!(axis.is_busy);
        assert!(matches!(axis.op, AxisOp::Halting(_)));
        // Halt bit should be set
        assert!(view.control_word & (1 << 8) != 0);
    }

    #[test]
    fn negative_limit_switch_halts_negative_move() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        axis.tick(&mut view, &mut client);

        // Start a move in the negative direction
        axis.move_absolute(&mut view, -45.0, 90.0, 180.0, 180.0);
        view.velocity_actual = -100; // moving negative
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        view.status_word = 0x0027;

        // Negative limit switch trips
        view.negative_limit = true;
        axis.tick(&mut view, &mut client);

        // Limit hit during a regular move enters the Halting close-out:
        // no error, halt bit asserted, drive's queued setpoint will be
        // canceled and halt cleared before the axis returns to Idle.
        // Caller waits for !is_busy then inspects at_negative_limit_switch.
        assert!(!axis.is_error);
        assert!(axis.at_negative_limit_switch);
        assert!(axis.is_busy);
        assert!(matches!(axis.op, AxisOp::Halting(_)));
        assert!(view.control_word & (1 << 8) != 0);
    }

    #[test]
    fn limit_halt_cleanup_then_next_move_starts_cleanly() {
        // After a limit-switch halt, the axis must run the full Halting
        // close-out (motor stop → cancel queued setpoint → clear halt) so
        // the drive ends in a clean state. The next move then starts via
        // the normal one-tick start_move path.
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027); // OperationEnabled
        axis.tick(&mut view, &mut client);

        // Drive an initial move into a positive limit.
        axis.move_absolute(&mut view, 45.0, 90.0, 180.0, 180.0);
        view.velocity_actual = 100;
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        view.status_word = 0x0027;
        view.positive_limit = true;
        axis.tick(&mut view, &mut client);

        // Limit halt: in Halting close-out, halt asserted, busy, no error.
        assert!(!axis.is_error);
        assert!(axis.is_busy);
        assert!(matches!(axis.op, AxisOp::Halting(_)));
        assert!(view.control_word & (1 << 8) != 0);

        // Drive the close-out to completion. WaitStopped polls position
        // stability; with static MockView (vel=0 after we clear it), it
        // accumulates HALT_STABLE_TICKS_REQUIRED and advances to
        // WaitCancelAck.
        view.velocity_actual = 0;
        view.positive_limit = false;
        for _ in 0..HALT_STABLE_TICKS_REQUIRED {
            axis.tick(&mut view, &mut client);
        }
        assert!(matches!(axis.op, AxisOp::Halting(_)));
        let AxisOp::Halting(step) = axis.op.clone() else { unreachable!() };
        assert_eq!(step, HaltState::WaitCancelAck as u8);

        // WaitCancelAck: drive acks the cancel. We mock by setting bit 12.
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        let AxisOp::Halting(step) = axis.op.clone() else { unreachable!() };
        assert_eq!(step, HaltState::WaitCancelAckClear as u8);

        // WaitCancelAckClear: drive drops bit 12. Halting completes,
        // halt bit cleared, op back to Idle.
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client);
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(!axis.is_busy);
        assert!(view.control_word & (1 << 8) == 0, "halt should be cleared after close-out");

        // Now a fresh move should start via the normal one-tick path —
        // bit 4 asserted immediately, op at step 1.
        axis.move_absolute(&mut view, 30.0, 90.0, 180.0, 180.0);
        assert!(view.control_word & (1 << 4) != 0, "new-setpoint asserted on start_move tick");
        assert!(view.control_word & (1 << 8) == 0, "halt stays cleared");
        assert!(matches!(axis.op, AxisOp::Moving(MoveKind::Absolute, 1, _, _)));
    }

    #[test]
    fn limit_switch_allows_move_in_opposite_direction() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        // Positive limit is active, but we're moving negative (retreating)
        view.positive_limit = true;
        view.velocity_actual = -100;
        axis.tick(&mut view, &mut client);
        assert!(axis.at_positive_limit_switch);

        // Move in the negative direction should be allowed
        axis.move_absolute(&mut view, -10.0, 90.0, 180.0, 180.0);
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);

        // Should still be moving, no error
        assert!(!axis.is_error);
        assert!(matches!(axis.op, AxisOp::Moving(_, _, _, _)));
    }

    #[test]
    fn limit_switch_ignored_when_not_moving() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        view.positive_limit = true;

        axis.tick(&mut view, &mut client);

        // Output flag is set, but no error since we're not moving
        assert!(axis.at_positive_limit_switch);
        assert!(!axis.is_error);
    }

    #[test]
    fn home_sensor_output_tracks_view() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);

        axis.tick(&mut view, &mut client);
        assert!(!axis.home_sensor);

        view.home_sensor = true;
        axis.tick(&mut view, &mut client);
        assert!(axis.home_sensor);

        view.home_sensor = false;
        axis.tick(&mut view, &mut client);
        assert!(!axis.home_sensor);
    }

    #[test]
    fn velocity_output_converted() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);
        // 3200 counts/s = 90 deg/s
        view.velocity_actual = 3200;

        axis.tick(&mut view, &mut client);

        assert!((axis.speed - 90.0).abs() < 0.1);
        assert!(axis.moving_positive);
        assert!(!axis.moving_negative);
    }

    // ── Software homing tests ──

    fn soft_homing_config() -> AxisConfig {
        let mut cfg = AxisConfig::new(12_800).with_user_scale(360.0);
        cfg.homing_speed = 10.0;
        cfg.homing_accel = 20.0;
        cfg.homing_decel = 20.0;
        cfg
    }

    fn soft_homing_axis() -> (Axis, CommandClient, tokio::sync::mpsc::UnboundedSender<mechutil::ipc::CommandMessage>, tokio::sync::mpsc::UnboundedReceiver<String>) {
        use tokio::sync::mpsc;
        let (write_tx, write_rx) = mpsc::unbounded_channel();
        let (response_tx, response_rx) = mpsc::unbounded_channel();
        let client = CommandClient::new(write_tx, response_rx);
        let axis = Axis::new(soft_homing_config(), "TestDrive");
        (axis, client, response_tx, write_rx)
    }

    /// Helper: enable the axis and put it in OperationEnabled state.
    fn enable_axis(axis: &mut Axis, view: &mut MockView, client: &mut CommandClient) {
        view.set_state(0x0027); // OperationEnabled
        axis.tick(view, client);
    }

    /// Helper: drive the soft homing state machine through phases 2-4
    /// (halt, back-off, set home). Call after sensor triggers (step 4).
    /// `trigger_pos`: position where sensor triggered
    /// `clear_sensor`: closure to deactivate the sensor on the view
    fn complete_soft_homing(
        axis: &mut Axis,
        view: &mut MockView,
        client: &mut CommandClient,
        resp_tx: &tokio::sync::mpsc::UnboundedSender<mechutil::ipc::CommandMessage>,
        trigger_pos: i32,
        clear_sensor: impl FnOnce(&mut MockView),
    ) {
        use mechutil::ipc::CommandMessage as IpcMsg;

        // Phase 2: HALT (steps 4-6)
        // Step 4: halt
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(5)));

        // Step 5: motor decelerating then stopped
        view.position_actual = trigger_pos + 100;
        axis.tick(view, client);
        view.position_actual = trigger_pos + 120;
        axis.tick(view, client);
        // 10 stable ticks
        for _ in 0..10 { axis.tick(view, client); }
        assert!(matches!(axis.op, AxisOp::SoftHoming(6)));

        // Step 6: cancel ack → delay step 60
        view.status_word = 0x1027;
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(60)));
        view.status_word = 0x0027;

        // Step 60: delay (~1 second = 100 ticks)
        for _ in 0..100 { axis.tick(view, client); }
        assert!(matches!(axis.op, AxisOp::SoftHoming(7)));

        // Phase 3: BACK-OFF (steps 7-11)
        // Step 7: start back-off move
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(8)));

        // Step 8: ack
        view.status_word = 0x1027;
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(9)));
        view.status_word = 0x0027;

        // Step 9: sensor still active, then clears
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(9)));
        clear_sensor(view);
        view.position_actual = trigger_pos - 200;
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(10)));

        // Step 10-11: halt after back-off, wait stable
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(11)));
        for _ in 0..10 { axis.tick(view, client); }
        assert!(matches!(axis.op, AxisOp::SoftHoming(12)));

        // Phase 4: SET HOME (steps 12-19)
        // Step 12: cancel ack + SDO write home offset
        view.status_word = 0x1027;
        axis.tick(view, client);
        view.status_word = 0x0027;
        assert!(matches!(axis.op, AxisOp::SoftHoming(13)));

        // Step 13: SDO ack for home offset
        let tid = axis.homing_sdo_tid;
        resp_tx.send(IpcMsg::response(tid, json!(null))).unwrap();
        client.poll();
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(14)));

        // Step 14→15: SDO write homing method, ack
        axis.tick(view, client);
        let tid = axis.homing_sdo_tid;
        resp_tx.send(IpcMsg::response(tid, json!(null))).unwrap();
        client.poll();
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(16)));

        // Step 16: switch to homing mode + trigger
        view.modes_of_operation_display = ModesOfOperation::Homing.as_i8();
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(17)));

        // Step 17: homing complete (attained + reached)
        view.status_word = 0x1427; // bit 12 + bit 10
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(18)));
        view.modes_of_operation_display = ModesOfOperation::ProfilePosition.as_i8();
        view.status_word = 0x0027;

        // Step 18: hold position
        axis.tick(view, client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(19)));

        // Step 19: hold ack
        view.status_word = 0x1027;
        axis.tick(view, client);
        view.status_word = 0x0027;

        assert_eq!(axis.op, AxisOp::Idle);
        assert!(!axis.is_busy);
        assert!(!axis.is_error);
        assert_eq!(axis.home_offset, 0); // drive handles it now
    }

    #[test]
    fn soft_homing_pnp_home_sensor_full_sequence() {
        let (mut axis, mut client, resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        axis.home(&mut view, HomingMethod::HomeSensorPosPnp);

        // Phase 1: search
        axis.tick(&mut view, &mut client); // step 0→1
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client); // step 1→2 (ack)
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client); // step 2→3

        // Sensor triggers
        view.home_sensor = true;
        view.position_actual = 5000;
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(4)));

        complete_soft_homing(&mut axis, &mut view, &mut client, &resp_tx, 5000,
            |v| { v.home_sensor = false; });
    }

    #[test]
    fn soft_homing_npn_home_sensor_full_sequence() {
        let (mut axis, mut client, resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        // NPN: sensor reads true normally, false when detected
        view.home_sensor = true;
        enable_axis(&mut axis, &mut view, &mut client);

        axis.home(&mut view, HomingMethod::HomeSensorPosNpn);

        // Phase 1: search
        axis.tick(&mut view, &mut client);
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client);

        // NPN: sensor goes false = detected
        view.home_sensor = false;
        view.position_actual = 3000;
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(4)));

        complete_soft_homing(&mut axis, &mut view, &mut client, &resp_tx, 3000,
            |v| { v.home_sensor = true; }); // NPN: back to true = cleared
    }

    #[test]
    fn soft_homing_limit_switch_suppresses_halt() {
        let (mut axis, mut client, _resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        // Software homing on positive limit switch (rising edge)
        axis.home(&mut view, HomingMethod::LimitSwitchPosPnp);

        // Progress through initial steps
        axis.tick(&mut view, &mut client); // step 0 → 1
        view.status_word = 0x1027; // ack
        axis.tick(&mut view, &mut client); // step 1 → 2
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client); // step 2 → 3

        // Positive limit switch trips — should NOT halt (suppressed)
        view.positive_limit = true;
        view.velocity_actual = 100; // moving positive
        view.position_actual = 8000;
        axis.tick(&mut view, &mut client);

        // Should have detected rising edge → step 4, NOT an error halt
        assert!(matches!(axis.op, AxisOp::SoftHoming(4)));
        assert!(!axis.is_error);
    }

    #[test]
    fn soft_homing_opposite_limit_still_protects() {
        let (mut axis, mut client, _resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        // Software homing on home sensor (positive direction)
        axis.home(&mut view, HomingMethod::HomeSensorPosPnp);

        // Progress through initial steps
        axis.tick(&mut view, &mut client); // step 0 → 1
        view.status_word = 0x1027; // ack
        axis.tick(&mut view, &mut client); // step 1 → 2
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client); // step 2 → 3

        // Negative limit switch trips while searching positive (shouldn't happen
        // in practice, but tests protection)
        view.negative_limit = true;
        view.velocity_actual = -100; // moving negative
        axis.tick(&mut view, &mut client);

        // Should have halted with error (negative limit protects)
        assert!(axis.is_error);
        assert!(axis.error_message.contains("Negative limit switch"));
    }

    #[test]
    // fn soft_homing_sensor_already_active_rejects() {
    //     let (mut axis, mut client, _resp_tx, _write_rx) = soft_homing_axis();
    //     let mut view = MockView::new();
    //     enable_axis(&mut axis, &mut view, &mut client);

    //     // Home sensor is already active (rising edge would never happen)
    //     view.home_sensor = true;
    //     axis.tick(&mut view, &mut client); // update prev state

    //     axis.home(&mut view, HomingMethod::HomeSensorPosPnp);

    //     // Should have been rejected immediately
    //     assert!(axis.is_error);
    //     assert!(axis.error_message.contains("already in trigger state"));
    //     assert_eq!(axis.op, AxisOp::Idle);
    // }

    #[test]
    fn soft_homing_negative_direction_sets_negative_target() {
        let (mut axis, mut client, _resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        axis.home(&mut view, HomingMethod::HomeSensorNegPnp);
        axis.tick(&mut view, &mut client); // step 0

        // Target should be negative (large negative value in counts)
        assert!(view.target_position < 0);
    }

    #[test]
    fn home_integrated_method_starts_hardware_homing() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        axis.home(&mut view, HomingMethod::CurrentPosition);
        assert!(matches!(axis.op, AxisOp::Homing(0)));
        assert_eq!(axis.homing_method, 37);
    }

    #[test]
    fn home_integrated_arbitrary_code() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        axis.home(&mut view, HomingMethod::Integrated(35));
        assert!(matches!(axis.op, AxisOp::Homing(0)));
        assert_eq!(axis.homing_method, 35);
    }

    #[test]
    fn hardware_homing_skips_speed_sdos_when_zero() {
        use mechutil::ipc::CommandMessage;

        let (mut axis, mut client, resp_tx, mut write_rx) = test_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        // Config has homing_speed = 0 and homing_accel = 0 (defaults)
        axis.home(&mut view, HomingMethod::Integrated(37));

        // Step 0: writes homing method SDO
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::Homing(1)));

        // Drain the SDO write message
        let _ = write_rx.try_recv();

        // Simulate SDO ack — need to use the correct tid from the sdo write
        let tid = axis.homing_sdo_tid;
        resp_tx.send(CommandMessage::response(tid, serde_json::json!(null))).unwrap();
        client.poll();
        axis.tick(&mut view, &mut client);

        // Should have skipped to step 8 (set homing mode)
        assert!(matches!(axis.op, AxisOp::Homing(8)));
    }

    #[test]
    fn hardware_homing_writes_speed_sdos_when_nonzero() {
        use mechutil::ipc::CommandMessage;

        let (mut axis, mut client, resp_tx, mut write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        // Config has homing_speed = 10.0, homing_accel = 20.0
        axis.home(&mut view, HomingMethod::Integrated(37));

        // Step 0: writes homing method SDO
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::Homing(1)));
        let _ = write_rx.try_recv();

        // SDO ack for homing method
        let tid = axis.homing_sdo_tid;
        resp_tx.send(CommandMessage::response(tid, serde_json::json!(null))).unwrap();
        client.poll();
        axis.tick(&mut view, &mut client);
        // Should go to step 2 (write speed SDO), not skip to 8
        assert!(matches!(axis.op, AxisOp::Homing(2)));
    }

    #[test]
    fn soft_homing_edge_during_ack_step() {
        let (mut axis, mut client, _resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        axis.home(&mut view, HomingMethod::HomeSensorPosPnp);
        axis.tick(&mut view, &mut client); // step 0 → 1

        // Sensor rises during step 1 (before ack)
        view.home_sensor = true;
        view.position_actual = 2000;
        axis.tick(&mut view, &mut client);

        // Should jump straight to step 4 (edge detected)
        assert!(matches!(axis.op, AxisOp::SoftHoming(4)));
    }

    #[test]
    fn soft_homing_applies_home_position() {
        let mut cfg = soft_homing_config();
        cfg.home_position = 90.0;

        use tokio::sync::mpsc;
        let (write_tx, _write_rx) = mpsc::unbounded_channel();
        let (resp_tx, response_rx) = mpsc::unbounded_channel();
        let mut client = CommandClient::new(write_tx, response_rx);
        let mut axis = Axis::new(cfg, "TestDrive");

        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        axis.home(&mut view, HomingMethod::HomeSensorPosPnp);

        // Search phase
        axis.tick(&mut view, &mut client);
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client);

        // Sensor triggers
        view.home_sensor = true;
        view.position_actual = 5000;
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(4)));

        // Complete full sequence (halt, back-off, set home via drive)
        complete_soft_homing(&mut axis, &mut view, &mut client, &resp_tx, 5000,
            |v| { v.home_sensor = false; });

        // After completion, home_offset = 0 (drive handles it)
        assert_eq!(axis.home_offset, 0);
    }

    #[test]
    fn soft_homing_default_home_position_zero() {
        let (mut axis, mut client, resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        enable_axis(&mut axis, &mut view, &mut client);

        axis.home(&mut view, HomingMethod::HomeSensorPosPnp);

        // Search phase
        axis.tick(&mut view, &mut client);
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client);

        // Sensor triggers
        view.home_sensor = true;
        view.position_actual = 5000;
        axis.tick(&mut view, &mut client);

        complete_soft_homing(&mut axis, &mut view, &mut client, &resp_tx, 5000,
            |v| { v.home_sensor = false; });

        assert_eq!(axis.home_offset, 0);
    }
}