#define K_P    0.22
#define K_I    0.006
#define K_D    0.001

#define SCALING_FACTOR  128

typedef struct PID_DATA{
  int lastProcessValue;
  long sumError;
  int P_Factor;
  int I_Factor;
  int D_Factor;
  int maxError;
  long maxSumError;
} pidData_t;

#define INT16_MAX    32767
#define INT32_MAX    2147483647

#define MAX_INT    INT16_MAX
#define MAX_LONG    INT32_MAX
#define MAX_I_TERM    (MAX_LONG / 2)

extern struct pidData_t PID_DATA;

void pid_Init(int p_factor, int i_factor, int d_factor, struct PID_DATA *pid);
int pid_Controller(int setPoint, int processValue, struct PID_DATA *pid_st);

#include "pid.h"

void pid_Init(int p_factor, int i_factor, int d_factor, struct PID_DATA *pid)
{
  pid->sumError = 0;
  pid->lastProcessValue = 0;

  pid->P_Factor = p_factor;
  pid->I_Factor = i_factor;
  pid->D_Factor = d_factor;
  
  pid->maxError = MAX_INT / (pid->P_Factor + 1);
  pid->maxSumError = MAX_I_TERM / (pid->I_Factor + 1);
}

int pid_Controller(int setPoint, int processValue, struct PID_DATA *pid_st)
{
  int error, p_term, d_term;
  long i_term, ret, temp;

  error = setPoint - processValue;

  //----- 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;
  }

  //----- Calculate Iterm and limit integral runaway
  temp = pid_st->sumError + error;
  if(temp > pid_st->maxSumError){
    i_term = MAX_I_TERM;
    pid_st->sumError = pid_st->maxSumError;
  }
  else if(temp < -pid_st->maxSumError){
    i_term = -MAX_I_TERM;
    pid_st->sumError = -pid_st->maxSumError;
  }
  else{
    pid_st->sumError = temp;
    i_term = pid_st->I_Factor * pid_st->sumError;
  }

  //----- Calculate Dterm
  d_term = pid_st->D_Factor * (pid_st->lastProcessValue - processValue);

  pid_st->lastProcessValue = processValue;

  ret = (p_term + i_term + d_term) / SCALING_FACTOR;
  if(ret > MAX_INT){
    ret = MAX_INT;
  }
  else if(ret < -MAX_INT){
    ret = -MAX_INT;
  }

  return((int)ret);
}

pid_Init(K_P * SCALING_FACTOR, K_I * SCALING_FACTOR , K_D * SCALING_FACTOR, &pidData);