autocore-std 3.3.35

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

/// Move To Load function block.
///
/// Moves an axis towards a target load (e.g., from a load cell) and stops
/// as quickly as possible once the edge of that load is reached.
/// 
/// - If `current_load > target_load`, it moves in the negative direction.
/// - If `current_load < target_load`, it moves in the positive direction.
///
/// A hysteresis value (minimum 1.0) is used to handle noise spikes. The axis
/// will halt as soon as `current_load` crosses the `target_load +/- hysteresis` threshold.
///
/// If the axis reaches the `position_limit` before the load is achieved, the block
/// halts the axis and enters an error state.
#[derive(Debug, Clone)]
pub struct MoveToLoad {
    /// Output: operation completed successfully.
    pub done: bool,
    /// Output: block is currently executing motion.
    pub active: bool,
    /// Output: operation failed. Check `state.error_code`.
    pub error: bool,
    /// Internal state machine.
    pub state: StateMachine,
    
    moving_negative: bool,
}

impl Default for MoveToLoad {
    fn default() -> Self {
        Self {
            done: false,
            active: false,
            error: false,
            state: StateMachine::new(),
            moving_negative: false,
        }
    }
}

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

    /// Abort the operation immediately.
    pub fn abort(&mut self, axis: &mut impl AxisHandle) {
        axis.halt();
        self.error = true;
        self.active = false;
        self.state.set_error(1, "Abort called");
        self.state.index = 100;
    }

    /// Execute the function block.
    ///
    /// - `axis`: The handle to the PP axis.
    /// - `execute`: Set to true to start the move. On a falling edge, the axis halts.
    /// - `target_load`: The target load to reach.
    /// - `current_load`: The raw, instantaneous reading from the load cell.
    /// - `position_limit`: The absolute position boundary. If reached before the load, an error occurs.
    /// - `hysteresis`: The acceptable noise band (minimum 1.0) to prevent premature stopping.
    pub fn call(
        &mut self,
        axis: &mut impl AxisHandle,
        execute: bool,
        target_load: f64,
        current_load: f64,
        position_limit: f64,
        hysteresis: f64,
    ) {
        let hyst = hysteresis.max(1.0);

        if !execute {
            if self.active {
                axis.halt();
            }
            self.done = false;
            self.active = false;
            self.error = false;
            self.state.index = 0;
            return;
        }

        // Safety: Check for axis faults
        if axis.is_error() && self.state.index != 100 {
            self.error = true;
            self.active = false;
            self.state.set_error(120, "Axis is in error state");
            self.state.index = 100;
        }

        match self.state.index {
            0 => { // Idle -> Start
                self.done = false;
                self.error = false;
                self.active = true;
                self.state.clear_error();

                self.moving_negative = current_load > target_load;

                // Check if we're already there
                let reached = if self.moving_negative {
                    current_load <= target_load + hyst
                } else {
                    current_load >= target_load - hyst
                };

                if reached {
                    self.done = true;
                    self.active = false;
                    self.state.index = 30;
                } else {
                    // Are we already out of bounds?
                    if (self.moving_negative && axis.position() <= position_limit) ||
                       (!self.moving_negative && axis.position() >= position_limit) {
                        self.error = true;
                        self.active = false;
                        self.state.set_error(110, "Axis already past position limit before starting");
                        self.state.index = 100;
                    } else {
                        let vel = axis.config().jog_speed;
                        let acc = axis.config().jog_accel;
                        let dec = axis.config().jog_decel;
                        axis.move_absolute(position_limit, vel, acc, dec);
                        self.state.index = 10;
                    }
                }
            }
            10 => { // Moving
                let reached = if self.moving_negative {
                    current_load <= target_load + hyst
                } else {
                    current_load >= target_load - hyst
                };

                if reached {
                    axis.halt();
                    self.state.index = 20; // Wait to stop
                } else {
                    // Check limit
                    let hit_limit = if self.moving_negative {
                        axis.position() <= position_limit + 0.0001
                    } else {
                        axis.position() >= position_limit - 0.0001
                    };

                    if hit_limit || !axis.is_busy() {
                        axis.halt();
                        self.error = true;
                        self.active = false;
                        self.state.set_error(150, "Reached position limit without hitting target load");
                        self.state.index = 100;
                    }
                }
            }
            20 => { // Stopping
                if !axis.is_busy() {
                    self.done = true;
                    self.active = false;
                    self.state.index = 30;
                }
            }
            30 => { // Done
                // Wait for execute to drop
            }
            100 => { // Error
                // Wait for execute to drop
            }
            _ => {
                self.state.index = 0;
            }
        }
        
        self.state.call();
    }
}