#include "pid.h"


static INT32S L_sub(register INT32S src_dst, register INT32S src2)
{
	return src2-src_dst;
}
static INT32S L_deposit_l(register INT16S ssrc)
{
	return (INT32S)(ssrc);
}

static INT16S extract_l(register INT32S lsrc)
{
	return (INT16S)lsrc;
}
static INT32S L_mult(register INT16S sinp1, register INT16S sinp2)
{
	register INT32S laccum;
	laccum=sinp1;
	laccum*=sinp2;
	
	return laccum;
}
static INT32S L_add(register INT32S src_dst, register INT32S src2)
{
	return src_dst+src2;
}


INT16S CaiXinBoPIDController(INT16S DesiredValue,INT16S MeasuredValue,sCaiXinBoPID *pParams)
{
  INT32S ProportionalPortion, IntegralPortion, PIDoutput;
  INT16S InputError;

  /*-------------------------------------------------------------------------------------------------*/
  /* Saturation mode must be set */
  /* InputError = sub(DesiredValue, MeasuredValue); */  /* input error */
  /*-------------------------------------------------------------------------------------------------*/
  /* input error calculation - 16bit range, with and without saturation mode  */
  PIDoutput = L_sub(L_deposit_l(DesiredValue),L_deposit_l(MeasuredValue)); /* input error - 32bit range */
  if(PIDoutput > MAX_16)               /* inpur error is greater than 0x00007fff = 32767  - 32bit range */
    InputError = MAX_16;               /* input error = max. positive 16 bit signed value */
  else
    if(PIDoutput < MIN_16)             /* input error is less than 0xffff7fff = -32768    - 32bit range */
      InputError = MIN_16;             /* input error = min. negative 16 bit signed value */
    else
      InputError = extract_l(PIDoutput); /* input error - 16bit range */

  /*-------------------------------------------------------------------------------------------------*/
  /* proportional portion calculation */
  ProportionalPortion = L_mult((pParams -> ProportionalGain), InputError) >> (pParams -> ProportionalGainScale + 1);

  /*-------------------------------------------------------------------------------------------------*/
  /* integral portion calculation */
  IntegralPortion = L_mult((pParams -> IntegralGain), InputError) >> (pParams -> IntegralGainScale + 1);

  /* integral portion in step k + integral portion in step k-1 */
  IntegralPortion = L_add(IntegralPortion, L_deposit_l(pParams -> IntegralPortionK_1));

  /* integral portion limitation */
  if(IntegralPortion > pParams -> PositivePIDLimit)
    pParams -> IntegralPortionK_1 = pParams -> PositivePIDLimit;
  else
    if(IntegralPortion < pParams -> NegativePIDLimit)
      pParams -> IntegralPortionK_1 =  pParams -> NegativePIDLimit;
    else
      pParams -> IntegralPortionK_1 =  extract_l(IntegralPortion);

  /*-------------------------------------------------------------------------------------------------*/
  /* derivative portion calculation */
  PIDoutput = L_sub(L_deposit_l(InputError),L_deposit_l(pParams -> InputErrorK_1)); /* [e(k) - e(k-1)] - 32bit range */
  pParams -> InputErrorK_1 = InputError; /* e(k-1) = e(k) */
  if(PIDoutput > MAX_16)               /* [e(k) - e(k-1)] is greater than 0x00007fff = 32767  - 32bit range */
    InputError = MAX_16;               /* [e(k) - e(k-1)] = max. positive 16 bit signed value - 16 bit range */
  else
    if(PIDoutput < MIN_16)             /* [e(k) - e(k-1)] is less than 0xffff7fff = -32768    - 32bit range */
      InputError = MIN_16;             /* [e(k) - e(k-1)] = min. negative 16 bit signed value - 16 bit range */
    else
      InputError = extract_l(PIDoutput); /* [e(k) - e(k-1)]    - 16bit range */

  /* drivative portion in step k - integer */
  PIDoutput = L_mult((pParams -> DerivativeGain), InputError) >> (pParams -> DerivativeGainScale + 1);

  /*-------------------------------------------------------------------------------------------------*/
  /* controller output calculation */
  PIDoutput = L_add(PIDoutput, ProportionalPortion); /* derivative portion + proportional portion */
  PIDoutput = L_add(PIDoutput, L_deposit_l(pParams -> IntegralPortionK_1)); /* + integral portion = controller output */

  /* controller output limitation */
  if(PIDoutput > pParams -> PositivePIDLimit)
    PIDoutput = pParams -> PositivePIDLimit;
  else
    if(PIDoutput < pParams -> NegativePIDLimit)
      PIDoutput =  pParams -> NegativePIDLimit;

  /*-------------------------------------------------------------------------------------------------*/

  return (extract_l(PIDoutput));  /* controller output with limitation */
}