autocore-std 3.3.43

use crate::fb::StateMachine;
use super::axis_view::AxisHandle;

/// Move To Load function block.
///
/// Drives an axis toward a target load (e.g. from a load cell) and halts
/// as soon as the reading reaches that load.
///
/// - If `current_load > target_load`, the axis moves in the negative direction.
/// - If `current_load < target_load`, the axis moves in the positive direction.
///
/// If the axis reaches `position_limit` before the load is achieved, the
/// FB halts and enters an error state.
///
/// # Lifecycle
///
/// `is_busy()` is `true` between [`start`](Self::start) and the moment the
/// axis stops (either by reaching the load, hitting the position limit, or
/// being aborted). When the FB returns to idle, check [`is_error`](Self::is_error)
/// to find out whether the move succeeded.
///
/// # Noise rejection
///
/// This FB does **single-sample threshold detection** — it compares
/// `current_load` against the target once per [`tick`](Self::tick) and
/// triggers the moment the threshold is crossed. There is no FB-side
/// debounce, because adding scan-count debounce would compound latency on
/// top of whatever filtering the load card already applies, and overshoot
/// in load control is force on the specimen.
///
/// **Configure the load card's filter as the noise-rejection layer.** For
/// example, the Beckhoff EL3356's IIR filter or the NI 9237's BAA / decimation
/// filter both run in hardware with known phase response. By the time the
/// reading reaches `current_load`, it should already be smooth enough that
/// a single-sample threshold is reliable.
///
/// # Overshoot avoidance: `dead_band`
///
/// Even with a perfectly clean signal, there is unavoidable latency between
/// trigger and stop: the trigger fires, the FB calls `axis.halt()`, and
/// the axis decelerates over some distance — during which the load
/// continues to change. The result is overshoot past the requested target.
///
/// The `dead_band` configuration trips the threshold *early* by `dead_band`
/// units in the direction of motion so the deceleration phase brings the
/// load to the actual target rather than past it:
///
/// ```text
///     load (moving_negative = true, load decreasing)
///       │
/// start ●───────────╮          trigger at target + dead_band
///                   │          ↓
///                   ●·······╮  halt() called
///                            ╲
///         target ────────────●◀── final load (close to target)
///                              ╲ → undershoot here without dead_band
///                  └─ time ─────────────▶
/// ```
///
/// `dead_band` defaults to `0.0` (strict triggering, fine for slow moves
/// or where the position-limit safety net catches overshoot). To configure
/// it, measure the worst-case overshoot during commissioning and set
/// `dead_band` to that value via [`set_dead_band`](Self::set_dead_band).
/// It persists across [`start`](Self::start) calls.
#[derive(Debug, Clone)]
pub struct MoveToLoad {
    /// Internal state machine.
    state:               StateMachine,
    moving_negative:     bool,
    position_limit:      f64,
    target_load:         f64,
    
    /// Target speed of the move in user units. Slower is more precise.
    /// Faster usually means damage
    target_speed: f64,
    /// Target accel of the move in user units.
    /// This setting does not exact the stop decel, only the initial acceleration.
    target_accel : f64,

    /// True after `axis.is_busy()` has been observed `true` at least once
    /// during the current move. Prevents the moving-state branch from
    /// treating the one-tick window between issuing `move_absolute` and
    /// the drive flipping its busy bit as "stopped without reaching load."
    seen_busy:           bool,
    /// Position at the instant the load threshold was crossed.
    /// `f64::NAN` until the FB triggers at least once.
    triggered_position:  f64,
    /// Load reading at the instant the threshold was crossed.
    /// `f64::NAN` until the FB triggers at least once.
    triggered_load:      f64,
    /// Early-trigger margin in load units. The threshold fires when the
    /// load is within `dead_band` of `target_load` in the direction of
    /// motion, so the post-halt deceleration brings the load to the
    /// actual target rather than past it. Configured via
    /// [`set_dead_band`](Self::set_dead_band); defaults to `0.0`.
    /// Persists across [`start`](Self::start) calls.
    dead_band:           f64,
    /// Deceleration (user units / s²) passed to `move_absolute` for the
    /// drive-side motion profile. `None` means "use `target_accel`."
    /// Typically set once at init from the drive's 0x6085 (quick-stop
    /// deceleration) SDO by the owning process — see the type-level docs.
    /// Persists across [`start`](Self::start) calls.
    stop_decel:          Option<f64>,

    /// Speed (user units / s) for the optional post-halt settle move.
    /// When paired with a non-zero `settle_tolerance`, the FB will issue
    /// a slow corrective move in the direction opposite to the primary
    /// motion if the measured load has overshot the target. Set to 0 to
    /// disable. Persists across [`start`](Self::start) calls.
    settle_speed:        f64,
    /// Load tolerance (same units as `target_load`) used to decide
    /// whether the post-halt reading is acceptable. If
    /// `|current_load − target_load| > settle_tolerance` after the axis
    /// stops, the FB enters a `Settling` state and moves slowly against
    /// the original motion until the load is back inside the tolerance
    /// band. Set to 0 to disable. Persists across [`start`](Self::start) calls.
    settle_tolerance:    f64,
}

/// Slack on the position-limit comparison. In axis user units (mm / deg /
/// counts depending on `AxisConfig`). Treats positions within this many
/// units of the limit as "at the limit," so floating-point round-off
/// doesn't miss an exact-match crossing.
const POSITION_LIMIT_TOLERANCE: f64 = 1e-4;

#[repr(i32)]
#[derive(Copy, Clone, PartialEq, Debug)]
enum MtlState {
    Idle     = 0,
    Start    = 1,
    Moving   = 10,
    Stopping = 20,
    /// Post-halt corrective move when the settle feature is enabled and
    /// the load has overshot target beyond `settle_tolerance`. Axis is
    /// commanded in the direction opposite to the primary motion at
    /// `settle_speed`. Exits to `StoppingSettle` once the load is back
    /// inside the tolerance band.
    Settling = 30,
    /// Waiting for the axis to come to rest after the settle move halts.
    /// Returns to `Idle` once `axis.is_busy()` is false.
    StoppingSettle = 40,
    /// Externally-set: requests an immediate halt, returns to Idle.
    Halt     = 50,
}

impl Default for MoveToLoad {
    fn default() -> Self {
        Self {
            state:              StateMachine::new(),
            moving_negative:    false,
            position_limit:     0.0,
            target_load:        0.0,
            target_speed: 0.0,
            target_accel : 0.0,
            seen_busy:          false,
            triggered_position: f64::NAN,
            triggered_load:     f64::NAN,
            dead_band:          0.0,
            stop_decel:         None,
            settle_speed:       0.0,
            settle_tolerance:   0.0,
        }
    }
}

impl MoveToLoad {
    /// Constructor.
    pub fn new() -> Self {
        Self::default()
    }

    /// Abort the operation. The axis will be halted by the next [`tick`](Self::tick).
    pub fn abort(&mut self) {
        self.state.set_error(200, "Abort called");
        self.state.index = MtlState::Idle as i32;
    }

    /// Start the move-to-load operation.
    ///
    /// `target_load` and `position_limit` are latched on this call and used
    /// by every subsequent [`tick`](Self::tick) until the FB returns to idle.
    /// Any previous trigger position / load values are cleared.
    pub fn start(
        &mut self, 
        target_load: f64, 
        target_speed : f64, 
        target_accel : f64, 
        position_limit: f64
    ) {
        self.state.clear_error();
        self.target_load        = target_load;
        self.target_speed = target_speed;
        self.target_accel = target_accel;
        self.position_limit     = position_limit;
        self.seen_busy          = false;
        self.triggered_position = f64::NAN;
        self.triggered_load     = f64::NAN;
        self.state.index        = MtlState::Start as i32;
    }

    /// Reset the state machine to Idle. Does not halt the axis (call
    /// [`abort`](Self::abort) for that, or halt the axis directly).
    pub fn reset(&mut self) {
        self.state.clear_error();
        self.state.index = MtlState::Idle as i32;
    }

    /// True if the FB encountered an error during the last command.
    pub fn is_error(&self) -> bool {
        self.state.is_error()
    }

    /// Returns the error message for the last command.
    pub fn error_message(&self) -> String {
        return self.state.error_message.clone();
    }    


    /// True if the FB is currently executing a command.
    pub fn is_busy(&self) -> bool {
        self.state.index > MtlState::Idle as i32
    }

    /// Axis position at the moment the load threshold was crossed.
    /// Returns `f64::NAN` until the FB has triggered at least once.
    /// Cleared by [`start`](Self::start).
    pub fn triggered_position(&self) -> f64 {
        self.triggered_position
    }

    /// Load reading at the moment the threshold was crossed.
    /// Returns `f64::NAN` until the FB has triggered at least once.
    /// Cleared by [`start`](Self::start).
    pub fn triggered_load(&self) -> f64 {
        self.triggered_load
    }

    /// Set the early-trigger margin in load units. See the type-level
    /// docs for what this is and when to use it.
    ///
    /// Negative values are clamped to `0.0`. The new value is read on
    /// every [`tick`](Self::tick), so it can be changed mid-move if the
    /// caller needs to.
    pub fn set_dead_band(&mut self, value: f64) {
        self.dead_band = value.max(0.0);
    }

    /// Currently-configured early-trigger margin.
    pub fn dead_band(&self) -> f64 {
        self.dead_band
    }

    /// Set the deceleration (user units / s²) used for the drive-side motion
    /// profile. Typically populated once at process startup by SDO-reading
    /// the drive's quick-stop deceleration (0x6085) and converting counts/s²
    /// to user units.
    ///
    /// Pass `None` to revert to the default behavior (use `target_accel` as
    /// the deceleration). Values ≤ 0 are treated as `None`. Persists across
    /// [`start`](Self::start) calls.
    pub fn set_stop_decel(&mut self, value: Option<f64>) {
        self.stop_decel = value.filter(|v| *v > 0.0);
    }

    /// Currently-configured deceleration, or `None` if the FB is falling
    /// back to `target_accel`.
    pub fn stop_decel(&self) -> Option<f64> {
        self.stop_decel
    }

    /// Enable the post-halt settle correction by setting both
    /// `settle_speed` and `settle_tolerance` to non-zero values. Either
    /// value at 0 disables the feature. See the `Settling` state docs.
    ///
    /// Values clamped to `>= 0`. Persists across [`start`](Self::start).
    pub fn set_settle(&mut self, speed: f64, tolerance: f64) {
        self.settle_speed     = speed.max(0.0);
        self.settle_tolerance = tolerance.max(0.0);
    }

    /// Currently-configured settle speed (user units / s). Zero = disabled.
    pub fn settle_speed(&self) -> f64 {
        self.settle_speed
    }

    /// Currently-configured settle load tolerance. Zero = disabled.
    pub fn settle_tolerance(&self) -> f64 {
        self.settle_tolerance
    }

    /// Is the settle feature enabled? (Both speed and tolerance must be
    /// > 0 for the Settling state to engage.)
    fn settle_enabled(&self) -> bool {
        self.settle_speed > 0.0 && self.settle_tolerance > 0.0
    }

    /// Execute the function block.
    ///
    /// - `axis`: the axis being driven.
    /// - `current_load`: the latest load reading. Assumed already filtered
    ///   by the load card — see the type-level docs on noise rejection.
    pub fn tick(
        &mut self,
        axis:         &mut impl AxisHandle,
        current_load: f64,
    ) {
        // Safety: a fault on the axis aborts any in-progress command.
        if axis.is_error() && self.state.index > MtlState::Idle as i32 {
            self.state.set_error(120, "Axis is in error state");
            self.state.index = MtlState::Idle as i32;
        }

        match MtlState::from_index(self.state.index) {
            Some(MtlState::Idle) => {
                // do nothing
            }
            Some(MtlState::Start) => {
                self.state.clear_error();
                self.seen_busy = false;
                // Direction is determined strictly so the dead_band region
                // doesn't make direction undefined when current ≈ target.
                self.moving_negative = current_load > self.target_load;

                // Already at or past the load threshold (or within the
                // dead-band of it)? Done — no point starting a move.
                let reached = self.threshold_reached(current_load);

                if reached {
                    self.triggered_position = axis.position();
                    self.triggered_load     = current_load;
                    self.state.index        = MtlState::Idle as i32;
                } else if self.already_past_limit(axis) {
                    self.state.set_error(110, "Axis already past position limit before starting");
                    self.state.index = MtlState::Idle as i32;
                } else {
                    let decel = self.stop_decel.unwrap_or(self.target_accel);
                    axis.move_absolute(
                        self.position_limit,
                        self.target_speed,
                        self.target_accel,
                        decel,
                    );
                    self.state.index = MtlState::Moving as i32;
                }
            }
            Some(MtlState::Moving) => {
                // Latch is_busy=true once we see it so a one-tick
                // command-acceptance window can't false-trigger the
                // "stopped before reaching load" error below.
                if axis.is_busy() {
                    self.seen_busy = true;
                }

                let reached = self.threshold_reached(current_load);

                if reached {
                    self.triggered_position = axis.position();
                    self.triggered_load     = current_load;
                    axis.halt();
                    self.state.index = MtlState::Stopping as i32;
                    return;
                }

                let hit_limit = if self.moving_negative {
                    axis.position() <= self.position_limit + POSITION_LIMIT_TOLERANCE
                } else {
                    axis.position() >= self.position_limit - POSITION_LIMIT_TOLERANCE
                };
                let stopped_unexpectedly = self.seen_busy && !axis.is_busy();

                if hit_limit || stopped_unexpectedly {
                    axis.halt();
                    if hit_limit {
                        
                        self.state.set_error(150, 
                            format!("[FB MoveToLoad] Reached position limit {} {} without hitting target load",
                            if self.moving_negative {"moving NEG"} else {"moving POS"},
                            self.position_limit)
                        );

                    }
                    else {
                        self.state.set_error(151, 
                            "[FB MoveToLoad] Stoped unexpectedly without hitting target load."
                        );                        
                    }
                    self.state.index = MtlState::Idle as i32;
                }
            }
            Some(MtlState::Stopping) => {
                if !axis.is_busy() {
                    // Axis has come to rest after the primary halt. Decide
                    // whether to trigger the settle correction. The feature
                    // is only engaged if both config values are positive AND
                    // the load has overshot target by more than
                    // settle_tolerance. If the load is already close enough,
                    // skip straight to Idle.
                    if self.settle_enabled()
                        && (current_load - self.target_load).abs() > self.settle_tolerance
                    {
                        // Reverse the direction of motion. "Moving negative"
                        // in the primary move means load was decreasing, so
                        // overshoot means the load went below target; we need
                        // to go positive to raise it again. And vice versa.
                        let settle_pos_limit = if self.moving_negative {
                            // Went too far down; move up, but no farther than
                            // the starting side of the primary move.
                            // Use a conservative bound — the opposite of the
                            // primary position_limit isn't known to us, so we
                            // just use axis.position() + a generous window.
                            axis.position() + self.settle_tolerance.abs() * 1.0e3
                        } else {
                            axis.position() - self.settle_tolerance.abs() * 1.0e3
                        };
                        let decel = self.stop_decel.unwrap_or(self.target_accel);
                        axis.move_absolute(
                            settle_pos_limit,
                            self.settle_speed,
                            self.target_accel,
                            decel,
                        );
                        self.seen_busy     = false;
                        self.state.index   = MtlState::Settling as i32;
                    } else {
                        self.state.index = MtlState::Idle as i32;
                    }
                }
            }
            Some(MtlState::Settling) => {
                // Latch axis.is_busy() once; see Moving state for the same
                // reasoning about the one-tick command-acceptance window.
                if axis.is_busy() {
                    self.seen_busy = true;
                }

                // Target direction for the settle move is the opposite of
                // the primary move. When moving_negative was true, the
                // primary move was decreasing the load; during settle we're
                // INCREASING it back up, so we've settled once load has
                // climbed back within tolerance OR gone past target.
                let within_tolerance =
                    (current_load - self.target_load).abs() <= self.settle_tolerance;

                // Safety: if axis reports not busy before we've converged,
                // either it hit a physical limit or the move was rejected.
                // Either way, bail rather than spinning.
                let stopped_unexpectedly = self.seen_busy && !axis.is_busy();

                if within_tolerance {
                    axis.halt();
                    // Re-latch the trigger record to the settled values so
                    // the caller gets what they actually ended up at.
                    self.triggered_position = axis.position();
                    self.triggered_load     = current_load;
                    self.state.index = MtlState::StoppingSettle as i32;
                } else if stopped_unexpectedly {
                    axis.halt();
                    self.state.set_error(
                        160,
                        "[FB MoveToLoad] Settle move stopped before load returned to tolerance",
                    );
                    self.state.index = MtlState::Idle as i32;
                }
            }
            Some(MtlState::StoppingSettle) => {
                if !axis.is_busy() {
                    self.state.index = MtlState::Idle as i32;
                }
            }
            Some(MtlState::Halt) => {
                axis.halt();
                self.state.index = MtlState::Idle as i32;
            }
            None => {
                self.state.index = MtlState::Idle as i32;
            }
        }

        self.state.call();
    }

    fn already_past_limit(&self, axis: &impl AxisHandle) -> bool {
        if self.moving_negative {
            axis.position() <= self.position_limit
        } else {
            axis.position() >= self.position_limit
        }
    }

    /// Has the load reached (or come within `dead_band` of) the target,
    /// from the direction we're moving? Used in both Start and Moving.
    fn threshold_reached(&self, current_load: f64) -> bool {
        if self.moving_negative {
            // Load decreasing toward target — trip slightly above target
            // so deceleration brings us down to it.
            current_load <= self.target_load + self.dead_band
        } else {
            // Load increasing toward target — trip slightly below target.
            current_load >= self.target_load - self.dead_band
        }
    }
}

impl MtlState {
    fn from_index(idx: i32) -> Option<Self> {
        match idx {
            x if x == Self::Idle as i32            => Some(Self::Idle),
            x if x == Self::Start as i32           => Some(Self::Start),
            x if x == Self::Moving as i32          => Some(Self::Moving),
            x if x == Self::Stopping as i32        => Some(Self::Stopping),
            x if x == Self::Settling as i32        => Some(Self::Settling),
            x if x == Self::StoppingSettle as i32  => Some(Self::StoppingSettle),
            x if x == Self::Halt as i32            => Some(Self::Halt),
            _ => None,
        }
    }
}

// -------------------------------------------------------------------------
// Tests
// -------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;
    use crate::motion::axis_config::AxisConfig;

    /// Mock axis: records move/halt calls; the test mutates `position` and
    /// `busy` to simulate motion.
    struct MockAxis {
        position:         f64,
        busy:             bool,
        error:            bool,
        config:           AxisConfig,
        halt_called:      u32,
        last_move_target: f64,
        last_move_accel:  f64,
        last_move_decel:  f64,
    }

    impl MockAxis {
        fn new() -> Self {
            let cfg = AxisConfig::new(1000);
            Self {
                position: 0.0, busy: false, error: false, config: cfg,
                halt_called: 0, last_move_target: 0.0,
                last_move_accel: 0.0, last_move_decel: 0.0,
            }
        }
    }

    impl AxisHandle for MockAxis {
        fn position(&self) -> f64 { self.position }
        fn config(&self) -> &AxisConfig { &self.config }
        fn move_relative(&mut self, _: f64, _: f64, _: f64, _: f64) {}
        fn move_absolute(&mut self, p: f64, _: f64, accel: f64, decel: f64) {
            self.last_move_target = p;
            self.last_move_accel = accel;
            self.last_move_decel = decel;
            self.busy = true;
        }
        fn halt(&mut self) { self.halt_called += 1; self.busy = false; }
        fn is_busy(&self) -> bool { self.busy }
        fn is_error(&self) -> bool { self.error }
        fn motor_on(&self) -> bool { true }
    }

    #[test]
    fn already_at_load_completes_without_moving() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.position = 5.0;

        fb.start(100.0, 10.0, 100.0, 50.0);              // moving positive
        fb.tick(&mut axis, 100.0);          // already at target

        assert!(!fb.is_busy());
        assert!(!fb.is_error());
        assert_eq!(axis.last_move_target, 0.0, "must not issue a move");
        assert_eq!(fb.triggered_position(), 5.0);
        assert_eq!(fb.triggered_load(), 100.0);
    }

    #[test]
    fn already_past_limit_errors_immediately() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.position = 60.0;               // already past the +50 limit

        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);            // load not reached

        assert!(fb.is_error());
        assert!(!fb.is_busy());
        assert_eq!(axis.last_move_target, 0.0);
    }

    #[test]
    fn moves_positive_then_triggers_on_load_threshold() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.position = 0.0;

        // Load below target → should command +position move.
        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);
        assert_eq!(axis.last_move_target, 50.0);
        assert!(axis.busy);

        // Tick a few times below threshold — no halt.
        axis.position = 10.0; fb.tick(&mut axis, 50.0);
        axis.position = 20.0; fb.tick(&mut axis, 80.0);
        assert_eq!(axis.halt_called, 0);

        // Cross the threshold — halt + record trigger values.
        axis.position = 25.0; fb.tick(&mut axis, 100.5);
        assert_eq!(axis.halt_called, 1);
        assert_eq!(fb.triggered_position(), 25.0);
        assert_eq!(fb.triggered_load(), 100.5);

        // Stopping → Idle once axis flips !busy. (halt() in the mock
        // already cleared busy.)
        fb.tick(&mut axis, 100.5);
        assert!(!fb.is_busy());
        assert!(!fb.is_error());
    }

    #[test]
    fn moves_negative_when_load_above_target() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.position = 100.0;

        // Load above target → should command -position move.
        fb.start(50.0, 10.0, 100.0,0.0);
        fb.tick(&mut axis, 100.0);
        assert_eq!(axis.last_move_target, 0.0);

        axis.position = 50.0; fb.tick(&mut axis, 49.0);  // crossed (descending)
        assert_eq!(axis.halt_called, 1);
        assert_eq!(fb.triggered_load(), 49.0);
    }

    #[test]
    fn position_limit_without_load_triggers_error() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.position = 0.0;

        fb.start(100.0, 10.0, 100.0,50.0);
        fb.tick(&mut axis, 0.0);            // start → Moving
        axis.position = 50.0;
        fb.tick(&mut axis, 10.0);           // hit limit, load not reached

        assert!(fb.is_error());
        assert_eq!(axis.halt_called, 1);
    }

    #[test]
    fn startup_busy_race_does_not_false_trigger() {
        // Reproduces the original bug: between move_absolute and the drive
        // flipping is_busy=true, the FB used to interpret !is_busy as
        // "axis stopped without reaching load" and erroneously errored.
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.position = 0.0;
        axis.busy = false;                  // drive briefly reports !busy

        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 10.0);           // start → Moving; busy goes true via move_absolute
        axis.busy = false;                  // simulate drive briefly reporting !busy
        fb.tick(&mut axis, 20.0);           // would have errored before the fix

        // Without seen_busy latching, this would now be in error.
        assert!(!fb.is_error(), "must not error during the busy-acceptance window");
    }

    #[test]
    fn abort_sets_error_and_returns_idle() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);            // Moving
        assert!(fb.is_busy());

        fb.abort();
        assert!(!fb.is_busy());
        assert!(fb.is_error());
    }

    #[test]
    fn external_halt_state_halts_axis() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.busy = true;

        // Set the Halt state index directly (this is how external code
        // requests the halt path — the FB doesn't expose a public method
        // for it; halting the axis directly is also fine).
        fb.state.index = MtlState::Halt as i32;
        fb.tick(&mut axis, 0.0);

        assert_eq!(axis.halt_called, 1);
        assert!(!fb.is_busy());
    }

    #[test]
    fn axis_fault_aborts_in_progress_command() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);            // Moving
        axis.error = true;
        fb.tick(&mut axis, 0.0);

        assert!(fb.is_error());
        assert!(!fb.is_busy());
    }

    #[test]
    fn triggered_values_clear_on_restart() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();

        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 100.0);          // already at target → triggered
        assert_eq!(fb.triggered_load(), 100.0);

        fb.start(50.0,10.0, 100.0, 0.0);                // restart
        assert!(fb.triggered_load().is_nan());
        assert!(fb.triggered_position().is_nan());
    }

    // ── dead_band (Pass B) ─────────────────────────────────────────────

    #[test]
    fn default_dead_band_is_zero() {
        let fb = MoveToLoad::new();
        assert_eq!(fb.dead_band(), 0.0);
    }

    #[test]
    fn set_dead_band_clamps_negative_to_zero() {
        let mut fb = MoveToLoad::new();
        fb.set_dead_band(-5.0);
        assert_eq!(fb.dead_band(), 0.0);
        fb.set_dead_band(2.5);
        assert_eq!(fb.dead_band(), 2.5);
    }

    #[test]
    fn dead_band_persists_across_start_calls() {
        let mut fb = MoveToLoad::new();
        fb.set_dead_band(3.0);
        fb.start(100.0,10.0, 100.0, 50.0);
        assert_eq!(fb.dead_band(), 3.0);
        fb.start(50.0,10.0, 100.0, 0.0);
        assert_eq!(fb.dead_band(), 3.0, "configuration must outlive a single move");
    }

    #[test]
    fn dead_band_triggers_early_for_positive_motion() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();

        // Without dead_band: would trigger at current >= 100.
        // With dead_band = 5: triggers at current >= 95.
        fb.set_dead_band(5.0);
        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);            // start moving
        assert!(axis.busy);

        // 94 — not yet inside the dead-band.
        axis.position = 10.0; fb.tick(&mut axis, 94.0);
        assert_eq!(axis.halt_called, 0);

        // 95.5 — within dead_band of target → trip.
        axis.position = 11.0; fb.tick(&mut axis, 95.5);
        assert_eq!(axis.halt_called, 1);
        assert_eq!(fb.triggered_load(), 95.5);
    }

    #[test]
    fn dead_band_triggers_early_for_negative_motion() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.position = 100.0;

        // Target 50, current 100, dead_band 5:
        // Without dead_band: triggers at current <= 50.
        // With dead_band = 5: triggers at current <= 55.
        fb.set_dead_band(5.0);
        fb.start(50.0,10.0, 100.0, 0.0);
        fb.tick(&mut axis, 100.0);          // start moving negative

        axis.position = 75.0; fb.tick(&mut axis, 60.0);   // not yet in dead-band
        assert_eq!(axis.halt_called, 0);

        axis.position = 70.0; fb.tick(&mut axis, 54.5);   // inside dead_band → trip
        assert_eq!(axis.halt_called, 1);
        assert_eq!(fb.triggered_load(), 54.5);
    }

    #[test]
    fn within_dead_band_at_start_completes_immediately() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        axis.position = 5.0;

        // Target 100, dead_band 10. Current 92 is within dead_band → done.
        fb.set_dead_band(10.0);
        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 92.0);

        assert!(!fb.is_busy());
        assert!(!fb.is_error());
        assert_eq!(axis.last_move_target, 0.0, "must not issue a move");
        assert_eq!(fb.triggered_load(), 92.0);
    }

    // ── stop_decel ─────────────────────────────────────────────────────

    #[test]
    fn default_stop_decel_is_none_and_falls_back_to_target_accel() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        assert_eq!(fb.stop_decel(), None);

        fb.start(100.0, 10.0, 250.0, 50.0);
        fb.tick(&mut axis, 0.0);                 // Start → Moving
        assert_eq!(axis.last_move_accel, 250.0);
        assert_eq!(axis.last_move_decel, 250.0, "without set_stop_decel, decel == accel");
    }

    #[test]
    fn set_stop_decel_forwards_to_move_absolute() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        fb.set_stop_decel(Some(800.0));
        assert_eq!(fb.stop_decel(), Some(800.0));

        fb.start(100.0, 10.0, 250.0, 50.0);
        fb.tick(&mut axis, 0.0);
        assert_eq!(axis.last_move_accel, 250.0);
        assert_eq!(axis.last_move_decel, 800.0, "explicit stop_decel must flow through");
    }

    #[test]
    fn set_stop_decel_none_reverts_to_fallback() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        fb.set_stop_decel(Some(800.0));
        fb.set_stop_decel(None);
        assert_eq!(fb.stop_decel(), None);

        fb.start(100.0, 10.0, 250.0, 50.0);
        fb.tick(&mut axis, 0.0);
        assert_eq!(axis.last_move_decel, 250.0);
    }

    #[test]
    fn set_stop_decel_rejects_non_positive() {
        let mut fb = MoveToLoad::new();
        fb.set_stop_decel(Some(0.0));
        assert_eq!(fb.stop_decel(), None);
        fb.set_stop_decel(Some(-5.0));
        assert_eq!(fb.stop_decel(), None);
    }

    #[test]
    fn stop_decel_persists_across_start_calls() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();
        fb.set_stop_decel(Some(800.0));

        fb.start(100.0, 10.0, 250.0, 50.0);
        fb.tick(&mut axis, 0.0);
        assert_eq!(axis.last_move_decel, 800.0);

        fb.start(50.0, 10.0, 250.0, 0.0);
        assert_eq!(fb.stop_decel(), Some(800.0), "configuration must outlive a single move");
    }

    #[test]
    fn dead_band_zero_matches_strict_pass_a_behavior() {
        // Regression: dead_band = 0 must reproduce the strict-equality
        // behavior tested in Pass A.
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();

        fb.set_dead_band(0.0);
        fb.start(100.0,10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);

        axis.position = 10.0; fb.tick(&mut axis, 99.99);
        assert_eq!(axis.halt_called, 0, "must not trip at 99.99 with dead_band=0");

        axis.position = 11.0; fb.tick(&mut axis, 100.0);
        assert_eq!(axis.halt_called, 1, "exact equality must trip");
    }

    // ---------------------------------------------------------------------
    // Settle feature
    // ---------------------------------------------------------------------

    #[test]
    fn settle_disabled_skips_settling_state() {
        // Default settle_speed=0 / settle_tolerance=0 means the feature
        // is off: even with a huge load mismatch after halt, we should
        // go Stopping → Idle without visiting Settling.
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();

        fb.start(100.0, 10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);                 // Start → Moving, issues move
        axis.position = 5.0; fb.tick(&mut axis, 100.0); // trigger
        assert_eq!(axis.halt_called, 1);

        // axis finishes decelerating; load overshot to 150 N.
        axis.busy = false;
        fb.tick(&mut axis, 150.0);
        assert!(!fb.is_busy(), "should return to Idle without settling");
        assert_eq!(axis.halt_called, 1, "no second halt from settle");
    }

    #[test]
    fn settle_inside_tolerance_skips_settling_state() {
        // Feature enabled but the post-halt load is already within
        // settle_tolerance — should still skip Settling.
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();

        fb.set_settle(1.0, 5.0);                 // 5 N tolerance
        fb.start(100.0, 10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);
        axis.position = 5.0; fb.tick(&mut axis, 100.0);
        axis.busy = false;
        fb.tick(&mut axis, 102.0);               // overshoot 2 N, within tol
        assert!(!fb.is_busy(), "should go straight Idle");
    }

    #[test]
    fn settle_triggers_corrective_move_on_overshoot() {
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();

        fb.set_settle(1.0, 5.0);                 // slow, 5 N tolerance
        fb.start(100.0, 10.0, 100.0, 50.0);      // moving positive (load increasing)
        fb.tick(&mut axis, 0.0);                 // Start → Moving
        axis.position = 5.0; fb.tick(&mut axis, 100.0); // trigger at exact target
        assert_eq!(axis.halt_called, 1);

        // Overshot to 120 N while decelerating.
        axis.busy = false;
        let settle_calls_before = axis.halt_called;
        fb.tick(&mut axis, 120.0);

        assert!(fb.is_busy(), "should enter Settling");
        assert_eq!(axis.halt_called, settle_calls_before,
            "no halt yet; Settling issues a slow move first");

        // Simulate the axis retreating — load drops back into tolerance.
        axis.position = 4.7; fb.tick(&mut axis, 103.0);
        assert_eq!(axis.halt_called, settle_calls_before + 1,
            "settle converged, halt issued");

        axis.busy = false;
        fb.tick(&mut axis, 103.0);               // StoppingSettle → Idle
        assert!(!fb.is_busy());
        assert_eq!(fb.triggered_load(), 103.0,
            "triggered values re-latched to settled state");
    }

    #[test]
    fn settle_unexpected_stop_is_an_error() {
        // If the axis stops during Settling before the load gets back into
        // tolerance, emit an error — probably hit a physical limit.
        let mut fb = MoveToLoad::new();
        let mut axis = MockAxis::new();

        fb.set_settle(1.0, 5.0);
        fb.start(100.0, 10.0, 100.0, 50.0);
        fb.tick(&mut axis, 0.0);
        axis.position = 5.0; fb.tick(&mut axis, 100.0);
        axis.busy = false;
        fb.tick(&mut axis, 120.0);               // Stopping → Settling

        // Tick once with busy=true so seen_busy latches.
        fb.tick(&mut axis, 118.0);

        // Now axis reports not busy without the load reaching tolerance.
        axis.busy = false;
        fb.tick(&mut axis, 118.0);

        assert!(fb.is_error());
        assert!(!fb.is_busy());
        assert!(fb.error_message().contains("Settle"));
    }

    #[test]
    fn set_settle_clamps_negative_values() {
        let mut fb = MoveToLoad::new();
        fb.set_settle(-5.0, -1.0);
        assert_eq!(fb.settle_speed(), 0.0);
        assert_eq!(fb.settle_tolerance(), 0.0);
    }
}