#include "PID.h"
#include "TC.h"

#include <ArduinoLog.h>

#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif

/**********************************************************
 *    The parameters specified here are those for for which we can't set up
 *    reliable defaults, so we need to have the user set them.
 ***************************************************************************/
PIDModel_S1::PIDModel_S1(
    Component *owner,
    TC *_tc,
    double *Input,
    double *Output,
    double *Setpoint,
    double Kp,
    double Ki,
    double Kd) : Component("PID", 0, Component::COMPONENT_DEFAULT, owner),
                 tc(_tc)
{
   myOutput = Output;
   myInput = Input;
   mySetpoint = Setpoint;
   inAuto = false;
   setOutputLimits(0, 255); // default output limit corresponds to
   SampleTime = 500;        // default Controller Sample Time is 0.1 seconds
   setControllerDirection(0);
   setTunings(Kp, Ki, Kd);
   lastTime = now - SampleTime;
}

/***********************************************************************
 *     This, as they say, is where the magic happens.  this function should be called
 *   every time "void loop()" executes.  the function will decide for itself whether a new
 *   pid Output needs to be computed.  returns true when the output is computed,
 *   false when nothing has been done.
 **********************************************************************************/
bool PIDModel_S1::compute()
{
   if (!inAuto)
      return false;

   unsigned long timeChange = (now - lastTime);

   if (timeChange >= SampleTime)
   {
      /*compute all the working error variables*/
      double input = *myInput;
      double error = *mySetpoint - input;

      ITerm += (ki * error);

      if (ITerm > outMax)
         ITerm = outMax;
      else if (ITerm < outMin)
         ITerm = outMin;

      double dInput = (input - lastInput);

      /*compute PID Output*/
      double output = kp * error + ITerm - kd * dInput;

      if (output > outMax)
         output = outMax;
      else if (output < outMin)
         output = outMin;

      *myOutput = output;

      /*Remember some variables for next time*/

      lastInput = input;
      lastTime = now;
      return true;
   }
   else
      return false;
}

/* setTunings(...)*************************************************************
 * This function allows the controller's dynamic performance to be adjusted.
 * it's called automatically from the constructor, but tunings can also
 * be adjusted on the fly during normal operation
 ******************************************************************************/
void PIDModel_S1::setTunings(double Kp, double Ki, double Kd)
{
   if (Kp < 0 || Ki < 0 || Kd < 0)
      return;

   dispKp = Kp;
   dispKi = Ki;
   dispKd = Kd;

   double SampleTimeInSec = ((double)SampleTime) / 1000;
   kp = Kp;
   ki = Ki * SampleTimeInSec;
   kd = Kd / SampleTimeInSec;

   if (controllerDirection == EPM_REVERSE)
   {
      kp = (0 - kp);
      ki = (0 - ki);
      kd = (0 - kd);
   }
}

/* setSampleTime(...) *********************************************************
 * sets the period, in Milliseconds, at which the calculation is performed
 ******************************************************************************/
void PIDModel_S1::setSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio = (double)NewSampleTime / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;
   }
}

/* setOutputLimits(...)****************************************************
 *     This function will be used far more often than SetInputLimits.  while
 *  the input to the controller will generally be in the 0-1023 range (which is
 *  the default already,)  the output will be a little different.  maybe they'll
 *  be doing a time window and will need 0-8000 or something.  or maybe they'll
 *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
 *  here.
 **************************************************************************/
void PIDModel_S1::setOutputLimits(double Min, double Max)
{
   if (Min >= Max)
      return;

   outMin = Min;
   outMax = Max;

   if (inAuto)
   {
      if (*myOutput > outMax)
         *myOutput = outMax;
      else if (*myOutput < outMin)
         *myOutput = outMin;

      if (ITerm > outMax)
         ITerm = outMax;
      else if (ITerm < outMin)
         ITerm = outMin;
   }
}

/* setMode(...)****************************************************************
 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
 * when the transition from manual to auto occurs, the controller is
 * automatically initialized
 ******************************************************************************/
void PIDModel_S1::setMode(int Mode)
{
   bool newAuto = (Mode == EPM_AUTOMATIC);
   if (newAuto == !inAuto)
   { /*we just went from manual to auto*/
      setup();
   }
   inAuto = newAuto;
}

/*****************************************************************
 *	does all the things that need to happen to ensure a bumpless transfer
 *  from manual to automatic mode.
 ******************************************************************************/
short PIDModel_S1::setup()
{
   ITerm = *myOutput;
   lastInput = *myInput;

   if (ITerm > outMax)
      ITerm = outMax;
   else if (ITerm < outMin)
      ITerm = outMin;
   return E_OK;
}

/**************************************************
 * The PID will either be connected to a DIRECT acting process (+Output leads
 * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
 * know which one, because otherwise we may increase the output when we should
 * be decreasing.  This is called from the constructor.
 ******************************************************************************/
void PIDModel_S1::setControllerDirection(int Direction)
{
   if (inAuto && Direction != controllerDirection)
   {
      kp = (0 - kp);
      ki = (0 - ki);
      kd = (0 - kd);
   }
   controllerDirection = Direction;
}

/* Status Functions*************************************************************
 * Just because you set the Kp=-1 doesn't mean it actually happened.  these
 * functions query the internal state of the PID.  they're here for display
 * purposes.  this are the functions the PID Front-end uses for example
 ******************************************************************************/
double PIDModel_S1::getKp() { return dispKp; }
double PIDModel_S1::getKi() { return dispKi; }
double PIDModel_S1::getKd() { return dispKd; }
int PIDModel_S1::getMode() { return inAuto ? EPM_AUTOMATIC : EPM_MANUAL; }
int PIDModel_S1::getDirection() { return controllerDirection; }

/////////////////////////////////////////////////////////////////////
//
//
//
short PIDModel_S1::setValueFromRegister(E_PID_MBRegister reg, int value)
{
   /*
   switch (reg)
   {
   case EPID_REG_CHANNEL:
      channel = value;
      break;
   case EPID_REG_SP:
      _SP = value;
      break;
   case EPID_REG_PV:
      _PV = value;
      break;
   case EPID_REG_KP:
      kp = value;
      break;
   case EPID_REG_KI:
      ki = value;
      break;
   case EPID_REG_KD:
      kd = value;
      break;
   case EPID_REG_MODE:
      setMode(value);
      break;
   case EPID_REG_DIRECTION:
      setControllerDirection(value);
      break;
   case EPID_REG_PID_STATE:
      setPIDState(static_cast<PID_STATE>(value));
      break;
   default:
      return E_NOT_FOUND;
      break;
   }
   */
   return E_OK;
}