#include "PI_Regler.h"
#include <stdlib.h>


/*! \brief PID control algorithm.
 *
 *  Calculates output from setpoint, process value and PID status.
 *
 *  \param setPoint  Desired value.
 *  \param processValue  Measured value.
 *  \param pid_st  PID status struct.
 */
int16_t pid_Controller(int16_t sollwert, int16_t istwert, struct regler_werte *pid_st)
{
  int16_t error, p_term, d_term;
  int32_t i_term, ret, temp;

  error = sollwert - istwert;

  // Calculate Pterm and limit error overflow
  if (error > pid_st->maxError){
    p_term = MAX_INT;
  }
  else if (error < -pid_st->maxError){
    p_term = -MAX_INT;
  }
  else{
    p_term = pid_st->P_Factor * error;
  }

  
  // Anti-Wind-Up: sum up esum only if last output value was less than ANTI_WIND_UP
  if (abs(pid_st->ausgabewert_alt) < ANTI_WIND_UP)
  {
	  temp = pid_st->esum + error;	// sum up esum
  }
  else
  {
	  temp = pid_st->esum;	// keep old value for esum
  }
  
  // Calculate Iterm and limit integral runaway
  if(temp > pid_st->maxSumError)
  {
	  i_term = MAX_I_TERM;
	  pid_st->esum = pid_st->maxSumError;
  }
  else if(temp < -pid_st->maxSumError)
  {
	  i_term = -MAX_I_TERM;
	  pid_st->esum = -pid_st->maxSumError;
  }
  else
  {
	  pid_st->esum = temp;
	  i_term = pid_st->I_Factor * pid_st->esum;
  }



  // Calculate Dterm
  d_term = pid_st->D_Factor * (pid_st->istwert_alt - istwert);

  pid_st->istwert_alt = istwert;

  ret = (p_term + i_term + d_term) / SCALING_FACTOR;
  if(ret > MAX_INT){
    ret = MAX_INT;
  }
  else if(ret < -MAX_INT){
    ret = -MAX_INT;
  }
	
	
	if (ret > 255)			ret = 255;
	else if (ret < -255)	ret = -255;
	
	
	// save output value
	pid_st->ausgabewert_alt = (int16_t)ret;
	
  return((int16_t)ret);
}


/*! \brief Initialisation of PID controller parameters.
 *
 *  Initialise the variables used by the PID algorithm.
 *
 *  \param p_factor  Proportional term.
 *  \param i_factor  Integral term.
 *  \param d_factor  Derivate term.
 *  \param pid  Struct with PID status.
 */
void pid_Init(int16_t p_factor, int16_t i_factor, int16_t d_factor, struct regler_werte *pid_st)
// Set up PID controller parameters
{
	// Start values for PID controller
	pid_st->esum = 0;
	pid_st->ausgabewert_alt = 0;
	// Tuning constants for pid loop
	pid_st->P_Factor = p_factor;
	pid_st->I_Factor = i_factor;
	pid_st->D_Factor = d_factor;
	// Limits to avoid overflow
	pid_st->maxError = MAX_INT / (pid_st->P_Factor + 1);
	pid_st->maxSumError = MAX_I_TERM / (pid_st->I_Factor + 1);
}




/*! \brief Resets the integrator.
 *
 *  Calling this function will reset the integrator in the PID regulator.
 */
void pid_Reset_Integrator(pidData_t *pid_st)
{
  pid_st->esum = 0;
}