pid.hpp

#pragma once
 
#include <algorithm>
#include <cmath>
#include <utility>
 
#include "cycle_value.hpp"
 
namespace LibXR
{
using DefaultScalar = LIBXR_DEFAULT_SCALAR;
 
constexpr DefaultScalar PID_SIGMA = 1e-6f;
 
template <typename Scalar = DefaultScalar>
class PID
{
 public:
  struct Param
  {
    Scalar k = 1.0;          
    Scalar p = 0.0;          
    Scalar i = 0.0;          
    Scalar d = 0.0;          
    Scalar i_limit = 0.0;    
    Scalar out_limit = 0.0;  
    bool cycle = false;      
  };
 
  template <typename P>
  PID(P&& p) : param_(std::forward<P>(p))
  {
    Reset();
  }
 
  Scalar Calculate(Scalar sp, Scalar fb, Scalar dt)
  {
    if (!std::isfinite(sp) || !std::isfinite(fb) || !std::isfinite(dt) || dt <= Scalar(0))
    {
      return last_out_;
    }
 
    // Compute error
    const Scalar ERR = param_.cycle ? (CycleValue<Scalar>(sp) - fb) : (sp - fb);
    const Scalar E_K = ERR * param_.k;
 
    // Derivative from feedback change (scaled by k)
    Scalar fb_dot = (fb - last_fb_) / dt;
    if (!std::isfinite(fb_dot))
    {
      fb_dot = Scalar(0);
    }
 
    Scalar fb_d_k = fb_dot * param_.k;
    if (!std::isfinite(fb_d_k))
    {
      fb_d_k = Scalar(0);
    }
 
    // Compute PD
    const Scalar OUTPUT_PD = (E_K * param_.p) - (fb_d_k * param_.d);
 
    // -------------------------------
    // Integrator update: anti-windup + allow unwind
    // Rule: i_limit == 0 disables I (force i_ = 0)
    // -------------------------------
    if (param_.i > PID_SIGMA && param_.i_limit > PID_SIGMA)
    {
      Scalar i_candidate = i_ + E_K * dt;
 
      if (std::isfinite(i_candidate))
      {
        // Clamp integrator state
        i_candidate = std::clamp(i_candidate, -param_.i_limit, param_.i_limit);
 
        bool accept = true;
 
        // Output-limit-aware gating (windup prevention + unwind)
        if (param_.out_limit > PID_SIGMA)
        {
          const Scalar OUT_BEFORE = OUTPUT_PD + (i_ * param_.i) + feed_forward_;
          const Scalar OUT_AFTER = OUTPUT_PD + (i_candidate * param_.i) + feed_forward_;
 
          if (std::isfinite(OUT_BEFORE) && std::isfinite(OUT_AFTER))
          {
            const bool BEFORE_SAT = (std::abs(OUT_BEFORE) > param_.out_limit);
            const bool AFTER_SAT = (std::abs(OUT_AFTER) > param_.out_limit);
 
            if (AFTER_SAT)
            {
              // If saturated (or would be), only allow integral update if it reduces
              // saturation magnitude
              // - If not saturated before but would saturate after: reject (prevent
              // windup)
              // - If already saturated: allow only if |out_after| < |out_before| (unwind)
              accept = BEFORE_SAT && (std::abs(OUT_AFTER) < std::abs(OUT_BEFORE));
            }
          }
          else
          {
            accept = false;
          }
        }
 
        if (accept)
        {
          i_ = i_candidate;
        }
      }
    }
    else
    {
      // Disable I
      i_ = Scalar(0);
    }
 
    const Scalar I_OUT = i_ * param_.i;
 
    // Apply output limits
    Scalar output = OUTPUT_PD + I_OUT + feed_forward_;
    if (std::isfinite(output) && param_.out_limit > PID_SIGMA)
    {
      output = std::clamp(output, -param_.out_limit, param_.out_limit);
    }
 
    // Store states
    last_err_ = ERR;
    last_fb_ = fb;  // store raw feedback
    last_out_ = output;
    last_der_ = fb_d_k;  // store scaled derivative: k * d(fb)/dt
    return output;
  }
 
  Scalar Calculate(Scalar sp, Scalar fb, Scalar fb_dot, Scalar dt)
  {
    if (!std::isfinite(sp) || !std::isfinite(fb) || !std::isfinite(fb_dot) ||
        !std::isfinite(dt) || dt <= Scalar(0))
    {
      return last_out_;
    }
 
    // Compute error
    const Scalar ERR = param_.cycle ? (CycleValue<Scalar>(sp) - fb) : (sp - fb);
    const Scalar E_K = ERR * param_.k;
 
    // Use externally provided derivative (scaled by k)
    Scalar fb_d_k = fb_dot * param_.k;
    if (!std::isfinite(fb_d_k))
    {
      fb_d_k = Scalar(0);
    }
 
    // Compute PD
    const Scalar OUTPUT_PD = (E_K * param_.p) - (fb_d_k * param_.d);
 
    // -------------------------------
    // Integrator update: anti-windup + allow unwind
    // Rule: i_limit == 0 disables I (force i_ = 0)
    // -------------------------------
    if (param_.i > PID_SIGMA && param_.i_limit > PID_SIGMA)
    {
      Scalar i_candidate = i_ + E_K * dt;
 
      if (std::isfinite(i_candidate))
      {
        // Clamp integrator state
        i_candidate = std::clamp(i_candidate, -param_.i_limit, param_.i_limit);
 
        bool accept = true;
 
        if (param_.out_limit > PID_SIGMA)
        {
          const Scalar OUT_BEFORE = OUTPUT_PD + (i_ * param_.i) + feed_forward_;
          const Scalar OUT_AFTER = OUTPUT_PD + (i_candidate * param_.i) + feed_forward_;
 
          if (std::isfinite(OUT_BEFORE) && std::isfinite(OUT_AFTER))
          {
            const bool BEFORE_SAT = (std::abs(OUT_BEFORE) > param_.out_limit);
            const bool AFTER_SAT = (std::abs(OUT_AFTER) > param_.out_limit);
 
            if (AFTER_SAT)
            {
              accept = BEFORE_SAT && (std::abs(OUT_AFTER) < std::abs(OUT_BEFORE));
            }
          }
          else
          {
            accept = false;
          }
        }
 
        if (accept)
        {
          i_ = i_candidate;
        }
      }
    }
    else
    {
      // Disable I
      i_ = Scalar(0);
    }
 
    const Scalar I_OUT = i_ * param_.i;
 
    // Apply output limits
    Scalar output = OUTPUT_PD + I_OUT + feed_forward_;
    if (std::isfinite(output) && param_.out_limit > PID_SIGMA)
    {
      output = std::clamp(output, -param_.out_limit, param_.out_limit);
    }
 
    // Store states
    last_err_ = ERR;
    last_fb_ = fb;  // store raw feedback
    last_out_ = output;
    last_der_ = fb_d_k;  // store scaled derivative: k * d(fb)/dt
    return output;
  }
 
  void SetK(Scalar k) { param_.k = k; }
  void SetP(Scalar p) { param_.p = p; }
  void SetI(Scalar i) { param_.i = i; }
  void SetD(Scalar d) { param_.d = d; }
  void SetILimit(Scalar limit) { param_.i_limit = limit; }
  void SetOutLimit(Scalar limit) { param_.out_limit = limit; }
 
  Scalar K() const { return param_.k; }
  Scalar P() const { return param_.p; }
  Scalar I() const { return param_.i; }
  Scalar D() const { return param_.d; }
  Scalar ILimit() const { return param_.i_limit; }
  Scalar OutLimit() const { return param_.out_limit; }
  Scalar LastError() const { return last_err_; }
  Scalar LastFeedback() const { return last_fb_; }
  Scalar LastOutput() const { return last_out_; }
  Scalar LastDerivative() const { return last_der_; }
 
  void Reset()
  {
    i_ = 0;
    last_err_ = 0;
    last_fb_ = 0;
    last_out_ = 0;
    last_der_ = 0;
  }
 
  void SetIntegralError(Scalar err) { i_ = err; }
 
  Scalar GetIntegralError() const { return i_; }
 
  void SetFeedForward(Scalar feed_forward) { feed_forward_ = feed_forward; }
 
  Scalar GetFeedForward() const { return feed_forward_; }
 
 private:
  Param param_;              
  Scalar i_ = 0;             
  Scalar last_err_ = 0;      
  Scalar last_fb_ = 0;       
  Scalar last_der_ = 0;      
  Scalar last_out_ = 0;      
  Scalar feed_forward_ = 0;  
};
 
}  // namespace LibXR