#include "Utils.h"

extern int LOG;

float PID (int aciStatus, struct tPID *PID, float acrGS,
		   float acrX, float acrZ, float acrZ0, float acrDyExtra,
		   float acrYmin, float acrYmax,
		   float acrRatMin, float acrRatMax, float acrY0)
{
	//Local declarations
    float f, dy, y, dy1, dy2;

	//Initialise
    if (aciStatus == 0)
	{
		PID->Work[0] = acrX;
		PID->Work[1] = acrZ;
		PID->Work[3] = acrY0;
		if (PID->Kd > 0)
		{
			PID->Work[2] = acrX;
			PID->Work[4] = 0.0;
		}
	}
    if (aciStatus == 0 || PID->iGainsChanged)
	{
		if (PID->Kd > 0)
		{
			f = PID->DT + 2*(PID->Td);
			PID->Den = (2*(PID->Td) - PID->DT)/f;
			PID->Num[0] = PID->Ki*PID->DT/2.0F + (PID->Kp) + 2*(PID->Kd)/f;
			PID->Num[1] = (PID->Ki*PID->DT*PID->DT - 4*(PID->Kd + PID->Kp*PID->Td)) / f;
			PID->Num[2] = ((PID->Ki*PID->DT/2.0F - PID->Kp)*(PID->DT - 2*PID->Td) + 2*PID->Kd) / f;
		}
		else
		{
			PID->Num[0] = PID->Ki*PID->DT/2.0F + PID->Kp;
			PID->Num[1] = PID->Ki*PID->DT/2.0F - PID->Kp;
		}
		PID->Num2[0] = PID->Ki2*PID->DT/2.0F + PID->Kp2;
		PID->Num2[1] = PID->Ki2*PID->DT/2.0F - PID->Kp2;
	}

	//Discretised PI(D) without integrator
	dy1 = acrGS*(PID->Num[0]*acrX + PID->Num[1]*PID->Work[0]);
	if (PID->Kd > 0) dy1 += acrGS*PID->Num[2]*PID->Work[2] + PID->Den*PID->Work[4];
	
	//2nd input term
	dy2 = acrGS*(PID->Num2[0]*acrZ + PID->Num2[1]*PID->Work[1]);

	//Any set point variation only comes through in the integral term
	dy2 -= acrGS*(PID->Ki*0.0F + PID->Ki2*acrZ0)*PID->DT;

	//Extra contributions?
	dy1 += acrDyExtra;

		dy = dy1 + dy2;
		if (!PID->iNoRateLimits)
		{	dy = MAX(acrRatMin*PID->DT, dy);
			dy = MIN(acrRatMax*PID->DT, dy);
		}
	
	//Integrator
    y = PID->Work[3] + dy;

	//Limits
	if (!PID->iNoLimits)
		y = MAX(acrYmin, MIN(acrYmax, y));

	//Update
	if (PID->Kd > 0)
	{
		PID->Work[2] = PID->Work[0];
        PID->Work[4] = y - PID->Work[3];
	}
    PID->Work[0] = acrX;
	PID->Work[1] = acrZ;
    PID->Work[3] = y;

	//Save data for logging
	PID->dy = dy;
	PID->dy1 = dy1;
	PID->dy2 = dy2;

    return(y);
}


float PID_ChangeOutput(struct tPID *PID, float NewOutput)
{
	float OldOutput;
	OldOutput = PID->Work[3];
	if (PID->Kd > 0) PID->Work[4] += NewOutput - OldOutput;
	PID->Work[3] = NewOutput;
	return(NewOutput);
}