autocore-std 3.3.35

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

/// Configuration for the Pressure Control function block.
#[derive(Debug, Clone)]
pub struct PressureControlConfig {
    /// Proportional gain (Kp).
    pub kp: f64,
    /// Integral gain (Ki).
    pub ki: f64,
    /// Derivative gain (Kd).
    pub kd: f64,
    /// Feed forward value added directly to the output.
    pub feed_forward: f64,
    /// Maximum allowed position delta (in user units) per call/tick.
    /// Critical for safety to prevent crushing the load cell.
    /// E.g., 0.001 inches per ms tick.
    pub max_step: f64,
    /// Maximum accumulated integral windup.
    pub max_integral: f64,
    /// Exponential Moving Average filter coefficient for the load cell (0.0 to 1.0).
    /// 1.0 = No filtering (raw data).
    /// 0.1 = Heavy filtering.
    pub filter_alpha: f64,
    /// If true, a positive error (need more pressure) results in a *negative* move.
    /// Set to true if moving the axis down (negative) increases compression.
    pub invert_direction: bool,
    /// The acceptable load error window to be considered "in tolerance" (e.g., +/- 2.0 lbs).
    pub tolerance: f64,
    /// How long the load must remain within `tolerance` before reporting `in_tolerance = true`.
    pub settling_time: f64,
}

impl Default for PressureControlConfig {
    fn default() -> Self {
        Self {
            kp: 0.0,
            ki: 0.0,
            kd: 0.0,
            feed_forward: 0.0,
            max_step: 0.005, // Conservative 5 thousandths max step
            max_integral: 100.0,
            filter_alpha: 0.5,
            invert_direction: false,
            tolerance: 1.0,
            settling_time: 0.1,
        }
    }
}

/// A closed-loop PID pressure/force controller for Profile Position (PP) axes.
///
/// This function block uses an Exponential Moving Average (EMA) filter to smooth
/// incoming load cell data. It calculates a PID output which is clamped to a safe
/// maximum step size and issued as a small, incremental `move_absolute` target
/// to the drive every cycle.
#[derive(Debug, Clone)]
pub struct PressureControl {
    /// True when the block is actively executing and controlling the axis.
    pub active: bool,
    /// True when the current load has been within `config.tolerance` of the target
    /// for at least `config.settling_time` seconds.
    pub in_tolerance: bool,
    /// True if a fault occurred (e.g., axis error). Check `state.error_code`.
    pub error: bool,
    /// Internal state machine for operation sequencing and error reporting.
    pub state: StateMachine,

    // Internal PID state
    integral: f64,
    prev_error: f64,
    filtered_load: f64,
    
    // Internal Setpoint state (to prevent read-modify-write jitter on actual position)
    commanded_position: f64,

    // Tolerance timer
    settling_timer: f64,
    
    // First execution flag
    is_first_run: bool,
}

impl Default for PressureControl {
    fn default() -> Self {
        Self {
            active: false,
            in_tolerance: false,
            error: false,
            state: StateMachine::new(),
            integral: 0.0,
            prev_error: 0.0,
            filtered_load: 0.0,
            commanded_position: 0.0,
            settling_timer: 0.0,
            is_first_run: true,
        }
    }
}

impl PressureControl {
    /// Creates a new, uninitialized PressureControl block.
    pub fn new() -> Self {
        Self::default()
    }

    /// Execute the pressure control loop.
    ///
    /// - `axis`: The handle to the PP axis.
    /// - `execute`: Set to true to engage the controller. On a falling edge, the axis halts.
    /// - `target_load`: The desired pressure/force (e.g., lbs or Newtons).
    /// - `current_load`: The raw, instantaneous reading from the load cell.
    /// - `config`: Tuning and safety parameters.
    /// - `dt`: The cycle time delta in seconds (e.g., 0.001 for 1ms).
    pub fn call(
        &mut self,
        axis: &mut impl AxisHandle,
        execute: bool,
        target_load: f64,
        current_load: f64,
        config: &PressureControlConfig,
        dt: f64,
    ) {
        // Handle abort/halt on falling edge of execute
        if !execute {
            if self.active {
                axis.halt();
                self.active = false;
                self.in_tolerance = false;
                self.state.index = 0; // Idle
            }
            self.is_first_run = true;
            self.integral = 0.0;
            self.prev_error = 0.0;
            self.settling_timer = 0.0;
            self.error = false;
            return;
        }

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

        // 1. Initialize on rising edge
        if self.is_first_run {
            self.filtered_load = current_load; // Seed filter
            self.commanded_position = axis.position(); // Seed trajectory from current reality
            self.is_first_run = false;
            self.active = true;
            self.error = false;
            self.state.index = 10; // Controlling
        }

        // 2. Exponential Moving Average (EMA) Filter
        let alpha = config.filter_alpha.clamp(0.0, 1.0);
        self.filtered_load = (alpha * current_load) + ((1.0 - alpha) * self.filtered_load);

        // 3. PID Calculation
        let error = target_load - self.filtered_load;
        
        // Anti-windup Integral
        self.integral += error * dt;
        self.integral = self.integral.clamp(-config.max_integral, config.max_integral);

        // Derivative (change in error over time)
        let derivative = if dt > 0.0 { (error - self.prev_error) / dt } else { 0.0 };
        self.prev_error = error;

        // Calculate raw output
        let mut output = (config.kp * error) + (config.ki * self.integral) + (config.kd * derivative) + config.feed_forward;

        // 4. Direction & Clamping
        if config.invert_direction {
            output = -output;
        }
        
        // Clamp output to the maximum safe step size
        let step = output.clamp(-config.max_step, config.max_step);

        // 5. Issue PP Move
        // We accumulate the internal commanded_position rather than reading axis.position()
        // every tick. Reading actual position every tick to calculate the next target
        // creates a positive feedback loop of lag that destabilizes the PID.
        self.commanded_position += step;
        
        // We use move_absolute to our internal tracking position rather than move_relative
        // to prevent compounding loss of precision over thousands of micro-moves.
        let vel = axis.config().jog_speed;
        let acc = axis.config().jog_accel;
        let dec = axis.config().jog_decel;
        
        // Only command motion if we actually need to move a non-zero distance.
        if step.abs() > f64::EPSILON {
            // Note: If the axis options include dynamic max/min position limits, 
            // the DriveHandle's internal move_absolute implementation will automatically
            // reject this move if it violates the safety envelope.
            axis.move_absolute(self.commanded_position, vel, acc, dec);
        }

        // 6. Tolerance Window Evaluation
        if error.abs() <= config.tolerance {
            self.settling_timer += dt;
            if self.settling_timer >= config.settling_time {
                self.in_tolerance = true;
            }
        } else {
            self.settling_timer = 0.0;
            self.in_tolerance = false;
        }
    }
}