#include <math.h>

#include "pid.h"

// Ku = 0.10
// Tu = 4.0
// Ziegler–Nichols =>
// Kp = 0.6*0.10 = 0.06
// Ki = 1.2*0.10/4.0 = 0.03
// Kd = 3.0*0.10*4.0/40 = 0.03


const float pid_kp = 0.08;
const float pid_ki = 0.04;
const float pid_kd = 0.035;

const float pid_outMin = 300;
const float pid_outMax = 940;

const float pid_ifMax = 50;

/*! Compute the PID output for the next timestep
 *  \param pid_inst struct that holds the PID controller's configuration
 *  \param input the current input to the PID controller
 *  \return the PID controller output
 */
float pid_compute(pid_cntrl_t *pid_inst, float input)
{
        /*Compute all the working error variables*/
        float error = pid_inst->setpoint - input;
        // measurement is done in 30 RPM increments
        // set a deadband in e=±30RPM
        if ((error > -31) && (error < 31))
                error = 0;
        float output = pid_kp * error;
        float dInput = (input - pid_inst->last_input);
        if ((dInput > -31) && (dInput < 31))
                dInput = 0;
        if (pid_ki * error < -pid_ifMax)
                pid_inst->output_sum += -pid_ifMax;
        else if (pid_ki * error > pid_ifMax)
                pid_inst->output_sum += pid_ifMax;
        else
                pid_inst->output_sum += pid_ki * error;
#ifdef DEBUG_PID
        pid_inst->p_factor = pid_kp * error;
        pid_inst->i_factor = pid_ki * error;
#endif

        if (pid_inst->output_sum > pid_outMax)
                pid_inst->output_sum = pid_outMax;
        else if (pid_inst->output_sum < pid_outMin)
                pid_inst->output_sum = pid_outMin;

        output += pid_inst->output_sum - (pid_kd * dInput);
#ifdef DEBUG_PID
        pid_inst->d_factor = -(pid_kd * dInput);
#endif

        if (output > pid_outMax)
                output = pid_outMax;
        else if (output < pid_outMin)
                output = pid_outMin;

        pid_inst->last_input = input;
        ++pid_inst->id_cnt;

        return output;
}
