// -----------------------------------------------------------------------------
/// @file  PID.c
/// @brief Description
// -----------------------------------------------------------------------------
//  Micro-Key bv
//  Industrieweg 28, 9804 TG Noordhorn
//  Postbus 92, 9800 AB Zuidhorn
//  The Netherlands
//  Tel: +31 594 503020
//  Fax: +31 594 505825
//  Email: support@microkey.nl
//  Web: www.microkey.nl
// -----------------------------------------------------------------------------
/// $Revision$
/// $Author$
/// $Date$
//  (c) 2017 Micro-Key bv
// -----------------------------------------------------------------------------

/// @file PID.c
/// @ingroup {group_name}


// -----------------------------------------------------------------------------
// Include files
// -----------------------------------------------------------------------------

#include "PID.h"

#include "Logger.h"

// -----------------------------------------------------------------------------
// Constant and macro definitions
// -----------------------------------------------------------------------------



// -----------------------------------------------------------------------------
// Type definitions
// -----------------------------------------------------------------------------


// -----------------------------------------------------------------------------
// File-scope variables
// -----------------------------------------------------------------------------



// -----------------------------------------------------------------------------
// Function declarations
// -----------------------------------------------------------------------------



// -----------------------------------------------------------------------------
// Function definitions
// -----------------------------------------------------------------------------


ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd, int iMin, int iMax)
{
   ErrorStatus returnValue = SUCCESS;

   if (!self->initialized)
   {

      if ((Kp >= 0) && (Ki >= 0) && (Kd >= 0))
      {
         self->iTerm       = 0;
         self->Kd          = Kd;
         self->Ki          = Ki;
         self->Kp          = Kp;
         self->input_d1    = 0;
         self->iMin        = iMin;
         self->iMax        = iMax;

         self->initialized = true;
      }
      else
      {
         returnValue = ERROR;
      }
   }
   else
   {
      returnValue = ERROR;
   }
   return returnValue;
}


void PID_destruct(struct Pid* self)
{
   self->initialized = false;
}


void PID_reset(struct Pid* self)
{
   if (self->initialized)
   {
      self->iTerm    = 0;
      self->input_d1 = 0;
   }
}


int PID_calculate(struct Pid* self, int error)
{
   int returnValue = 0;

   int dTerm;
   int pTerm;

   if (self->initialized)
   {
      // Calculate integral
      self->iTerm += (self->Ki * error);

      // Control integrator
      if (self->iTerm > self->iMax)
      {
         self->iTerm = self->iMax;
      }
      else if(self->iTerm < self->iMin)
      {
         self->iTerm = self->iMin;
      }

      // Calculate differential
      dTerm = ((error - self->input_d1) * self->Kd);

      // Calculate proportional
      pTerm = (self->Kp * error);

      returnValue = (self->iTerm + dTerm + pTerm) / PID_FIXED_POINT_FACTOR;
      self->input_d1 = error;
   }
   else
   {
      returnValue = 0;
   }

   return returnValue;

}