#include "AC_PID_Basic.h"



// Constructor
AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz)
{
    // load parameter values from eeprom


    _kp=(initial_p);
    _ki=(initial_i);
    _kd=(initial_d);
    _kff=(initial_ff);
    _kimax=(initial_imax);
    _filt_E_hz=(initial_filt_E_hz);
    _filt_D_hz=(initial_filt_D_hz);

    // reset input filter to first value received
    _reset_filter = true;
    _error=0;
}

float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit)
{
    return update_all(target, measurement, dt, (limit && is_negative(_integrator)), (limit && is_positive(_integrator)));
}

//  update_all - 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 AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos)
{
    // don't process inf or NaN
    /*if (!isfinite(target) || isnan(target) ||
        !isfinite(measurement) || isnan(measurement)) {
        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
        return 0.0f;
    }*/

    
    if(target<0)
        target+=2*M_PI;
    if(measurement<0)
        measurement+=2*M_PI;    
    _error=target-measurement;
    _target = target;
    float error_last = _error;
    if(_error>(M_PI+2.0*M_PI/180.0))
        _error=-2*M_PI+_error;
    if(_error<(-M_PI-2.0*M_PI/180.0))
            _error=2*M_PI+_error;
   _error*=180.0/M_PI;
    // reset input filter to value received
    if (_reset_filter) {
        _reset_filter = false;
        error_last=_error;
        //_error = _target - measurement;
        _derivative = 0.0f;
    } else {
        
        _error += get_filt_E_alpha(dt) * (_error - error_last);

        // calculate and filter derivative
        if (is_positive(dt)) {
            float derivative = (_error - error_last) / dt;
            _derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
        }
    }

    // update I term
    update_i(dt, limit_neg, limit_pos);

    const float P_out = _error * _kp;
    const float D_out = _derivative * _kd;

    float out=P_out + _integrator + D_out + _target * _kff;
    if(out>45.0f)
        out=45.0f;
    if(out<-45.0f)
        out=-45.0f;    
    return out;
}

//  update_i - update the integral
//  if limit_neg is true, the integral can only increase
//  if limit_pos is true, the integral can only decrease
void AC_PID_Basic::update_i(float dt, bool limit_neg, bool limit_pos)
{
    if (!is_zero(_ki)) {
        // Ensure that integrator can only be reduced if the output is saturated
        if (!((limit_neg && is_negative(_error)) || (limit_pos && is_positive(_error)))) {
            _integrator += ((float)_error * _ki) * dt;
            _integrator = constrain_float(_integrator, -_kimax, _kimax);
        }
    } else {
        _integrator = 0.0f;
    }
}

void AC_PID_Basic::reset_I()
{
    _integrator = 0.0; 

}


// get_filt_T_alpha - get the target filter alpha
float AC_PID_Basic::get_filt_E_alpha(float dt)
{
    return calc_lowpass_alpha_dt(dt, _filt_E_hz);
}

// get_filt_D_alpha - get the derivative filter alpha
float AC_PID_Basic::get_filt_D_alpha(float dt)
{
    return calc_lowpass_alpha_dt(dt, _filt_D_hz);
}

void AC_PID_Basic::set_integrator(float target, float measurement, float i)
{
    set_integrator(target - measurement, i);
}

void AC_PID_Basic::set_integrator(float error, float i)
{
    set_integrator(i - error * _kp);
}

void AC_PID_Basic::set_integrator(float i)
{
    _integrator = constrain_float(i, -_kimax, _kimax);

}

float AC_PID_Basic::calc_lowpass_alpha_dt(float dt, float cutoff_freq)
{
    if (is_negative(dt) || is_negative(cutoff_freq)) {

        return 1.0;
    }
    if (is_zero(cutoff_freq)) {
        return 0.0;
    }
    if (is_zero(dt)) {
        return 0.0;
    }
    float rc = 1.0f / (M_2PI * cutoff_freq);
    return dt / (dt + rc);
}