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

int ScaleOutput(int *ControlOutput, int Scale) {  //Just applies Gain Scaling, doesn't adjust PID->controlOutput, must be done manually if this routine is used.

    int ScaledValue;

    if (*ControlOutput >= 32767 / Scale) {
        *ControlOutput = 32767 / Scale;
        ScaledValue = 32767;
    } else if (*ControlOutput <= -32768 / Scale) {
        *ControlOutput = -32768 / Scale;
        ScaledValue = -32768;
    } else
        ScaledValue = *ControlOutput*Scale;

    return ScaledValue;
}



/* 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;
    
    //Do nothing in trivial cases
    if((GSAL->GainScaleFactor == 0)  || (GSAL->LowerActuatorLimit > GSAL->UpperActuatorLimit))  
        return PID->controlOutput;
    
    //Apply gain scaling.  ScaledControllerOutput can still span -32768->32767 after this step.  
    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;

      
    //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;
        PID->controlOutput = GSAL->LowerActuatorLimit/GSAL->GainScaleFactor;
}
    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;
        PID->controlOutput = GSAL->UpperActuatorLimit/GSAL->GainScaleFactor;
    }             
       else
        GSAL->UpperLimitActive = 0;
    
    return ControllerOutput;
}