#include "PID.h"

/*test push*/

void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax,
                       float limMinInt, float limMaxInt, float T) {
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->KpDefault = Kp;
    pid->KiDefault = Ki;
    pid->KdDefault = Kd;

    pid->tau = tau;
    pid->limMin = limMin;
    pid->limMax = limMax;
    pid->limMinInt = limMinInt;
    pid->limMaxInt = limMaxInt;
    pid->T = T;

    /* Clear controller variables */
    pid->integrator = 0.0f;
    pid->prevError = 0.0f;

    pid->differentiator = 0.0f;
    pid->prevMeasurement = 0.0f;

    pid->out = 0.0f;

    pid->enable = false;
}

float pidControllerUpdate(PidController *pid, float setpoint, float measurement) {

    /*
    * Error signal
    */
    float error = setpoint - measurement;


    /*
    * Proportional
    */
    pid->proportional = pid->Kp * error;


    /*
    * Integral
    */
    pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);

    /* Anti-wind-up via integrator clamping */
    if (pid->integrator > pid->limMaxInt) {

        pid->integrator = pid->limMaxInt;

    } else if (pid->integrator < pid->limMinInt) {

        pid->integrator = pid->limMinInt;

    }


    /*
    * Derivative (band-limited differentiator)
    */

    pid->differentiator = -(2.0f * pid->Kd * (measurement -
                                              pid->prevMeasurement)    /* Note: derivative on measurement, therefore minus sign in front of equation! */
                            + (2.0f * pid->tau - pid->T) * pid->differentiator)
                          / (2.0f * pid->tau + pid->T);


    /*
    * Compute output and apply limits
    */
    pid->out = pid->proportional + pid->integrator + pid->differentiator;

    if (pid->proportional > pid->limMax) {

        pid->proportional = pid->limMax;

    } else if (pid->proportional < pid->limMin) {

        pid->proportional = pid->limMin;

    }

    if (pid->out > pid->limMax) {

        pid->out = pid->limMax;

    } else if (pid->out < pid->limMin) {

        pid->out = pid->limMin;

    }

    /* Store error and measurement for later use */
    pid->prevError = error;
    pid->prevMeasurement = measurement;

    /* Return controller output */
    return pid->out;
}