#include <modules/my_math/matrix/math.hpp>
#include <stdint.h>
#define AC_PID_Basic_FILT_E_HZ_DEFAULT 20.0f   // default input filter frequency
#define AC_PID_Basic_FILT_D_HZ_DEFAULT 10.0f   // default input filter frequency
#define M_2PI         (M_PI * 2)
#define FLT_EPSILON 1.19209289550781250000000000000000000e-7F
inline float constrain_float(float  amt, float  low, float high)
{
    // the check for NaN as a float prevents propagation of floating point
    // errors through any function that uses constrain_value(). The normal
    // float semantics already handle -Inf and +Inf



    if (amt < low) {
        return low;
    }

    if (amt > high) {
        return high;
    }

    return amt;
}

inline bool is_zero(const float x) {
    return fabs(x) < FLT_EPSILON;
}
inline bool is_positive(const float fVal1) {
    return (fVal1 >= FLT_EPSILON);
}

/*
 * @brief: Check whether a double is less than zero
 */
inline bool is_negative(const float fVal1) {
    return (fVal1 <= (-1.0 * FLT_EPSILON));
}


class AC_PID_Basic {
public:

    // Constructor for PID
    AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz=AC_PID_Basic_FILT_E_HZ_DEFAULT, float initial_filt_D_hz=AC_PID_Basic_FILT_D_HZ_DEFAULT);

    // set target and measured inputs to PID controller and calculate outputs
    // target and error are filtered
    // the derivative is then calculated and filtered
    // the integral is then updated based on the setting of the limit flag
    float update_all(float target, float measurement, float dt, bool limit = false) ;
    float update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos) ;

    // update the integral
    // if the limit flags are set the integral is only allowed to shrink
    void update_i(float dt, bool limit_neg, bool limit_pos);

    // get results from pid controller
    float get_p() const  { return _error * _kp; }
    float get_i() const  { return _integrator; }
    float get_d() const  { return _derivative * _kd; }
    float get_ff() const  { return _target * _kff; }
    float get_error() const  { return _error; }

    // reset the integrator
    void reset_I();

    // input and D term filter will be reset to the next value provided to set_input()
    void reset_filter() { _reset_filter = true; }


    // get accessors
    float &kP()  { return _kp; }
    float &kI()  { return _ki; }
    float &kD()  { return _kd; }
    float &ff()  { return _kff;}
    float &filt_E_hz()  { return _filt_E_hz; }
    float &filt_D_hz()  { return _filt_D_hz; }
    float imax() const  { return _kimax; }
    float get_filt_E_alpha(float dt) ;
    float get_filt_D_alpha(float dt)  ;

    // set accessors
    void kP(float v) { _kp=v; }
    void kI(float v) { _ki=v; }
    void kD(float v) { _kd=v; }
    void ff(float v) { _kff=v; }
    void imax(float v) { _kimax=fabs(v); }
    void filt_E_hz(float hz) { _filt_E_hz=fabs(hz); }
    void filt_D_hz(float hz) { _filt_D_hz=fabs(hz); }

    // integrator setting functions
    void set_integrator(float target, float measurement, float i);
    void set_integrator(float error, float i);
    void set_integrator(float i);
    float calc_lowpass_alpha_dt(float dt, float cutoff_freq);

    // parameter var table


protected:

    // parameters
    float _kp;
    float _ki;
    float _kd;
    float _kff;
    float _kimax;
    float _filt_E_hz;         // PID error filter frequency in Hz
    float _filt_D_hz;         // PID derivative filter frequency in Hz

    // internal variables
    float _target;      // target value to enable filtering
    float _error;       // error value to enable filtering
    float _derivative;  // last derivative for low-pass filter
    float _integrator;  // integrator value
    bool _reset_filter; // true when input filter should be reset during next call to set_input


};