#include <stdlib.h>
#include "xc.h"
#include "math.h"
#include "libq.h"
#include "dsp.h"
#include "../CommonCode/types.h"
#include "config.h"


/* Scale and LimitOutput applies a specified scaling to the gains
 * so that the control loop output still spans the entire range of an unsigned short.
 * This comes at a cost of resolution in the PID output.
 * This routine is intended to run after PID(tPID *PIDStruct) and modifies the errhor history
 * and output appropriately based on the actuator limits specified.
 * It would be possible to simply call PID() from within this routine but it is designed to 
 * operate on the results so the user has the option of disabling interrupts to protect the DSP code
 * if desired.
 * 
 * *PID is the PID control structure used by the DSP.
 * *GSAL is the GainScalingandLimit Structure set by the user.  This routine indicates the limiter state in GSAL in case that information is useful for outer loops.
 * LowerLim and UpperLim must be between -32768 and 32767.  
 * The function output will be a value between the limits and the PID integral windup
 * will be taken care of.
 * 
 * A scale factor of 0 and a LowerLim greater than UpperLim will cause the function
 * to return the raw PID output, unscaled and unlimited.
 */

fractional ScaleGainsAndLimitOutput(tPID *PID, tGSAL *GSAL)  
{

    fractional ControllerOutput;

    //Apply gain scaling.  ScaledControllerOutput can still span -32768->32767 after this step.  
#ifdef ACK
    if(GSAL->GainScaleFactor > 1)
    {
      if (PID->controlOutput >= 32767 / GSAL->GainScaleFactor) {
        PID->controlOutput = 32767 / GSAL->GainScaleFactor;
        ControllerOutput = 32767;
      } else if (PID->controlOutput <= -32768 / GSAL->GainScaleFactor) {
        PID->controlOutput = -32768 / GSAL->GainScaleFactor;
        ControllerOutput = -32768;
      } else
        ControllerOutput = PID->controlOutput*GSAL->GainScaleFactor;
    }
    else
        ControllerOutput = PID->controlOutput;    
#else
    if(GSAL->GainScaleFactor > 0)
    {
        int upper_lim;
        int lower_lim;
        upper_lim = _Q15shr(32767,GSAL->GainScaleFactor);
        lower_lim = _Q15neg(upper_lim);
      if (PID->controlOutput >= upper_lim) {
        PID->controlOutput = upper_lim;
        ControllerOutput = 32767;
      } else if (PID->controlOutput <= lower_lim) {
        PID->controlOutput = lower_lim;
        ControllerOutput = -32768;
      } else
        ControllerOutput = _Q15shl(PID->controlOutput,GSAL->GainScaleFactor);  //Guaranteed not to overflow.
    }
    else
        ControllerOutput = PID->controlOutput;
#endif

    //Apply limits to the ControllerOutput and Update the PID structures controlOutput appropriately
    if(ControllerOutput < GSAL->LowerActuatorLimit)  //Actuator will be saturated on the lower end
    {
        GSAL->LowerLimitActive = 1;
        PIDInit(PID);  //Clear error history so Loop acts as a position-mode proportional only loop next time around
        ControllerOutput = GSAL->LowerActuatorLimit;
        if(GSAL->GainScaleFactor > 0)
          PID->controlOutput = _Q15shr(GSAL->LowerActuatorLimit,GSAL->GainScaleFactor);
        else
            PID->controlOutput = GSAL->LowerActuatorLimit;
        
    }
    else
            GSAL->LowerLimitActive = 0;

 
       if(ControllerOutput > GSAL->UpperActuatorLimit)  //Actuator will be saturated on the higher end
    {
        GSAL->UpperLimitActive = 1;
        PIDInit(PID);  //Clear error history so Loop acts as a position-mode proportional only loop next time around
        ControllerOutput = GSAL->UpperActuatorLimit;
        if(GSAL->GainScaleFactor > 0)
          PID->controlOutput = _Q15shr(GSAL->UpperActuatorLimit,GSAL->GainScaleFactor);
        else
          PID->controlOutput = GSAL->UpperActuatorLimit;
    }             
       else
        GSAL->UpperLimitActive = 0;
    

    return ControllerOutput;
}