autocore_std/motion/

move_to_load.rs

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

/// 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,
    /// 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,
            seen_busy:          false,
            triggered_position: f64::NAN,
            triggered_load:     f64::NAN,
            dead_band:          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, position_limit: f64) {
        self.state.clear_error();
        self.target_load        = target_load;
        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
    }

    /// 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 cfg = axis.config();
                    axis.move_absolute(self.position_limit, cfg.jog_speed, cfg.jog_accel, cfg.jog_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();
                    self.state.set_error(150, "Reached position limit without hitting target load");
                    self.state.index = MtlState::Idle as i32;
                }
            }
            Some(MtlState::Stopping) => {
                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::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,
    }

    impl MockAxis {
        fn new() -> Self {
            let mut cfg = AxisConfig::new(1000);
            cfg.jog_speed = 10.0;
            cfg.jog_accel = 100.0;
            cfg.jog_decel = 100.0;
            Self {
                position: 0.0, busy: false, error: false, config: cfg,
                halt_called: 0, last_move_target: 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, _: f64, _: f64) {
            self.last_move_target = p;
            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, 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, 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, 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, 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, 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, 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, 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, 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, 50.0);
        fb.tick(&mut axis, 100.0);          // already at target → triggered
        assert_eq!(fb.triggered_load(), 100.0);

        fb.start(50.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, 50.0);
        assert_eq!(fb.dead_band(), 3.0);
        fb.start(50.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, 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, 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, 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);
    }

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