#define SCALING_FACTOR  128

#include <limits.h>

typedef struct PID_DATA
{
    int16_t lastProcessValue;

    int32_t sumError;

    int16_t P_Factor;

    int16_t I_Factor;

    int16_t D_Factor;

    int16_t maxError;

    int32_t maxSumError;
} pidData_t;

#define MAX_INT         INT_MAX
#define MAX_LONG        LONG_MAX
#define MAX_I_TERM      ( MAX_LONG / 2 )

void pidInit( int16_t, int16_t, int16_t, struct PID_DATA * );
int16_t regulator( int16_t, int16_t, struct PID_DATA * );

void pidInit( int16_t p_factor, int16_t i_factor, int16_t d_factor, struct PID_DATA *pid )
{
    // Start values for PID controller
    pid->sumError = 0;
    pid->lastProcessValue = 0;
    // Tuning constants for PID loop
    pid->P_Factor = p_factor;
    pid->I_Factor = i_factor;
    pid->D_Factor = d_factor;
    // Limits to avoid overflow
    pid->maxError = MAX_INT / (pid->P_Factor + 1);
    pid->maxSumError = MAX_I_TERM / (pid->I_Factor + 1);
}

int16_t regulator( int16_t need, int16_t current, struct PID_DATA *pid )
{
    int32_t pwmOutput;
    int32_t temp;
    int16_t pTerm;
    int16_t iTerm;
    int16_t dTerm;
    int16_t error;

    error = need - current;

    if ( error > pid -> maxError )
    {
        pTerm = MAX_INT;
    }
    else
        if ( error < -pid -> maxError )
        {
            pTerm = -MAX_INT;
        }
        else
        {
            pTerm = pid -> P_Factor * error;
        };

    temp = pid -> sumError + error;
    if ( temp > pid -> maxSumError )
    {
        iTerm = MAX_I_TERM;
        pid -> sumError = pid -> maxSumError;
    }
    else
        if ( temp < -pid -> maxSumError )
        {
            iTerm = -MAX_I_TERM;
            pid -> sumError = -pid -> maxSumError;
        }
        else
        {
            pid -> sumError = temp;
            iTerm = pid -> I_Factor * pid -> sumError;
        };

    dTerm = pid -> D_Factor * ( pid -> lastProcessValue - current );

    pid -> lastProcessValue = current;

    pwmOutput = pTerm + iTerm - dTerm;

    pwmOutput >>= 7;

    if ( pwmOutput < MIN_RM_MOTOR_OUTPUT )
        pwmOutput = MIN_RM_MOTOR_OUTPUT;
    else
        if ( pwmOutput > MAX_RM_MOTOR_OUTPUT )
              pwmOutput = MAX_RM_MOTOR_OUTPUT;

    return (int16_t)pwmOutput;
}