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 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),
}

#[repr(u8)]
#[derive(Debug, Clone, PartialEq, FromRepr)]
enum HomeState {
    Search = 0,
    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 = 200,
    WriteHomingMethod = 205,
    WaitWriteHomingMethodDone = 210,
    ExecHomingMode = 215,
    WaitHomingDone = 220,
    SendCurrentPositionTarget = 225,
    WaitCurrentPositionTargetSent = 230

}

#[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,

    // ── 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()
        }
    }

    /// 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 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)
    }

    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, client, 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.move_start_timed_out() {
                    self.set_op_error("Move timeout: set-point not acknowledged");
                }
            }
            2 => {
                // 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:  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.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 => {
                // 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.homing_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.homing_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::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) {
        // 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) {
            self.op = AxisOp::SoftHoming(HomeState::Search as u8);
        } else {
            self.op = AxisOp::SoftHoming(HomeState::WaitFoundSensor 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
    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
        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.
    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
        cw.set_bit(8, true); // 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) {
            // ── 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::debug!("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
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(8, false); 
                    view.set_control_word(cw.raw());

                    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::debug!("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::debug!("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 { 

                    // turn off halt and it still shouldn't move                    
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(8, false);   
                    view.set_control_word(cw.raw());


                    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 {}.",
                        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::WriteHomingMethod 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::WriteHomingMethod) => {
                // Write homing method = 37 (CurrentPosition)
                self.homing_sdo_tid = self.sdo.write(
                    client, 0x6098, 0, json!(37i8),
                );
                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(_) => { self.op = AxisOp::SoftHoming(HomeState::ExecHomingMode as u8); }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Software homing timeout: homing method SDO write");
                        }
                    }
                    SdoResult::Err(e) => {
                        self.set_op_error(&format!("Software homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.set_op_error("Software homing: homing method SDO timed out");
                    }
                }
            }
            Some(HomeState::ExecHomingMode) => {
                // Switch to homing mode and trigger
                view.set_modes_of_operation(ModesOfOperation::Homing.as_i8());
                let display = view.modes_of_operation_display();
                if display == ModesOfOperation::Homing.as_i8() {
                    log::info!("SoftHome[ExecHomingMode]: homing mode confirmed, triggering homing");
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, true); // trigger homing
                    view.set_control_word(cw.raw());
                    self.op = AxisOp::SoftHoming(HomeState::WaitHomingDone as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error(&format!("Software homing timeout: homing mode not confirmed (display={})", display));
                }
            }
            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.set_op_error("Software homing: drive reported homing error");
                } else if attained && reached {
                    log::info!("SoftHome[WaitHomingDone]: homing complete (sw=0x{:04X}), switching to PP", sw);
                    // Homing complete — clear trigger, switch to PP
                    let mut cw = RawControlWord(view.control_word());
                    cw.set_bit(4, false);
                    view.set_control_word(cw.raw());
                    view.set_modes_of_operation(ModesOfOperation::ProfilePosition.as_i8());
                    self.home_offset = 0; // drive handles it now
                    self.op = AxisOp::SoftHoming(HomeState::SendCurrentPositionTarget as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error(&format!("Software homing timeout: drive homing did not complete (sw=0x{:04X} attained={} reached={})", sw, attained, reached));
                }
            }
            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 ──
    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);
    }

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