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 crate::command_client::CommandClient;
use crate::ethercat::{SdoClient, SdoResult};
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),
    Homing(u8),
    SoftHoming(u8),
    Halting,
    FaultRecovery(u8),
}

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

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

#[derive(Debug, Clone, Copy, PartialEq)]
enum SoftHomeEdge {
    Rising,
    Falling,
}

// ──────────────────────────────────────────────
// 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,
    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_edge: SoftHomeEdge,
    soft_home_direction: f64,
    prev_positive_limit: bool,
    prev_negative_limit: bool,
    prev_home_sensor: bool,

    // ── 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,
}

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 {
        Self {
            config,
            sdo: SdoClient::new(device_name),
            op: AxisOp::Idle,
            home_offset: 0,
            last_raw_position: 0,
            op_started: None,
            op_timeout: Duration::from_secs(5),
            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_edge: SoftHomeEdge::Rising,
            soft_home_direction: 1.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,
        }
    }

    /// 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) {
            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;

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

    /// 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,
    ) {
        if let Some(msg) = self.check_target_limit(self.position + distance) {
            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;

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

    fn start_move(
        &mut self,
        view: &mut impl AxisView,
        raw_target: i32,
        raw_vel: u32,
        raw_accel: u32,
        raw_decel: u32,
        kind: MoveKind,
    ) {
        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)
        let mut cw = RawControlWord(view.control_word());
        cw.set_bit(6, kind == MoveKind::Relative);
        cw.set_bit(4, true); // new set-point
        view.set_control_word(cw.raw());

        self.op = AxisOp::Moving(kind, 1);
        self.op_started = Some(Instant::now());
    }

    /// Halt the current move (decelerate to stop).
    pub fn halt(&mut self, view: &mut impl AxisView) {
        let mut cw = RawControlWord(view.control_word());
        cw.set_bit(8, true); // halt
        view.set_control_word(cw.raw());
        self.op = AxisOp::Halting;
    }

    // ═══════════════════════════════════════════
    // 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();
        view.set_control_word(cw.raw());

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

    /// Start a fault reset sequence.
    ///
    /// Clears bit 7, then asserts it (rising edge), then waits for fault to clear.
    pub fn reset_faults(&mut self, view: &mut impl AxisView) {
        // 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.is_error = false;
        self.error_code = 0;
        self.error_message.clear();
        self.op = AxisOp::FaultRecovery(1);
        self.op_started = Some(Instant::now());
    }

    /// 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 = 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 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();
        self.speed = self.config.to_user(vel.unsigned_abs() as f64).abs();
        self.moving_positive = vel > 0;
        self.moving_negative = vel < 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 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 complete_op(&mut self) {
        self.op = AxisOp::Idle;
        self.op_started = None;
    }

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

    /// 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.
    fn check_target_limit(&self, target: f64) -> Option<String> {
        if self.config.enable_max_position_limit && target > self.config.max_position_limit {
            Some(format!(
                "Target {:.3} exceeds max software limit {:.3}",
                target, self.config.max_position_limit
            ))
        } else if self.config.enable_min_position_limit && target < self.config.min_position_limit {
            Some(format!(
                "Target {:.3} exceeds min software limit {:.3}",
                target, self.config.min_position_limit
            ))
        } else {
            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 ──
        let sw_max = self.config.enable_max_position_limit
            && self.position >= self.config.max_position_limit;
        let sw_min = self.config.enable_min_position_limit
            && self.position <= self.config.min_position_limit;

        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 is_moving = matches!(self.op, AxisOp::Moving(_, _));
        let is_soft_homing = matches!(self.op, AxisOp::SoftHoming(_));

        if !is_moving && !is_soft_homing {
            return;
        }

        // 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) && self.moving_positive;
        let negative_blocked = (effective_sw_min || effective_hw_neg) && self.moving_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 && self.moving_positive {
                "Positive limit switch active".to_string()
            } else if effective_hw_neg && self.moving_negative {
                "Negative limit switch active".to_string()
            } else if effective_sw_max && self.moving_positive {
                format!(
                    "Software position limit: position {:.3} >= max {:.3}",
                    self.position, self.config.max_position_limit
                )
            } else {
                format!(
                    "Software position limit: position {:.3} <= min {:.3}",
                    self.position, self.config.min_position_limit
                )
            };
            self.set_op_error(&msg);
        }
    }

    // ═══════════════════════════════════════════
    // 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) => self.tick_moving(view, kind, step),
            AxisOp::Homing(step) => self.tick_homing(view, client, step),
            AxisOp::SoftHoming(step) => self.tick_soft_homing(view, step),
            AxisOp::Halting => self.tick_halting(view),
            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 {
                    self.home_offset = view.position_actual();
                    log::info!("Axis enabled — home captured at {}", self.home_offset);
                    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) {
        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);
                } else if self.op_timed_out() {
                    self.set_op_error("Move timeout: set-point not acknowledged");
                }
            }
            2 => {
                // Wait for target reached (bit 10)
                let sw = RawStatusWord(view.status_word());
                if sw.target_reached() {
                    self.complete_op();
                } else if self.op_timed_out() {
                    self.set_op_error("Move timeout: target not reached");
                }
            }
            _ => 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:  set homing mode
    // Step 9:  wait mode confirmed
    // Step 10: trigger homing (bit 4)
    // Step 11: wait homing complete (bits 10+12 set, 13 clear)
    // Step 12: capture home offset, switch to PP → 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).
    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.op_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.op_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.op_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.op_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 => {
                // Set homing mode
                view.set_modes_of_operation(ModesOfOperation::Homing.as_i8());
                self.op = AxisOp::Homing(9);
            }
            9 => {
                // Wait for mode confirmed
                if view.modes_of_operation_display() == ModesOfOperation::Homing.as_i8() {
                    self.op = AxisOp::Homing(10);
                } else if self.op_timed_out() {
                    self.set_op_error("Homing timeout: mode not confirmed");
                }
            }
            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 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 {
                    // Homing complete
                    self.op = AxisOp::Homing(12);
                } else if self.op_timed_out() {
                    self.set_op_error("Homing timeout: procedure did not complete");
                }
            }
            12 => {
                // 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
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                // Switch to PP mode
                view.set_modes_of_operation(ModesOfOperation::ProfilePosition.as_i8());
                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::LimitSwitchPosRt => {
                self.soft_home_sensor = SoftHomeSensor::PositiveLimit;
                self.soft_home_edge = SoftHomeEdge::Rising;
                self.soft_home_direction = 1.0;
            }
            HomingMethod::LimitSwitchNegRt => {
                self.soft_home_sensor = SoftHomeSensor::NegativeLimit;
                self.soft_home_edge = SoftHomeEdge::Rising;
                self.soft_home_direction = -1.0;
            }
            HomingMethod::LimitSwitchPosFt => {
                self.soft_home_sensor = SoftHomeSensor::PositiveLimit;
                self.soft_home_edge = SoftHomeEdge::Falling;
                self.soft_home_direction = 1.0;
            }
            HomingMethod::LimitSwitchNegFt => {
                self.soft_home_sensor = SoftHomeSensor::NegativeLimit;
                self.soft_home_edge = SoftHomeEdge::Falling;
                self.soft_home_direction = -1.0;
            }
            HomingMethod::HomeSensorPosRt => {
                self.soft_home_sensor = SoftHomeSensor::HomeSensor;
                self.soft_home_edge = SoftHomeEdge::Rising;
                self.soft_home_direction = 1.0;
            }
            HomingMethod::HomeSensorNegRt => {
                self.soft_home_sensor = SoftHomeSensor::HomeSensor;
                self.soft_home_edge = SoftHomeEdge::Rising;
                self.soft_home_direction = -1.0;
            }
            HomingMethod::HomeSensorPosFt => {
                self.soft_home_sensor = SoftHomeSensor::HomeSensor;
                self.soft_home_edge = SoftHomeEdge::Falling;
                self.soft_home_direction = 1.0;
            }
            HomingMethod::HomeSensorNegFt => {
                self.soft_home_sensor = SoftHomeSensor::HomeSensor;
                self.soft_home_edge = SoftHomeEdge::Falling;
                self.soft_home_direction = -1.0;
            }
            _ => {} // integrated methods handled elsewhere
        }
    }

    fn start_soft_homing(&mut self, view: &mut impl AxisView) {
        // Validate: sensor should not already be in trigger state
        let already_active = match (self.soft_home_sensor, self.soft_home_edge) {
            (SoftHomeSensor::PositiveLimit, SoftHomeEdge::Rising) => view.positive_limit_active(),
            (SoftHomeSensor::NegativeLimit, SoftHomeEdge::Rising) => view.negative_limit_active(),
            (SoftHomeSensor::HomeSensor, SoftHomeEdge::Rising) => view.home_sensor_active(),
            (SoftHomeSensor::PositiveLimit, SoftHomeEdge::Falling) => !view.positive_limit_active(),
            (SoftHomeSensor::NegativeLimit, SoftHomeEdge::Falling) => !view.negative_limit_active(),
            (SoftHomeSensor::HomeSensor, SoftHomeEdge::Falling) => !view.home_sensor_active(),
        };

        if already_active {
            self.set_op_error("Software homing: sensor already in trigger state");
            return;
        }

        self.op = AxisOp::SoftHoming(0);
        self.op_started = Some(Instant::now());
    }

    fn check_soft_home_edge(&self, view: &impl AxisView) -> bool {
        let (current, prev) = match self.soft_home_sensor {
            SoftHomeSensor::PositiveLimit => (view.positive_limit_active(), self.prev_positive_limit),
            SoftHomeSensor::NegativeLimit => (view.negative_limit_active(), self.prev_negative_limit),
            SoftHomeSensor::HomeSensor    => (view.home_sensor_active(), self.prev_home_sensor),
        };
        match self.soft_home_edge {
            SoftHomeEdge::Rising  => !prev && current,   // false → true
            SoftHomeEdge::Falling => prev && !current,    // true → false
        }
    }

    // ── Software homing state machine ──
    // Step 0: Set PP mode. Set target to large position in search direction.
    //         Set speed/accel/decel. Trigger (bit 4).
    // Step 1: Wait set-point ack (bit 12). Also check sensor edge.
    // Step 2: Clear set-point bit (bit 4). Also check sensor edge.
    // Step 3: Monitor: each tick, check sensor edge. On edge → step 4.
    // Step 4: Halt (set bit 8). Capture home_offset = position_actual().
    // Step 5: Wait halt acknowledged (target_reached bit 10). Clear halt.
    //         Switch to PP mode. Complete → Idle.
    fn tick_soft_homing(&mut self, view: &mut impl AxisView, step: u8) {
        match step {
            0 => {
                // Ensure PP mode
                view.set_modes_of_operation(ModesOfOperation::ProfilePosition.as_i8());

                // Set target to a large position in the search direction
                let target = (self.soft_home_direction * 999_999.0) as i32 + self.home_offset;
                view.set_target_position(target);

                // Set speed/accel/decel from config
                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);

                // Trigger new set-point (bit 4)
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, true);
                cw.set_bit(6, false); // absolute
                cw.set_bit(8, false); // clear halt
                view.set_control_word(cw.raw());

                self.op = AxisOp::SoftHoming(1);
            }
            1 => {
                // Wait for set-point ack (bit 12), also check edge
                if self.check_soft_home_edge(view) {
                    self.op = AxisOp::SoftHoming(4);
                    return;
                }
                let sw = RawStatusWord(view.status_word());
                if sw.raw() & (1 << 12) != 0 {
                    // Ack received: clear set-point bit
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);
                    view.set_control_word(cw.raw());
                    self.op = AxisOp::SoftHoming(2);
                } else if self.op_timed_out() {
                    self.set_op_error("Software homing timeout: set-point not acknowledged");
                }
            }
            2 => {
                // One tick after clearing bit 4, move to monitoring
                if self.check_soft_home_edge(view) {
                    self.op = AxisOp::SoftHoming(4);
                    return;
                }
                self.op = AxisOp::SoftHoming(3);
            }
            3 => {
                // Monitor for sensor edge
                if self.check_soft_home_edge(view) {
                    self.op = AxisOp::SoftHoming(4);
                } else if self.op_timed_out() {
                    self.set_op_error("Software homing timeout: sensor edge not detected");
                }
            }
            4 => {
                // Edge detected: halt and capture position, applying home_position
                // so the reference point reads as config.home_position in user units.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(8, true); // halt
                view.set_control_word(cw.raw());
                self.home_offset = view.position_actual()
                    - self.config.to_counts(self.config.home_position).round() as i32;
                log::info!("Software homing: edge detected, home offset: {}", self.home_offset);
                self.op = AxisOp::SoftHoming(5);
            }
            5 => {
                // Wait for halt acknowledged (target_reached bit 10)
                let sw = RawStatusWord(view.status_word());
                if sw.target_reached() {
                    // Clear halt bit, switch to PP
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(8, false);
                    view.set_control_word(cw.raw());
                    view.set_modes_of_operation(ModesOfOperation::ProfilePosition.as_i8());
                    log::info!("Software homing complete — home offset: {}", self.home_offset);
                    self.complete_op();
                } else if self.op_timed_out() {
                    self.set_op_error("Software homing timeout: halt not acknowledged");
                }
            }
            _ => self.complete_op(),
        }
    }

    // ── Halting ──
    fn tick_halting(&mut self, _view: &mut impl AxisView) {
        // Halt bit was already set in halt(). Go idle immediately —
        // the drive will decelerate on its own, reflected in velocity outputs.
        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 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_sets_bit_and_goes_idle() {
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027);

        axis.halt(&mut view);
        // Bit 8 should be set
        assert!(view.control_word & (1 << 8) != 0);

        // Tick should go idle
        axis.tick(&mut view, &mut client);
        assert_eq!(axis.op, AxisOp::Idle);
    }

    #[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 and set error
        assert!(axis.is_error);
        assert!(axis.at_max_limit);
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(axis.error_message.contains("Software position limit"));
        // 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);

        assert!(axis.is_error);
        assert!(axis.at_positive_limit_switch);
        assert!(!axis.is_busy);
        assert!(axis.error_message.contains("Positive limit switch"));
        // 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);

        assert!(axis.is_error);
        assert!(axis.at_negative_limit_switch);
        assert!(axis.error_message.contains("Negative limit switch"));
    }

    #[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);
    }

    #[test]
    fn soft_homing_rising_edge_home_sensor_triggers_home() {
        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);

        // Start software homing: positive direction, rising edge on home sensor
        axis.home(&mut view, HomingMethod::HomeSensorPosRt);
        assert!(matches!(axis.op, AxisOp::SoftHoming(0)));

        // Step 0 → 1: sets target, trigger
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(1)));
        // Bit 4 should be set (new set-point)
        assert!(view.control_word & (1 << 4) != 0);

        // Simulate set-point ack (bit 12)
        view.status_word = 0x1027;
        axis.tick(&mut view, &mut client);
        // Should have cleared bit 4 and moved to step 2
        assert!(view.control_word & (1 << 4) == 0);
        assert!(matches!(axis.op, AxisOp::SoftHoming(2)));

        // Step 2 → 3: monitoring
        view.status_word = 0x0027;
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(3)));

        // Step 3: no edge yet
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(3)));

        // Simulate home sensor rising edge (false → true)
        view.home_sensor = true;
        view.position_actual = 5000;
        axis.tick(&mut view, &mut client);
        // Should have detected edge → step 4 (halt + capture)
        assert!(matches!(axis.op, AxisOp::SoftHoming(4)));

        // Step 4 → 5: halt
        axis.tick(&mut view, &mut client);
        assert!(view.control_word & (1 << 8) != 0); // halt bit
        assert_eq!(axis.home_offset, 5000);
        assert!(matches!(axis.op, AxisOp::SoftHoming(5)));

        // Simulate target_reached (halt acknowledged)
        view.status_word = 0x0427; // bit 10 set
        axis.tick(&mut view, &mut client);
        // Should be idle now
        assert_eq!(axis.op, AxisOp::Idle);
        assert!(!axis.is_busy);
        assert!(!axis.is_error);
    }

    #[test]
    fn soft_homing_falling_edge_home_sensor_triggers_home() {
        let (mut axis, mut client, _resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        // Start with home sensor active
        view.home_sensor = true;
        enable_axis(&mut axis, &mut view, &mut client);

        // Falling edge: sensor goes true → false
        axis.home(&mut view, HomingMethod::HomeSensorPosFt);
        assert!(matches!(axis.op, AxisOp::SoftHoming(0)));

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

        // Step 3: sensor still active, no edge
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(3)));

        // Simulate falling edge (true → false)
        view.home_sensor = false;
        view.position_actual = 3000;
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::SoftHoming(4)));

        // Complete the halt
        axis.tick(&mut view, &mut client);
        view.status_word = 0x0427;
        axis.tick(&mut view, &mut client);
        assert_eq!(axis.op, AxisOp::Idle);
        assert_eq!(axis.home_offset, 3000);
    }

    #[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::LimitSwitchPosRt);

        // 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::HomeSensorPosRt);

        // 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::HomeSensorPosRt);

        // 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::HomeSensorNegRt);
        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::HomeSensorPosRt);
        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() {
        // Configure home_position = 90.0° so the sensor trigger point
        // reads as 90° instead of 0°.
        let mut cfg = soft_homing_config();
        cfg.home_position = 90.0;

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

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

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

        // Progress through steps 0-3
        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

        // Trigger edge at position 5000 counts
        view.home_sensor = true;
        view.position_actual = 5000;
        axis.tick(&mut view, &mut client); // step 3 → 4

        // Step 4: halt + capture
        axis.tick(&mut view, &mut client); // step 4 → 5

        // home_offset should be position_actual - to_counts(90.0)
        // = 5000 - 3200 = 1800
        assert_eq!(axis.home_offset, 1800);

        // Complete halt
        view.status_word = 0x0427;
        axis.tick(&mut view, &mut client);
        assert_eq!(axis.op, AxisOp::Idle);

        // Position at the trigger point should read as 90°
        // position = to_user(5000 - 1800) = to_user(3200) = 90.0°
        assert!((axis.position - 90.0).abs() < 0.1);
    }

    #[test]
    fn soft_homing_default_home_position_zero() {
        // With default home_position = 0, sensor trigger reads as 0°
        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::HomeSensorPosRt);

        // Progress through steps 0-3
        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);

        // Trigger edge at position 5000 counts
        view.home_sensor = true;
        view.position_actual = 5000;
        axis.tick(&mut view, &mut client); // edge detected

        // Step 4: halt + capture
        axis.tick(&mut view, &mut client);

        // home_offset should equal position_actual (5000 - 0 = 5000)
        assert_eq!(axis.home_offset, 5000);

        // Complete halt
        view.status_word = 0x0427;
        axis.tick(&mut view, &mut client);

        // Position should be 0° at the trigger point
        assert!((axis.position - 0.0).abs() < 0.01);
    }
}