#include "PIDAlgo.h"

float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
{
	float error;
	float derivative;
	float output;

	//Calculate P,I,D
	error = _setPoint - _mesuredParam;

	//In case of error too small then stop integration
	if(fabs(error) > params->epsilon)
	{
		*_integral = *_integral + error*params->dt;
	}

	derivative = (error - *_pre_error)/params->dt;

	output = params->Kp*error + params->Ki**_integral + params->Kd*derivative;

	//Saturation Filter
	if(output > params->MAX)
	{
		output = params->MAX;
	}
	else if(output < params->MIN)
	{
		output = params->MIN;
	}

	//Update error
	*_pre_error = error;

	return output;
}

float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
{
    float error;
    float derivative;
    float output;

    //Calculate P,I,D
    error = _setPoint - _mesuredParam;

    //In case of error too small then stop integration
    if(fabs(error) > params->epsilon)
    {
        *_integral = *_integral + error*params->dt;
    }

    derivative = (error - *_pre_error)/params->dt;

    output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + params->Kd*derivative;

    //Saturation Filter
    if(output > params->MAX)
    {
        output = params->MAX;
    }
    else if(output < params->MIN)
    {
        output = params->MIN;
    }

    //Update error
    *_pre_error = error;

    return output;
}

float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
{
    float error;
    float derivative;
    float output;

    //double error = *mySetpoint - input;
    error = _setPoint - _mesuredParam;
    //ITerm+= (ki * error);
    *_integral = *_integral + (error*params->Ki);
    /*  if(ITerm > outMax)
      {
          ITerm= outMax;
      }
      else if(ITerm < outMin)
      {
          ITerm= outMin;
      } */
    if(*_integral > params->MAX)
    {
        *_integral = params->MAX;
    }
    else if(*_integral < params->MIN)
    {
        *_integral = params->MIN;
    }
    //  double dInput = (input - lastInput);
    derivative = error - *_pre_error;

      /*Compute PID Output*/
    //  double output = kp * error + ITerm- kd * dInput;
    output = params->Kp*error/params->ProportionalErrorMultiplier + *_integral/params->IntegralErrorMultiplier + params->Kd*derivative;

    //Saturation Filter
    if(output > params->MAX)
    {
        output = params->MAX;
    }
    else if(output < params->MIN)
    {
        output = params->MIN;
    }

    //Update error
    *_pre_error = error;

    return output;
}