autocore-std 3.3.66

//! 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::CurrentPosition35); // AKD; CurrentPosition37 elsewhere
//! ```

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

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

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

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

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

/// Sub-steps of `AxisOp::Halting`.
///
/// The set-point queue is flushed by **toggling the mode of operation** (PP →
/// Homing → PP) rather than by the CiA-402 set-point-acknowledge cancel
/// handshake. A mode change resets the drive's positioning function
/// deterministically, so there is no `SP_ACK` to coax out of the drive — which
/// removed three separate drive-specific failure modes on the Kollmorgen AKD
/// (won't-ack-while-halted, won't-ack-with-bit-5, won't-ack-zero-velocity).
/// This is the same mechanism (and `FbSetModeOfOperation`, with SDO
/// write + 0x6061 read-back) that homing already uses.
#[repr(u8)]
#[derive(Debug, Clone, PartialEq, FromRepr)]
enum HaltState {
    /// Halt bit set + new_setpoint cleared. Polling position for stability
    /// before flushing the queue.
    WaitStopped = 0,
    /// Flushing the set-point queue via the shared mode toggle ([`FbFlushQueue`]).
    /// Halt stays asserted until the flush completes; then it is released.
    Flushing = 10,
}

// ──────────────────────────────────────────────
// Shared set-point-queue flush
// ──────────────────────────────────────────────

/// Phase of a [`FbFlushQueue`] mode toggle.
#[derive(Debug, Clone, Copy, PartialEq)]
enum FlushPhase {
    Idle,
    /// Switching the mode of operation AWAY from PP (to Homing).
    Away,
    /// Switching the mode of operation BACK to PP.
    Back,
    Done,
    Error,
}

/// The **one** way the FB flushes a CiA-402 drive's set-point queue: toggle the
/// mode of operation PP → Homing → PP. A mode change resets the positioning
/// function deterministically, so there is no set-point-acknowledge handshake to
/// coax out of the drive (which removed three drive-specific AKD failure modes).
/// Shared by the halt close-out and both soft-homing stops so there is a single
/// queue-flush mechanism rather than several near-identical ones.
///
/// Drive with [`start`](Self::start) then [`tick`](Self::tick) until
/// `!is_busy()`, then check [`is_error`](Self::is_error). The mode switch goes
/// over SDO (write 0x6060 + read-back 0x6061 via [`FbSetModeOfOperation`]), the
/// same path homing uses — required by drives that pin mode by SDO (the AKD).
#[derive(Debug, Clone)]
struct FbFlushQueue {
    phase: FlushPhase,
    mode_fb: FbSetModeOfOperation,
}

impl FbFlushQueue {
    fn new() -> Self {
        Self { phase: FlushPhase::Idle, mode_fb: FbSetModeOfOperation::new() }
    }

    /// Begin the flush: switch the mode AWAY from PP (to Homing).
    fn start(&mut self, view: &mut impl AxisView, client: &mut CommandClient, sdo: &mut SdoClient) {
        self.begin_switch(view, client, sdo, ModesOfOperation::Homing);
        self.phase = FlushPhase::Away;
    }

    fn begin_switch(
        &mut self,
        view: &mut impl AxisView,
        client: &mut CommandClient,
        sdo: &mut SdoClient,
        mode: ModesOfOperation,
    ) {
        view.set_modes_of_operation(mode.as_i8());
        self.mode_fb.reset();
        self.mode_fb.start(mode.as_i8());
        self.mode_fb.tick(client, sdo);
    }

    fn tick(&mut self, view: &mut impl AxisView, client: &mut CommandClient, sdo: &mut SdoClient) {
        match self.phase {
            FlushPhase::Away => {
                self.mode_fb.tick(client, sdo);
                if !self.mode_fb.is_busy() {
                    if self.mode_fb.is_error() {
                        self.phase = FlushPhase::Error;
                    } else {
                        // Queue flushed; switch BACK to PP.
                        self.begin_switch(view, client, sdo, ModesOfOperation::ProfilePosition);
                        self.phase = FlushPhase::Back;
                    }
                }
            }
            FlushPhase::Back => {
                self.mode_fb.tick(client, sdo);
                if !self.mode_fb.is_busy() {
                    self.phase = if self.mode_fb.is_error() {
                        FlushPhase::Error
                    } else {
                        FlushPhase::Done
                    };
                }
            }
            _ => {}
        }
    }

    fn is_busy(&self) -> bool {
        matches!(self.phase, FlushPhase::Away | FlushPhase::Back)
    }
    fn is_error(&self) -> bool {
        self.phase == FlushPhase::Error
    }
    fn error_code(&self) -> i32 {
        self.mode_fb.error_code()
    }
    fn error_message(&self) -> String {
        self.mode_fb.error_message()
    }
}

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

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

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

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

#[repr(u8)]
#[derive(Debug, Clone, PartialEq, FromRepr)]
enum HomeState {
    EnsurePpMode = 0,
    WaitPpMode = 1,
    Search = 5,
    WaitSearching = 10,
    WaitFoundSensor = 20,
    WaitStoppedFoundSensor = 30,
    /// Flushing the set-point queue at the found-sensor stop (shared mode toggle).
    FlushingFound = 40,
    DebounceFoundSensor = 50,
    BackOff = 60,
    WaitBackingOff = 70,
    WaitLostSensor = 80,
    WaitStoppedLostSensor = 90,
    /// Flushing the set-point queue at the lost-sensor stop (shared mode toggle).
    FlushingLost = 100,
    /// Drive-side "set reference at present position" entry: write 0x607C =
    /// home_position, then run the drive's current-position homing method.
    WriteHomeOffset = 120,
    WaitHomeOffsetDone = 125,
    /// DIAGNOSTIC: read 0x607C straight back to confirm the drive stored it.
    WaitHomeOffsetReadback = 130,

    WriteHomingModeOp = 160,
    WaitWriteHomingModeOp = 165,

    WriteHomingMethod = 205,
    WaitWriteHomingMethodDone = 210,
    ClearHomingTrigger = 215,
    TriggerHoming = 217,
    WaitHomingStarted = 218,
    WaitHomingDone = 220,
    ResetHomingTrigger = 222,
    WaitHomingTriggerCleared = 223,
    /// After homing, before the PP-mode switch: align target (0x607A) with the
    /// just-homed actual (0x6064) and let one RxPDO cycle carry it to the drive,
    /// so PP re-entry has no demand/actual error to chase (the runaway).
    AlignTargetBeforePP = 225,
    WriteMotionModeOfOperation = 230,
    WaitWriteMotionModeOfOperation = 235,
    SendCurrentPositionTarget = 240,
    WaitCurrentPositionTargetSent = 245,
}

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

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

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

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

/// Stateful motion controller for a CiA 402 servo drive.
///
/// Manages the CiA 402 protocol (state machine, PP handshake, homing)
/// internally. Call [`tick()`](Self::tick) every control cycle to progress
/// operations and update output fields.
/// How many control-word/status-word *transitions* the per-axis trace keeps.
/// Recorded only on change, so this is an event history, not a per-tick log —
/// 48 transitions comfortably covers an enable → move → halt → cancel sequence.
const CW_SW_TRACE_CAP: usize = 48;

/// One entry in the CiA-402 transition trace: the op-state and the
/// control/status words at the moment one of them changed.
#[derive(Clone)]
struct TraceEntry {
    seq: u32,
    op: AxisOp,
    cw: u16,
    sw: u16,
}

/// Decode the PP-relevant control-word bits into a compact `A|B|C` string.
fn decode_cw(cw: u16) -> String {
    const BITS: [(u16, &str); 9] = [
        (0, "SwOn"),
        (1, "EnVolt"),
        (2, "QStop"),
        (3, "EnOp"),
        (4, "NewSP"),
        (5, "ChgSet"),
        (6, "Rel"),
        (7, "FltRst"),
        (8, "HALT"),
    ];
    let f: Vec<&str> = BITS
        .iter()
        .filter(|(b, _)| cw & (1 << b) != 0)
        .map(|(_, n)| *n)
        .collect();
    if f.is_empty() {
        "-".to_string()
    } else {
        f.join("|")
    }
}

/// Decode the status word: the CiA-402 state plus the PP handshake bits.
fn decode_sw(sw: u16) -> String {
    let mut f = vec![format!("{}", RawStatusWord(sw).state())];
    for (bit, name) in [
        (10, "TgtReached"),
        (11, "IntLim"),
        (12, "SP_ACK"),
        (13, "FollowErr"),
    ] {
        if sw & (1 << bit) != 0 {
            f.push(name.to_string());
        }
    }
    f.join("|")
}

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,

    // ── CiA-402 transition trace (debugging aid) ──
    cw_sw_trace: VecDeque<TraceEntry>,
    trace_seq: u32,

    fb_mode_of_operation: FbSetModeOfOperation,
    /// Shared set-point-queue flush (mode toggle) used by the halt close-out
    /// and both soft-homing stops.
    flush: FbFlushQueue,

    // ── 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,
            cw_sw_trace: VecDeque::new(),
            trace_seq: 0,
            is_error: false,
            error_code: 0,
            error_message: String::new(),
            motor_on: false,
            is_busy: false,
            in_motion: false,
            moving_positive: false,
            moving_negative: false,
            position: 0.0,
            raw_position: 0,
            speed: 0.0,
            at_max_limit: false,
            at_min_limit: false,
            at_positive_limit_switch: false,
            at_negative_limit_switch: false,
            home_sensor: false,
            ton: Ton::new(),
            fb_mode_of_operation: FbSetModeOfOperation::new(),
            flush: FbFlushQueue::new(),
        }
    }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    /// Start the disable sequence (OperationEnabled → ReadyToSwitchOn).
    ///
    /// Uses the CiA 402 Shutdown command (controlword 0x06) rather than
    /// Disable Operation (0x07). Per the spec, "Switched On" leaves motor
    /// energization vendor-defined — Teknic and Inovance happen to
    /// de-energize, Kollmorgen AKD keeps holding current. "Ready To
    /// Switch On" has the HV section *guaranteed* off, so torque drops
    /// on every compliant drive.
    pub fn disable(&mut self, view: &mut impl AxisView) {
        // Cancel any pending move BEFORE dropping to Ready-To-Switch-On: re-point
        // the profile target (0x607A) at the current actual position and clear the
        // new-setpoint request. On the AKD the setpoint-ack (SW bit 12) can survive
        // a Shutdown, so without this the drive finishes the *old* move when it is
        // re-enabled and returns to Operation Enabled — a dangerous uncommanded
        // resume of motion after an abort / e-stop / fault.
        let here = view.position_actual();
        view.set_target_position(here);
        self.pending_move_target = here;

        let mut cw = RawControlWord(view.control_word());
        cw.cmd_shutdown();
        cw.set_bit(4, false);
        cw.set_bit(8, false);
        cw.set_bit(7, false);
        cw.set_bit(2, true);
        view.set_control_word(cw.raw());

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

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

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

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

    /// Start a homing sequence with the given homing method.
    ///
    /// **Integrated** methods delegate to the drive's built-in CiA 402
    /// homing mode (SDO writes + homing trigger).
    ///
    /// **Software** methods are implemented by the Axis, which monitors
    /// [`AxisView`] sensor signals for edge triggers and captures home.
    pub fn home(&mut self, view: &mut impl AxisView, method: HomingMethod) {
        match method {
            // "Set reference at present position" (CiA 402 method 35 or 37).
            // Write the reference value to 0x607C and run the drive's
            // current-position homing, so the DRIVE owns the position — no
            // software offset. The method code comes straight from the variant
            // (35 = AKD, 37 = most others), replacing the former standalone
            // set_position_persistent and the soft_home_method indirection.
            HomingMethod::CurrentPosition35 | HomingMethod::CurrentPosition37 => {
                self.homing_method = method.cia402_code();
                self.op = AxisOp::SoftHoming(HomeState::WriteHomeOffset as u8);
                self.op_started = Some(Instant::now());
                let _ = view;
            }
            // Other drive-native (integrated) homing methods: delegate to the
            // drive's built-in CiA 402 homing mode. Starts at step 16, which
            // writes the home offset (0x607C) so the reference the drive finds
            // reads as home_position, before the normal sequence at step 0.
            m if m.is_integrated() => {
                self.homing_method = m.cia402_code();
                self.op = AxisOp::Homing(16);
                self.op_started = Some(Instant::now());
                let _ = view;
            }
            // Software search homing — the Axis monitors sensor/limit signals,
            // then sets the found position with the drive's configured
            // current-position method (soft_home_method).
            m => {
                self.homing_method = self.config.soft_home_method;
                self.configure_soft_homing(m);
                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);
        self.record_trace(view);
    }

    // ═══════════════════════════════════════════
    // Internal: CiA-402 transition trace (debug aid)
    // ═══════════════════════════════════════════

    /// Append to the trace if the op-state or the control/status words changed
    /// since the last entry. Change-driven, so a multi-second stall (e.g. a halt
    /// waiting on set-point-acknowledge) costs a single line, not one per tick.
    fn record_trace(&mut self, view: &impl AxisView) {
        let cw = view.control_word();
        let sw = view.status_word();
        let changed = self
            .cw_sw_trace
            .back()
            .map_or(true, |e| e.cw != cw || e.sw != sw || e.op != self.op);
        if !changed {
            return;
        }
        if self.cw_sw_trace.len() >= CW_SW_TRACE_CAP {
            self.cw_sw_trace.pop_front();
        }
        self.cw_sw_trace.push_back(TraceEntry {
            seq: self.trace_seq,
            op: self.op.clone(),
            cw,
            sw,
        });
        self.trace_seq = self.trace_seq.wrapping_add(1);
    }

    /// The recent CiA-402 control/status-word transitions, decoded into a
    /// human-readable block. A debugging aid for handshake problems such as a
    /// halt-cancel timeout: each line shows the op-state, the control word (with
    /// `HALT`, `NewSP`, `ChgSet`, … flags) and the status word (CiA-402 state
    /// plus `SP_ACK`, `TgtReached`, …). Because entries are recorded only on
    /// change, this is a compact event log rather than a per-tick PDO dump.
    ///
    /// It is also dumped automatically to the log whenever an operation errors
    /// (see the `Axis error:` log line), so failures arrive with the sequence
    /// that produced them.
    pub fn trace(&self) -> String {
        let mut out = String::new();
        for e in &self.cw_sw_trace {
            out.push_str(&format!(
                "  [{:>4}] {:<20} CW={:#06x} [{}]  SW={:#06x} [{}]\n",
                e.seq,
                format!("{:?}", e.op),
                e.cw,
                decode_cw(e.cw),
                e.sw,
                decode_sw(e.sw),
            ));
        }
        out
    }

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

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

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

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

        self.last_raw_position = raw;
    }

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

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

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

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

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

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

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

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

    fn set_op_error(&mut self, msg: &str) {
        self.is_error = true;
        self.error_message = msg.to_string();
        log::error!("Axis error: {}", msg);
        // Dump the CiA-402 handshake trace so failures (e.g. a halt-cancel
        // timeout) arrive with the control/status-word sequence that led to
        // them — far less data than logging every PDO every tick.
        if !self.cw_sw_trace.is_empty() {
            log::warn!(
                "Axis CiA-402 trace ({} recent transitions, oldest first):\n{}",
                self.cw_sw_trace.len(),
                self.trace(),
            );
        }
        self.op = AxisOp::Idle;
        self.op_started = None;
        self.is_busy = false;
        self.in_motion = false;
    }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    // ── Homing (hardware-delegated) ──
    // Step 16: write home offset SDO (0x607C) — ENTRY POINT, so the found
    //          reference reads home_position (rarely zero)
    // Step 17: wait SDO ack, then → step 0
    // Step 0:  write homing method SDO (0x6098:0)
    // Step 1:  wait SDO ack
    // Step 2:  write homing speed SDO (0x6099:1 — search for switch)
    // Step 3:  wait SDO ack
    // Step 4:  write homing speed SDO (0x6099:2 — search for zero)
    // Step 5:  wait SDO ack
    // Step 6:  write homing accel SDO (0x609A:0)
    // Step 7:  wait SDO ack
    // Step 8:  switch to Homing mode (PDO field + SDO write 0x6060)
    // Step 9:  wait for Homing mode written + verified via SDO read-back (0x6061)
    // Step 10: trigger homing (rising edge on CW bit 4)
    // Step 11: wait for drive to acknowledge start (bit 12 clears)
    // Step 12: wait homing complete (bits 10+12 set, 13 clear)
    // Step 13: drive owns position (home_offset = 0), clear CW bit 4
    // Step 14: switch back to PP mode (PDO field + SDO write 0x6060)
    // Step 15: wait for PP mode written + verified, then → Idle
    //
    // If homing_speed and homing_accel are both 0, steps 2-7 are skipped
    // (preserves backward compatibility for users who pre-configure via SDO).
    //
    // Mode is switched AND verified over SDO (via FbSetModeOfOperation) rather
    // than trusting modes_of_operation_display(): on drives that don't PDO-map
    // 0x6060/0x6061 (e.g. Kollmorgen AKD) the cyclic display field is a codegen
    // stub mirrored from the commanded value, so it would falsely confirm.
    fn tick_homing(&mut self, view: &mut impl AxisView, client: &mut CommandClient, step: u8) {
        match step {
            0 => {
                // Write homing method via SDO (0x6098:0)
                self.homing_sdo_tid = self.sdo.write(client, 0x6098, 0, json!(self.homing_method));
                self.op = AxisOp::Homing(1);
            }
            1 => {
                // Wait for SDO write ack
                match self
                    .sdo
                    .result(client, self.homing_sdo_tid, Duration::from_secs(5))
                {
                    SdoResult::Ok(_) => {
                        // Skip speed/accel SDOs if both are zero
                        if self.config.homing_speed == 0.0 && self.config.homing_accel == 0.0 {
                            self.op = AxisOp::Homing(8);
                        } else {
                            self.op = AxisOp::Homing(2);
                        }
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Homing timeout: SDO write for homing method");
                        }
                    }
                    SdoResult::Err(e) => {
                        self.set_op_error(&format!("Homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.set_op_error("Homing timeout: SDO write timed out");
                    }
                }
            }
            2 => {
                // Write homing speed (0x6099:1 — search for switch)
                let speed_counts = self.config.to_counts(self.config.homing_speed).round() as u32;
                self.homing_sdo_tid = self.sdo.write(client, 0x6099, 1, json!(speed_counts));
                self.op = AxisOp::Homing(3);
            }
            3 => {
                match self
                    .sdo
                    .result(client, self.homing_sdo_tid, Duration::from_secs(5))
                {
                    SdoResult::Ok(_) => {
                        self.op = AxisOp::Homing(4);
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error(
                                "Homing timeout: SDO write for homing speed (switch)",
                            );
                        }
                    }
                    SdoResult::Err(e) => {
                        self.set_op_error(&format!("Homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.set_op_error("Homing timeout: SDO write timed out");
                    }
                }
            }
            4 => {
                // Write homing speed (0x6099:2 — search for zero, same value)
                let speed_counts = self.config.to_counts(self.config.homing_speed).round() as u32;
                self.homing_sdo_tid = self.sdo.write(client, 0x6099, 2, json!(speed_counts));
                self.op = AxisOp::Homing(5);
            }
            5 => {
                match self
                    .sdo
                    .result(client, self.homing_sdo_tid, Duration::from_secs(5))
                {
                    SdoResult::Ok(_) => {
                        self.op = AxisOp::Homing(6);
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Homing timeout: SDO write for homing speed (zero)");
                        }
                    }
                    SdoResult::Err(e) => {
                        self.set_op_error(&format!("Homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.set_op_error("Homing timeout: SDO write timed out");
                    }
                }
            }
            6 => {
                // Write homing acceleration (0x609A:0)
                let accel_counts = self.config.to_counts(self.config.homing_accel).round() as u32;
                self.homing_sdo_tid = self.sdo.write(client, 0x609A, 0, json!(accel_counts));
                self.op = AxisOp::Homing(7);
            }
            7 => {
                match self
                    .sdo
                    .result(client, self.homing_sdo_tid, Duration::from_secs(5))
                {
                    SdoResult::Ok(_) => {
                        self.op = AxisOp::Homing(8);
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Homing timeout: SDO write for homing acceleration");
                        }
                    }
                    SdoResult::Err(e) => {
                        self.set_op_error(&format!("Homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.set_op_error("Homing timeout: SDO write timed out");
                    }
                }
            }
            8 => {
                // Switch into Homing mode on BOTH channels:
                //  - the cyclic PDO field, so drives that PDO-map 0x6060 carry the
                //    correct mode on the wire, and
                //  - an SDO write + read-back of 0x6060/0x6061 (FbSetModeOfOperation),
                //    so drives that pin mode by SDO (e.g. Kollmorgen AKD, which does
                //    not PDO-map mode) actually transition AND are verified.
                // The SDO read-back replaces the old modes_of_operation_display()
                // check, which silently passed on PDO-unmapped drives because codegen
                // mirrors the commanded value into the display stub.
                view.set_modes_of_operation(ModesOfOperation::Homing.as_i8());
                // Ensure CW bit 4 starts LOW so the next step issues a clean rising edge.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                self.fb_mode_of_operation.reset();
                self.fb_mode_of_operation
                    .start(ModesOfOperation::Homing as i8);
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                self.op = AxisOp::Homing(9);
            }
            9 => {
                // Wait for the mode switch to be written and verified over SDO.
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!(
                            "Homing: failed to enter Homing mode: {} {}",
                            self.fb_mode_of_operation.error_code(),
                            self.fb_mode_of_operation.error_message()
                        ));
                    } else {
                        self.op = AxisOp::Homing(10);
                    }
                }
            }
            10 => {
                // Trigger homing: rising edge on bit 4
                // DIAGNOSTIC: position_actual just before triggering, to compare
                // against the post-home value (step 13) — tells us whether the
                // drive's method actually changes 0x6064 or only sets the bit.
                log::info!(
                    "Homing[trigger]: method={} position_actual(0x6064) BEFORE = {}",
                    self.homing_method,
                    view.position_actual(),
                );
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, true);
                view.set_control_word(cw.raw());
                self.op = AxisOp::Homing(11);
            }
            11 => {
                // One drive cycle after the trigger edge, then evaluate completion.
                //
                // We deliberately do NOT wait for bit 12 (homing attained) to clear
                // first. That old start-gate assumed the drive de-asserts attained at
                // the start of a new run and re-asserts on completion — true for
                // methods that take time to run, but an instantaneous method (35/37,
                // set-current-position) completes within one cycle, so bit 12 goes
                // high immediately and is never observed low. The gate's original
                // purpose (rejecting a stale PP set-point acknowledge in bit 12) is
                // now covered by verifying mode 6 over SDO in steps 8–9 before we
                // ever trigger: once in Homing mode, bit 12 carries homing semantics.
                let sw = view.status_word();
                if sw & (1 << 13) != 0 {
                    self.set_op_error("Homing error: drive reported homing failure");
                } else {
                    self.op = AxisOp::Homing(12);
                }
            }
            12 => {
                // Wait for homing complete.
                //
                // Per CiA-402, bit 12 (homing attained) with bit 13 (error) clear is
                // the authoritative "homed" indicator. We do NOT require bit 10
                // (target reached): a set-current-position home reports "attained,
                // target not reached" (bit12=1, bit10=0) and never sets bit 10.
                // Methods that move to a target set both, so bit 12 alone still holds.
                let sw = view.status_word();
                let error = sw & (1 << 13) != 0;
                let attained = sw & (1 << 12) != 0;

                if error {
                    self.set_op_error("Homing error: drive reported homing failure");
                } else if attained {
                    self.op = AxisOp::Homing(13);
                } else if self.homing_timed_out() {
                    self.set_op_error(&format!(
                        "Homing timeout: procedure did not complete (sw=0x{:04X})",
                        sw
                    ));
                }
            }
            13 => {
                // The DRIVE owns position after homing (CiA 402 applies its home
                // offset 0x607C to the feedback at the found reference), so the
                // software offset stays ZERO — we never want the software axis to
                // be the source of truth for a real drive. Reported position is
                // then to_user(raw) directly.
                self.home_offset = 0;
                log::info!(
                    "Homing[done]: position_actual(0x6064) AFTER = {} (drive owns the reference)",
                    view.position_actual(),
                );
                // Clear homing start bit in its own cycle before switching modes
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                self.op = AxisOp::Homing(14);
            }
            14 => {
                // One tick later (bit 4 already cleared in step 13, so the drive
                // sees the falling edge before the mode change), switch back to PP
                // mode. Same dual-channel approach as the switch into Homing: cyclic
                // PDO field plus an SDO write + read-back so it lands and is verified
                // on drives that don't PDO-map mode.
                view.set_modes_of_operation(ModesOfOperation::ProfilePosition.as_i8());
                self.fb_mode_of_operation.reset();
                self.fb_mode_of_operation
                    .start(ModesOfOperation::ProfilePosition as i8);
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                self.op = AxisOp::Homing(15);
            }
            15 => {
                // Wait for the switch back to PP mode to be written and verified.
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!(
                            "Homing: failed to restore PP mode: {} {}",
                            self.fb_mode_of_operation.error_code(),
                            self.fb_mode_of_operation.error_message()
                        ));
                    } else {
                        log::info!("Homing complete — home offset: {}", self.home_offset);
                        self.complete_op();
                    }
                }
            }
            16 => {
                // Write the home offset (0x607C) so the reference the drive finds
                // reads as home_position — rarely zero. Runs first for every
                // integrated (drive-native) method, then falls through to the
                // normal method/speed/trigger sequence at step 0.
                let desired_counts =
                    self.config.to_counts(self.config.home_position).round() as i32;
                self.homing_sdo_tid = self.sdo.write(client, 0x607C, 0, json!(desired_counts));
                log::info!(
                    "Homing[offset]: writing 0x607C = {} counts ({} user units)",
                    desired_counts, self.config.home_position
                );
                self.op = AxisOp::Homing(17);
            }
            17 => {
                match self
                    .sdo
                    .result(client, self.homing_sdo_tid, Duration::from_secs(5))
                {
                    SdoResult::Ok(_) => {
                        self.op = AxisOp::Homing(0);
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.set_op_error("Homing timeout: SDO write for home offset");
                        }
                    }
                    SdoResult::Err(e) => {
                        self.set_op_error(&format!("Homing SDO error (home offset): {}", e));
                    }
                    SdoResult::Timeout => {
                        self.set_op_error("Homing timeout: home offset SDO write timed out");
                    }
                }
            }
            _ => self.complete_op(),
        }
    }

    // ── Software homing helpers ──

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

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

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

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

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

        return target;
    }

    /// Configure the command word for an immediate halt: set the halt bit and
    /// clear the new-setpoint bit. The set-point queue is flushed afterward by
    /// the shared mode toggle (`FbFlushQueue`), so no cancel set-point is needed.
    pub fn command_halt(&self, view: &mut impl AxisView) {
        let mut cw = RawControlWord(view.control_word());
        cw.set_bit(8, true); // halt
        cw.set_bit(4, false); // reset new setpoint bit
        cw.set_bit(5, false);
        cw.set_bit(6, false); // absolute move
        view.set_control_word(cw.raw());
    }

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

    // ── Software homing state machine ──
    //
    // Phase 1: SEARCH (steps 0-3)
    //   Relative move in search direction until sensor triggers.
    //
    // Phase 2: HALT (steps 4-6)
    //   Stop the motor, cancel the old target.
    //
    // Phase 3: BACK-OFF (steps 7-11)
    //   Move opposite direction until sensor clears, then stop.
    //
    // Phase 4: SET HOME (steps 12-18)
    //   Write home offset to drive via SDO, trigger CurrentPosition homing,
    //   send hold set-point, complete.
    //
    fn tick_soft_homing(&mut self, view: &mut impl AxisView, client: &mut CommandClient, step: u8) {
        match HomeState::from_repr(step) {
            Some(HomeState::EnsurePpMode) => {
                //
                // If the drive crapped out in a previous mode, it might still be in homing mode.
                // Make sure we're in Profile Position mode.
                //
                log::info!("SoftHome: Ensuring PP mode..");
                self.fb_mode_of_operation
                    .start(ModesOfOperation::ProfilePosition as i8);
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                self.op = AxisOp::SoftHoming(HomeState::WaitPpMode as u8);
            }
            Some(HomeState::WaitPpMode) => {
                self.fb_mode_of_operation.tick(client, &mut self.sdo);
                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!(
                            "Software homing SDO error writing homing mode of operation: {} {}",
                            self.fb_mode_of_operation.error_code(),
                            self.fb_mode_of_operation.error_message()
                        ));
                    } else {
                        log::info!("SoftHome: Drive is in PP mode!");

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

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

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

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

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

                self.command_homing_speed(view);

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

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

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

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

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

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

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

                if self.halt_stable_count >= STABLE_TICKS_REQUIRED {
                    log::info!(
                        "SoftHome[5] motor is stopped. Cancel move and wait for bit 12 go true."
                    );
                    self.flush.start(view, client, &mut self.sdo);
                    self.op = AxisOp::SoftHoming(HomeState::FlushingFound as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error(
                        "Software homing timeout: motor did not stop after sensor trigger",
                    );
                }
            }
            Some(HomeState::FlushingFound) => {
                self.flush.tick(view, client, &mut self.sdo);
                if self.flush.is_busy() {
                    if self.homing_timed_out() {
                        self.set_op_error("Software homing timeout: queue flush did not complete");
                    }
                    return;
                }
                if self.flush.is_error() {
                    self.set_op_error(&format!(
                        "Software homing: queue flush failed: {} {}",
                        self.flush.error_code(),
                        self.flush.error_message()
                    ));
                    return;
                }
                // Queue flushed and back in PP. Release halt + set-point bits, drop
                // stale target, then debounce before backing off.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(8, false);
                cw.set_bit(4, false);
                cw.set_bit(5, false);
                view.set_control_word(cw.raw());
                view.set_target_position(view.position_actual());
                log::info!("SoftHome[6]: queue flushed; proceeding to debounce.");
                self.op = AxisOp::SoftHoming(HomeState::DebounceFoundSensor as u8);
                self.ton.call(false, Duration::from_secs(3));
            }
            // Delay before back-off (60 = wait ~1 second for drive to settle)
            Some(HomeState::DebounceFoundSensor) => {
                self.ton.call(true, Duration::from_secs(3));

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

            // ── Phase 3: BACK-OFF until sensor clears ──
            Some(HomeState::BackOff) => {
                let target = (self.calculate_max_relative_target(-self.soft_home_direction)) / 2;
                view.set_target_position(target);

                self.command_homing_speed(view);

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

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

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

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

                if self.halt_stable_count >= STABLE_TICKS_REQUIRED {
                    log::info!("SoftHome[WaitStoppedLostSensor] motor is stopped. Cancel move and wait for bit 12 go true.");
                    self.flush.start(view, client, &mut self.sdo);
                    self.op = AxisOp::SoftHoming(HomeState::FlushingLost as u8);
                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: motor did not stop after back-off");
                }
            }
            Some(HomeState::FlushingLost) => {
                self.flush.tick(view, client, &mut self.sdo);
                if self.flush.is_busy() {
                    if self.homing_timed_out() {
                        self.set_op_error("Software homing timeout: queue flush did not complete");
                    }
                    return;
                }
                if self.flush.is_error() {
                    self.set_op_error(&format!(
                        "Software homing: queue flush failed: {} {}",
                        self.flush.error_code(),
                        self.flush.error_message()
                    ));
                    return;
                }
                // Queue flushed and back in PP. Release halt + set-point bits, then
                // write the home offset (0x607C) and verify.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(8, false);
                cw.set_bit(4, false);
                cw.set_bit(5, false);
                view.set_control_word(cw.raw());

                let desired_counts =
                    self.config.to_counts(self.config.home_position).round() as i32;
                self.homing_sdo_tid = self.sdo.write(client, 0x607C, 0, json!(desired_counts));
                log::info!(
                    "SoftHome[FlushingLost]: queue flushed; writing home offset {} [{} counts].",
                    self.config.home_position, desired_counts
                );
                self.op = AxisOp::SoftHoming(HomeState::WaitHomeOffsetDone as u8);
            }

            Some(HomeState::WriteHomeOffset) => {
                // Drive-side "set reference at present position" entry point (used
                // by home(CurrentPosition)). Write the reference value to the
                // drive's home offset (0x607C); the current-position homing method
                // (soft_home_method) then sets position_actual to it, so the DRIVE
                // owns the position — no software offset.
                let desired_counts =
                    self.config.to_counts(self.config.home_position).round() as i32;
                self.homing_sdo_tid = self.sdo.write(client, 0x607C, 0, json!(desired_counts));
                log::info!(
                    "Home[CurrentPosition]: writing 0x607C = {} counts ({} user units)",
                    desired_counts, self.config.home_position
                );
                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(_) => {
                        // DIAGNOSTIC: read 0x607C straight back so the log shows
                        // whether the drive actually stored the home offset we
                        // just wrote, vs. silently dropping it.
                        self.homing_sdo_tid = self.sdo.read(client, 0x607C, 0);
                        self.op = AxisOp::SoftHoming(HomeState::WaitHomeOffsetReadback 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::WaitHomeOffsetReadback) => {
                // DIAGNOSTIC ONLY — never fails the op; logs the value the drive
                // reports for 0x607C after our write, then continues.
                let target = self.config.to_counts(self.config.home_position).round() as i32;
                match self
                    .sdo
                    .result(client, self.homing_sdo_tid, Duration::from_secs(5))
                {
                    SdoResult::Ok(val) => {
                        log::info!(
                            "SoftHome[diag]: 0x607C (home offset) read-back = {} (wrote {} counts)",
                            val,
                            target,
                        );
                        self.op = AxisOp::SoftHoming(HomeState::WriteHomingModeOp as u8);
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            log::warn!("SoftHome[diag]: 0x607C read-back timed out; continuing");
                            self.op = AxisOp::SoftHoming(HomeState::WriteHomingModeOp as u8);
                        }
                    }
                    SdoResult::Err(e) => {
                        log::warn!("SoftHome[diag]: 0x607C read-back error: {}; continuing", e);
                        self.op = AxisOp::SoftHoming(HomeState::WriteHomingModeOp as u8);
                    }
                    SdoResult::Timeout => {
                        log::warn!("SoftHome[diag]: 0x607C read-back timed out; continuing");
                        self.op = AxisOp::SoftHoming(HomeState::WriteHomingModeOp as u8);
                    }
                }
            }
            Some(HomeState::WriteHomingModeOp) => {
                // Switch the mode of operation into Homing Mode so that we can execute
                // the homing command.

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

                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!(
                            "Software homing SDO error writing homing mode of operation: {} {}",
                            self.fb_mode_of_operation.error_code(),
                            self.fb_mode_of_operation.error_message()
                        ));
                    } else {
                        log::info!("SoftHome: Drive is now in Homing Mode.");
                        self.op = AxisOp::SoftHoming(HomeState::WriteHomingMethod as u8);
                    }
                }
            }
            Some(HomeState::WriteHomingMethod) => {
                // Write the CiA 402 homing method (0x6098). For current-position
                // homing this is the code carried by the HomingMethod variant
                // (35/37); for software search homing it is the drive's configured
                // current-position method (soft_home_method) — both are stashed in
                // self.homing_method by home().
                log::info!(
                    "SoftHome[diag]: writing 0x6098 (homing method) = {}",
                    self.homing_method
                );
                self.homing_sdo_tid =
                    self.sdo.write(client, 0x6098, 0, json!(self.homing_method));
                self.op = AxisOp::SoftHoming(HomeState::WaitWriteHomingMethodDone as u8);
            }
            Some(HomeState::WaitWriteHomingMethodDone) => {
                // Wait for method SDO ack
                match self
                    .sdo
                    .result(client, self.homing_sdo_tid, Duration::from_secs(5))
                {
                    SdoResult::Ok(_) => {
                        log::info!("SoftHome: Successfully wrote homing method.");
                        self.op = AxisOp::SoftHoming(HomeState::ClearHomingTrigger as u8);
                    }
                    SdoResult::Pending => {
                        if self.homing_timed_out() {
                            self.restore_pp_after_error(
                                "Software homing timeout: homing method SDO write",
                            );
                        }
                    }
                    SdoResult::Err(e) => {
                        self.restore_pp_after_error(&format!("Software homing SDO error: {}", e));
                    }
                    SdoResult::Timeout => {
                        self.restore_pp_after_error("Software homing: homing method SDO timed out");
                    }
                }
            }
            Some(HomeState::ClearHomingTrigger) => {
                // Switch to homing mode and ensure CW bit 4 starts LOW, so the
                // next state can issue a clean rising edge the drive will see.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                self.op = AxisOp::SoftHoming(HomeState::TriggerHoming as u8);
            }
            Some(HomeState::TriggerHoming) => {
                // Rising edge on CW bit 4 to start homing.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, true);
                view.set_control_word(cw.raw());
                log::info!("SoftHome[TriggerHoming]: start homing");
                self.op = AxisOp::SoftHoming(HomeState::WaitHomingStarted as u8);
            }
            Some(HomeState::WaitHomingStarted) => {
                // One drive cycle after the trigger edge, then evaluate completion.
                //
                // We deliberately do NOT wait for bit 12 (homing attained) to clear
                // first. That old start-gate assumed the drive de-asserts attained at
                // the start of a new run and re-asserts on completion — true for
                // methods that take time to run, but an instantaneous method (35/37,
                // set-current-position) completes within one cycle, so bit 12 goes
                // high immediately and may never be observed low, racing into a
                // spurious "did not acknowledge homing start" timeout. The gate's
                // original purpose (rejecting a stale bit 12 carried over from PP
                // set-point acknowledge) is already covered by the SDO mode-6
                // readback done earlier (FbSetModeOfOperation): once in Homing mode,
                // bit 12 carries homing semantics. This matches tick_homing step 11.
                let sw = view.status_word();
                if sw & (1 << 13) != 0 {
                    self.restore_pp_after_error("Software homing: drive reported homing error");
                } else {
                    self.op = AxisOp::SoftHoming(HomeState::WaitHomingDone as u8);
                }
            }
            Some(HomeState::WaitHomingDone) => {
                // Wait for homing complete. Per CiA-402, bit 12 (homing attained)
                // with bit 13 (error) clear is the authoritative "homed" indicator;
                // we do NOT require bit 10 (target reached). A set-current-position
                // home (method 35/37) reports "attained, target not reached"
                // (bit12=1, bit10=0) and never sets bit 10, so requiring it here
                // would hang until timeout (observed sw=0x1237). Methods that move
                // to a target set both, so bit 12 alone still holds. This matches
                // the integrated tick_homing completion check (step 12).
                let sw = view.status_word();
                let error = sw & (1 << 13) != 0;
                let attained = sw & (1 << 12) != 0;
                let reached = sw & (1 << 10) != 0;

                if error {
                    self.restore_pp_after_error("Software homing: drive reported homing error");
                } else if attained {
                    // DIAGNOSTIC: did the drive actually apply the set-position?
                    // position_actual (0x6064) should now read the value we wrote
                    // to 0x607C. If it instead reads a raw/unchanged number, the
                    // drive set the HOMED bit but did NOT move its reported
                    // position — meaning the persistence relies on software, not
                    // the drive.
                    log::info!(
                        "SoftHome[WaitHomingDone]: homing complete sw=0x{:04X} position_actual(0x6064)={} target(0x607C)={} counts",
                        sw,
                        view.position_actual(),
                        self.config.to_counts(self.config.home_position).round() as i32,
                    );
                    self.op = AxisOp::SoftHoming(HomeState::ResetHomingTrigger as u8);
                } else if self.homing_timed_out() {
                    self.restore_pp_after_error(&format!("Software homing timeout: drive homing did not complete (sw=0x{:04X} attained={} reached={})", sw, attained, reached));
                }
            }
            Some(HomeState::ResetHomingTrigger) => {
                // Clear CW bit 4 first, in its own RxPDO cycle, so the drive sees
                // the falling edge *before* we change modes_of_operation away from
                // Homing. Changing both at once can leave the drive committing
                // ambiguous state.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(4, false);
                view.set_control_word(cw.raw());
                self.op = AxisOp::SoftHoming(HomeState::WaitHomingTriggerCleared as u8);
            }
            Some(HomeState::WaitHomingTriggerCleared) => {
                // One tick after clearing the homing trigger, and BEFORE switching
                // back to PP mode: align the PP target (0x607A) with the just-homed
                // actual position (0x6064). On PP re-entry the drive adopts 0x607A
                // as its position demand; if that demand differs from actual (which
                // the home just changed) the position loop slams the motor to close
                // the gap — that is the runaway. Copying actual -> target here (raw
                // counts, no scaling) removes the error before PP can act on it,
                // regardless of whether the home landed where intended.
                let pos = view.position_actual();
                view.set_target_position(pos);
                log::info!(
                    "SoftHome[diag]: post-home, pre-PP-switch position_actual(0x6064) = {}; \
                     set target_position(0x607A) = {} so PP re-entry has nothing to chase",
                    pos, pos
                );
                self.home_offset = 0; // drive handles it now
                self.op = AxisOp::SoftHoming(HomeState::AlignTargetBeforePP as u8);
            }
            Some(HomeState::AlignTargetBeforePP) => {
                // One tick later: the RxPDO carrying 0x607A = actual has now been
                // sent, so the drive sees target == actual. Re-assert each tick
                // (cheap) in case actual drifted, then switch to PP mode.
                view.set_target_position(view.position_actual());
                self.op = AxisOp::SoftHoming(HomeState::WriteMotionModeOfOperation as u8);
            }

            Some(HomeState::WriteMotionModeOfOperation) => {
                // Switch back to PP motion mode

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

                if !self.fb_mode_of_operation.is_busy() {
                    if self.fb_mode_of_operation.is_error() {
                        self.set_op_error(&format!(
                            "Software homing SDO error writing homing mode of operation: {} {}",
                            self.fb_mode_of_operation.error_code(),
                            self.fb_mode_of_operation.error_message()
                        ));
                    } else {
                        if self.is_error {
                            log::error!("Drive back in PP mode after error. Homing sequence did not complete!");
                            self.finish_op_error();
                        } else {
                            log::info!(
                                "SoftHome[diag]: back in PP mode, position_actual(0x6064) = {}; \
                                 homing complete, holding (no new set-point latched)",
                                view.position_actual()
                            );
                            // DONE — do NOT latch a fresh PP set-point here.
                            //
                            // A CiA-402 PP drive only moves on a *rising edge* of
                            // controlword bit 4 (new set-point). After homing we are
                            // back in PP mode with bit 4 already low (cleared in
                            // ResetHomingTrigger) and 0x607A aligned to 0x6064
                            // (AlignTargetBeforePP), so the drive simply holds at the
                            // just-homed position — exactly like the proven SDO-only
                            // homing sequence (6060=6 … 6060=1), which never issues a
                            // post-home set-point and never moves.
                            //
                            // The old SendCurrentPositionTarget step DID drive a bit-4
                            // rising edge to latch an absolute set-point at
                            // view.position_actual(). Because homing has just changed
                            // the position frame (home_offset=0, 0x6064 := 0x607C) and
                            // the PDO snapshot lags that relabel by a cycle, the latched
                            // target raced the new 0x6064 and the position loop slammed
                            // the motor to close the gap — the ~10 mm post-home lurch on
                            // the AKD. Holding instead removes that move entirely.
                            self.complete_op();
                        }
                    }
                }
            }

            Some(HomeState::SendCurrentPositionTarget) => {
                // Hold position: send set-point to current position
                let current_pos = view.position_actual();
                log::info!(
                    "SoftHome[diag]: holding at position_actual(0x6064) = {}",
                    current_pos
                );
                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 — commanded {} user units; final position_actual(0x6064) = {} counts",
                        self.config.home_position,
                        view.position_actual()
                    );
                    self.complete_op();
                } else if self.homing_timed_out() {
                    self.set_op_error("Software homing timeout: hold position not acknowledged");
                }
            }
            _ => self.complete_op(),
        }
    }

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

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

                if pos_stable || vel_stopped {
                    self.halt_stable_count = self.halt_stable_count.saturating_add(1);
                } else {
                    self.halt_stable_count = 0;
                }

                if self.halt_stable_count >= HALT_STABLE_TICKS_REQUIRED {
                    // Motor stopped (halt still asserted). Flush the set-point
                    // queue via the shared mode toggle, then release halt.
                    self.flush.start(view, client, &mut self.sdo);
                    self.op_started = Some(Instant::now());
                    self.op = AxisOp::Halting(HaltState::Flushing as u8);
                } else if self.op_stage_timed_out(HALT_STAGE_TIMEOUT) {
                    self.set_op_error("Halt timeout: motor did not stop");
                }
            }
            Some(HaltState::Flushing) => {
                self.flush.tick(view, client, &mut self.sdo);
                if self.flush.is_busy() {
                    if self.op_stage_timed_out(HALT_STAGE_TIMEOUT) {
                        self.set_op_error("Halt timeout: set-point queue flush did not complete");
                    }
                    return;
                }
                if self.flush.is_error() {
                    self.set_op_error(&format!(
                        "Halt: set-point queue flush failed: {} {}",
                        self.flush.error_code(),
                        self.flush.error_message()
                    ));
                    return;
                }
                // Queue flushed and back in PP. Release halt + the set-point bits
                // and drop any stale target so the next move starts clean.
                let mut cw = RawControlWord(view.control_word());
                cw.set_bit(8, false); // clear halt
                cw.set_bit(4, false);
                cw.set_bit(5, false);
                view.set_control_word(cw.raw());
                view.set_target_position(view.position_actual());
                self.complete_op();
            }
            None => {
                log::warn!("Axis halt: unknown sub-step {}, forcing idle", step);
                self.complete_op();
            }
        }
    }

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

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

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

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

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

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

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

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

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

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

    #[test]
    fn trace_records_on_change_not_every_tick() {
        let (mut axis, mut client, _r, _w) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0040); // Switch On Disabled

        // Many idle ticks with a constant control/status word → one entry, not
        // one per tick. This is what keeps a multi-second stall cheap.
        for _ in 0..10 {
            axis.tick(&mut view, &mut client);
        }
        let after_idle = axis.trace().lines().count();
        assert!(
            after_idle <= 1,
            "constant state must record ≤1 line, got {}",
            after_idle
        );

        // A status-word change records exactly one new transition.
        view.set_state(0x0027); // Operation Enabled
        axis.tick(&mut view, &mut client);
        assert!(
            axis.trace().lines().count() > after_idle,
            "a SW change adds a line"
        );
        assert!(
            axis.trace().contains("Operation Enabled"),
            "trace decodes the SW state"
        );
    }

    #[test]
    fn trace_decoders_render_handshake_bits() {
        // Cancel control word with HALT cleared (the intended fix) vs still set.
        assert_eq!(decode_cw(0x003F), "SwOn|EnVolt|QStop|EnOp|NewSP|ChgSet");
        assert!(
            decode_cw(0x013F).contains("HALT"),
            "0x13F still asserts HALT"
        );
        // Status word: Operation Enabled with set-point acknowledge (bit 12).
        let s = decode_sw(0x1027);
        assert!(
            s.contains("Operation Enabled") && s.contains("SP_ACK"),
            "got {s}"
        );
    }

    #[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: drive confirms it saw bit 4 go low (bit 12 clears). The move
        // FB waits for this handshake reset before watching for target_reached.
        view.status_word = 0x0027; // OperationEnabled, bit 12 cleared
        axis.tick(&mut view, &mut client);

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

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

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

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

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

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

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

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

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

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

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

        axis.reset_faults(&mut view);

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

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

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

        // Simulate drive leaving OperationEnabled
        view.set_state(0x0021); // ReadyToSwitchOn
        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);

        // home_offset is now established by homing (the drive owns the
        // reference), not captured at enable(). Set it directly to verify the
        // reported-position formula: position = to_user(raw - home_offset).
        axis.home_offset = 5000;
        view.position_actual = 6600;
        axis.tick(&mut view, &mut client);

        // (6600 - 5000) = 1600 counts = 45° at 12800 counts / 360°.
        assert!((axis.position - 45.0).abs() < 0.1);
    }

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

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

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

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

        axis.halt(&mut view);

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

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

        // Tick alone does NOT immediately return to Idle anymore —
        // WaitStopped polls position stability (5 consecutive ticks
        // with position stable or velocity near zero). With a static
        // MockView, position stays at 0 and velocity stays at 0, so
        // vel_stopped is true every tick → the counter accumulates
        // and the state progresses after HALT_STABLE_TICKS_REQUIRED.
        for _ in 0..HALT_STABLE_TICKS_REQUIRED {
            axis.tick(&mut view, &mut client);
        }
        // After enough stable ticks we advance past WaitStopped into the mode
        // toggle (Flushing), with the SDO mode switch under way.
        assert!(matches!(axis.op, AxisOp::Halting(_)));
        let AxisOp::Halting(step) = axis.op.clone() else {
            unreachable!()
        };
        assert_eq!(
            step,
            HaltState::Flushing as u8,
            "should advance past WaitStopped once position/velocity is stable"
        );
        // Halt stays asserted through the away-switch (the old target is still
        // live until the queue is flushed).
        assert!(view.control_word & (1 << 8) != 0, "halt held through the away mode switch");

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    #[test]
    fn limit_halt_flushes_queue_via_mode_toggle() {
        // After a limit-switch halt, the axis runs the Halting close-out:
        // motor stop → toggle the mode of operation (PP → Homing → PP) to flush
        // the set-point queue → clear halt. This verifies it enters that toggle
        // with halt held; the SDO mode-switch completion is hardware-verified.
        let (mut axis, mut client, _resp_tx, _write_rx) = test_axis();
        let mut view = MockView::new();
        view.set_state(0x0027); // OperationEnabled
        axis.tick(&mut view, &mut client);

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

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

        // Advance past WaitStopped into the mode toggle. With a static MockView
        // and no SDO responder, the mode switch stays in flight — so we verify
        // the structural transition here (the full SDO mode-switch completion is
        // exercised on hardware via the CiA-402 trace): halt is held while the
        // set-point queue is flushed by switching the mode of operation away
        // from Profile Position.
        view.velocity_actual = 0;
        view.positive_limit = false;
        for _ in 0..HALT_STABLE_TICKS_REQUIRED {
            axis.tick(&mut view, &mut client);
        }
        let AxisOp::Halting(step) = axis.op.clone() else {
            unreachable!()
        };
        assert_eq!(step, HaltState::Flushing as u8, "halt flushes the queue via a mode toggle");
        assert!(view.control_word & (1 << 8) != 0, "halt held through the queue flush");
        assert!(axis.is_busy, "axis stays busy through the close-out");
        assert_ne!(
            view.modes_of_operation,
            ModesOfOperation::ProfilePosition.as_i8(),
            "mode switched away from PP to flush the queue",
        );
    }

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

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

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

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

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

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

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

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

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

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

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

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

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

    // ── Software homing tests ──

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

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

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

    /// Test helper: pull the next outgoing SDO request off `write_rx` and feed a
    /// success response back through the client. Returns the request so callers
    /// can inspect the object index / value. Works for ANY SDO — including the
    /// `FbSetModeOfOperation` mode write/read whose transaction id is private —
    /// because the outgoing message carries its own `transaction_id`.
    fn ack_next_sdo(
        write_rx: &mut tokio::sync::mpsc::UnboundedReceiver<String>,
        resp_tx: &tokio::sync::mpsc::UnboundedSender<mechutil::ipc::CommandMessage>,
        client: &mut CommandClient,
        value: serde_json::Value,
    ) -> mechutil::ipc::CommandMessage {
        let raw = write_rx
            .try_recv()
            .expect("expected an outgoing SDO request on write_rx");
        let req: mechutil::ipc::CommandMessage =
            serde_json::from_str(&raw).expect("outgoing SDO request is valid CommandMessage JSON");
        resp_tx
            .send(mechutil::ipc::CommandMessage::response(
                req.transaction_id,
                value,
            ))
            .unwrap();
        client.poll();
        req
    }

    /// Test helper: drive one `FbSetModeOfOperation` cycle (0x6060 write +
    /// 0x6061 read-back) to completion while the axis sits in whatever Wait*
    /// state owns it. The 100 ms read-back gate is real wall-clock time, so this
    /// sleeps just past it. `mode` is the value the drive reports for 0x6061.
    fn drive_one_mode_switch(
        axis: &mut Axis,
        view: &mut MockView,
        client: &mut CommandClient,
        write_rx: &mut tokio::sync::mpsc::UnboundedReceiver<String>,
        resp_tx: &tokio::sync::mpsc::UnboundedSender<mechutil::ipc::CommandMessage>,
        mode: i8,
    ) {
        // 0x6060 write ack → arms the 100 ms read-back timer.
        ack_next_sdo(write_rx, resp_tx, client, json!(null));
        axis.tick(view, client);
        std::thread::sleep(Duration::from_millis(110));
        // Timer elapsed → issues the 0x6061 read.
        axis.tick(view, client);
        ack_next_sdo(write_rx, resp_tx, client, json!({ "value": mode as i64 }));
        axis.tick(view, client);
    }

    #[test]
    fn soft_homing_pnp_sensor_detection_halts() {
        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);
        // Searching for the sensor.
        axis.op = AxisOp::SoftHoming(HomeState::WaitFoundSensor as u8);

        // PNP sensor trips (true = detected).
        view.home_sensor = true;
        view.position_actual = 5000;
        axis.tick(&mut view, &mut client);

        // Sensor found → halt asserted, advance to wait-stopped.
        assert_eq!(
            axis.op,
            AxisOp::SoftHoming(HomeState::WaitStoppedFoundSensor as u8)
        );
        assert!(
            view.control_word & (1 << 8) != 0,
            "halt bit set on sensor detection"
        );
        assert!(!axis.is_error);
    }

    #[test]
    fn soft_homing_npn_sensor_detection_halts() {
        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);
        axis.op = AxisOp::SoftHoming(HomeState::WaitFoundSensor as u8);

        // NPN: sensor goes false = detected.
        view.home_sensor = false;
        view.position_actual = 3000;
        axis.tick(&mut view, &mut client);

        assert_eq!(
            axis.op,
            AxisOp::SoftHoming(HomeState::WaitStoppedFoundSensor as u8)
        );
        assert!(!axis.is_error);
    }

    #[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 onto the positive limit switch.
        axis.home(&mut view, HomingMethod::LimitSwitchPosPnp);
        axis.op = AxisOp::SoftHoming(HomeState::WaitFoundSensor as u8);

        // The positive limit trips. Because we are homing *onto* it, the limit
        // protection in check_limits must suppress the error-halt and let the
        // homing FSM treat it as the home trigger instead.
        view.positive_limit = true;
        view.velocity_actual = 100; // moving positive
        view.position_actual = 8000;
        axis.tick(&mut view, &mut client);

        assert_eq!(
            axis.op,
            AxisOp::SoftHoming(HomeState::WaitStoppedFoundSensor as u8)
        );
        assert!(
            !axis.is_error,
            "homed-on limit switch must be suppressed, not faulted"
        );
    }

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

        // Negative-direction home: the search move target must be negative.
        axis.home(&mut view, HomingMethod::HomeSensorNegPnp);
        axis.op = AxisOp::SoftHoming(HomeState::Search as u8); // skip the PP-mode SDO preamble
        axis.tick(&mut view, &mut client);
        assert!(view.target_position < 0, "negative home seeks a negative target");

        // Positive-direction home: the search move target must be positive.
        axis.home(&mut view, HomingMethod::HomeSensorPosPnp);
        axis.op = AxisOp::SoftHoming(HomeState::Search as u8);
        axis.tick(&mut view, &mut client);
        assert!(view.target_position > 0, "positive home seeks a positive target");
    }

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

        // CurrentPosition35 is the drive-side "set reference here" path: it enters
        // the SoftHoming FSM at WriteHomeOffset (writes 0x607C) rather than the
        // generic integrated homing path, and carries its own method code (35).
        axis.home(&mut view, HomingMethod::CurrentPosition35);
        assert!(matches!(
            axis.op,
            AxisOp::SoftHoming(s) if s == HomeState::WriteHomeOffset as u8
        ));
        assert_eq!(axis.homing_method, 35);
    }

    #[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));
        // Integrated homing now starts at step 16 (home-offset write).
        assert!(matches!(axis.op, AxisOp::Homing(16)));
        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 16: writes home-offset SDO (0x607C), then waits ack → step 0.
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::Homing(17)));
        let _ = write_rx.try_recv();
        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);
        assert!(matches!(axis.op, AxisOp::Homing(0)));

        // 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 16: home-offset write (0x607C), ack → step 0.
        axis.tick(&mut view, &mut client);
        assert!(matches!(axis.op, AxisOp::Homing(17)));
        let _ = write_rx.try_recv();
        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);

        // 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_search_ack() {
        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);
        // Waiting for the search move's set-point acknowledge.
        axis.op = AxisOp::SoftHoming(HomeState::WaitSearching as u8);

        // Sensor rises before the ack arrives — must still be caught.
        view.home_sensor = true;
        view.position_actual = 2000;
        axis.tick(&mut view, &mut client);

        assert_eq!(
            axis.op,
            AxisOp::SoftHoming(HomeState::WaitFoundSensor as u8)
        );
    }

    /// Regression: a set-current-position home (method 35/37) reports homing
    /// *attained* (SW bit 12) but never *target reached* (SW bit 10). The
    /// completion check must accept attained-alone, or it hangs to timeout
    /// (the field symptom was sw=0x1237 on the Kollmorgen AKD).
    #[test]
    fn soft_homing_completes_on_attained_without_reached() {
        let (mut axis, mut client, _resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        // OperationEnabled + homing attained (bit 12), target NOT reached (bit 10 clear).
        view.status_word = 0x1027;
        axis.op = AxisOp::SoftHoming(HomeState::WaitHomingDone as u8);
        axis.op_started = Some(Instant::now());

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

        assert_eq!(
            axis.op,
            AxisOp::SoftHoming(HomeState::ResetHomingTrigger as u8),
            "attained-only must complete homing, not wait for target_reached"
        );
        assert!(!axis.is_error);
    }

    /// Regression: the homing-start gate must not wait for SW bit 12 to *clear*.
    /// An instantaneous method sets attained within one cycle, so bit 12 may
    /// never be observed low — gating on it would race into a spurious
    /// "did not acknowledge homing start" timeout.
    #[test]
    fn soft_homing_start_gate_does_not_require_attained_clear() {
        let (mut axis, mut client, _resp_tx, _write_rx) = soft_homing_axis();
        let mut view = MockView::new();
        // Bit 12 already high (attained), no error bit.
        view.status_word = 0x1027;
        axis.op = AxisOp::SoftHoming(HomeState::WaitHomingStarted as u8);
        axis.op_started = Some(Instant::now());

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

        assert_eq!(
            axis.op,
            AxisOp::SoftHoming(HomeState::WaitHomingDone as u8),
            "start gate must advance even with attained already set"
        );
        assert!(!axis.is_error);
    }

    /// The shared set-point-queue flush toggles the mode of operation
    /// PP → Homing → PP via `FbSetModeOfOperation` (0x6060 write + 0x6061
    /// read-back). Drives the `FbFlushQueue` directly to completion.
    #[test]
    fn flush_queue_toggles_pp_homing_pp() {
        use tokio::sync::mpsc;
        let (write_tx, mut write_rx) = mpsc::unbounded_channel();
        let (resp_tx, response_rx) = mpsc::unbounded_channel();
        let mut client = CommandClient::new(write_tx, response_rx);
        let mut view = MockView::new();
        let mut sdo = SdoClient::new("TestDrive");
        let mut flush = FbFlushQueue::new();

        // start() switches AWAY from PP (to Homing) — issues the 0x6060 write.
        flush.start(&mut view, &mut client, &mut sdo);
        assert!(flush.is_busy());
        assert_eq!(view.modes_of_operation, ModesOfOperation::Homing.as_i8());

        // Away phase: complete the PP → Homing switch.
        ack_next_sdo(&mut write_rx, &resp_tx, &mut client, json!(null)); // 0x6060 write ack
        flush.tick(&mut view, &mut client, &mut sdo);
        std::thread::sleep(Duration::from_millis(110));
        flush.tick(&mut view, &mut client, &mut sdo); // issues 0x6061 read
        ack_next_sdo(
            &mut write_rx,
            &resp_tx,
            &mut client,
            json!({ "value": ModesOfOperation::Homing.as_i8() as i64 }),
        );
        flush.tick(&mut view, &mut client, &mut sdo); // mode_fb done → begin switch BACK to PP

        assert!(flush.is_busy(), "still flushing — now switching back to PP");
        assert_eq!(
            view.modes_of_operation,
            ModesOfOperation::ProfilePosition.as_i8()
        );

        // Back phase: complete the Homing → PP switch.
        ack_next_sdo(&mut write_rx, &resp_tx, &mut client, json!(null)); // 0x6060 write ack
        flush.tick(&mut view, &mut client, &mut sdo);
        std::thread::sleep(Duration::from_millis(110));
        flush.tick(&mut view, &mut client, &mut sdo); // issues 0x6061 read
        ack_next_sdo(
            &mut write_rx,
            &resp_tx,
            &mut client,
            json!({ "value": ModesOfOperation::ProfilePosition.as_i8() as i64 }),
        );
        flush.tick(&mut view, &mut client, &mut sdo);

        assert!(!flush.is_busy(), "flush complete");
        assert!(!flush.is_error());
    }

    /// End-to-end coverage of the production `home(CurrentPosition)` path (the
    /// one gen4_ac uses for "Home Press"): write 0x607C, switch to Homing mode,
    /// write the method, trigger the set-current-position home, switch back to
    /// PP, and hold. Drives every SDO ack and mode read-back. The drive reports
    /// attained-without-reached throughout (method completes instantly). The
    /// DRIVE owns the position afterward — the software offset is zero.
    #[test]
    fn home_current_position_sets_drive_and_zeros_offset() {
        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);
        // OperationEnabled + homing attained (bit 12), target never reached.
        view.status_word = 0x1027;

        axis.set_home_position(90.0);
        axis.home(&mut view, HomingMethod::CurrentPosition35);
        assert_eq!(axis.config.home_position, 90.0);
        assert!(matches!(
            axis.op,
            AxisOp::SoftHoming(s) if s == HomeState::WriteHomeOffset as u8
        ));

        // WriteHomeOffset issues the 0x607C reference write.
        axis.tick(&mut view, &mut client); // → WaitHomeOffsetDone
        // WaitHomeOffsetDone: ack the 0x607C write.
        ack_next_sdo(&mut write_rx, &resp_tx, &mut client, json!(null));
        axis.tick(&mut view, &mut client); // → WaitHomeOffsetReadback (issues diagnostic 0x607C read)
        // Diagnostic read-back of 0x607C — value is logged only, not asserted on.
        ack_next_sdo(&mut write_rx, &resp_tx, &mut client, json!({ "value": -9701 }));
        axis.tick(&mut view, &mut client); // → WriteHomingModeOp

        // WriteHomingModeOp issues the Homing-mode switch.
        axis.tick(&mut view, &mut client); // → WaitWriteHomingModeOp
        drive_one_mode_switch(
            &mut axis,
            &mut view,
            &mut client,
            &mut write_rx,
            &resp_tx,
            ModesOfOperation::Homing.as_i8(),
        ); // → WriteHomingMethod

        // WriteHomingMethod issues the 0x6098 method write.
        axis.tick(&mut view, &mut client); // → WaitWriteHomingMethodDone
        ack_next_sdo(&mut write_rx, &resp_tx, &mut client, json!(null));
        axis.tick(&mut view, &mut client); // → ClearHomingTrigger

        axis.tick(&mut view, &mut client); // ClearHomingTrigger → TriggerHoming
        axis.tick(&mut view, &mut client); // TriggerHoming → WaitHomingStarted
        axis.tick(&mut view, &mut client); // WaitHomingStarted → WaitHomingDone
        axis.tick(&mut view, &mut client); // WaitHomingDone (bit 12) → ResetHomingTrigger
        axis.tick(&mut view, &mut client); // ResetHomingTrigger → WaitHomingTriggerCleared
        axis.tick(&mut view, &mut client); // WaitHomingTriggerCleared → AlignTargetBeforePP

        // Drive now owns the reference; the software offset is zeroed.
        assert_eq!(axis.home_offset, 0);

        // AlignTargetBeforePP re-asserts 0x607A = 0x6064, then advances to the
        // PP-mode switch (no SDO of its own).
        axis.tick(&mut view, &mut client); // AlignTargetBeforePP → WriteMotionModeOfOperation

        // Switch back to PP mode. Completing the PP switch finishes the home:
        // the drive holds at the just-homed position and NO fresh PP set-point is
        // latched (a latch would race the just-relabeled 0x6064 and lurch the
        // motor — the old SendCurrentPositionTarget bug). Mirrors the proven
        // SDO-only homing sequence, which never issues a post-home set-point.
        axis.tick(&mut view, &mut client); // WriteMotionModeOfOperation → WaitWriteMotionModeOfOperation
        drive_one_mode_switch(
            &mut axis,
            &mut view,
            &mut client,
            &mut write_rx,
            &resp_tx,
            ModesOfOperation::ProfilePosition.as_i8(),
        ); // → complete (Idle), no set-point latched

        assert_eq!(axis.op, AxisOp::Idle);
        assert!(!axis.is_error, "persistent home must finish clean");
        assert!(!axis.is_busy);
        assert_eq!(axis.home_offset, 0);
    }
}