#include "arduinopid.h"
#include <stdbool.h>
#include <stdint.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

static void clampOutput(struct PID *self) {
    if (self->Output > self->outMax) self->Output = self->outMax;
    else if (self->Output < self->outMin) self->Output = self->outMin;
}

static void clampIterm(struct PID *self) {
    if (self->ITerm > self->outMax) self->ITerm = self->outMax;
    else if (self->ITerm < self->outMin) self->ITerm = self->outMin;
}

void PID_Compute(struct PID *self, float Input)
{
    if (!self->ctlMode) return;
    self->Input = Input;

    uint32_t now = xTaskGetTickCount();
    int32_t timeChange = (now - self->lastTime);
    if (timeChange >= self->SampleTime) {
        /*Compute all the working error variables*/
        float error = self->Setpoint - Input;
        self->ITerm += (self->ki * error);

        clampIterm(self);

        float dInput = (Input - self->lastInput);

        /*Compute PID Output*/
        self->Output = self->kp * error + self->ITerm - self->kd * dInput;

        clampOutput(self);

        /*Remember some variables for next time*/
        self->lastInput = Input;
        self->lastTime = now;
    }
}

void PID_SetSetpoint(struct PID *self, float Setpoint)
{
    self->Setpoint = Setpoint;
}

void PID_SetTunings(struct PID *self, float Kp, float Ki, float Kd)
{
    if (Kp < 0 || Ki < 0 || Kd < 0) return;

    float SampleTimeInSec = ((float) self->SampleTime) / 1000;
    self->kp = Kp;
    self->ki = Ki * SampleTimeInSec;
    self->kd = Kd / SampleTimeInSec;

    if (self->controllerDirection == PID_REVERSE) {
        self->kp = -self->kp;
        self->ki = -self->ki;
        self->kd = -self->kd;
    }
}

void PID_SetSampleTime(struct PID *self, uint32_t NewSampleTime)
{
    if (NewSampleTime > 0) {
        float ratio = (float) NewSampleTime
                      / (float) self->SampleTime;
        self->ki *= ratio;
        self->kd /= ratio;
        self->SampleTime = (uint32_t) NewSampleTime;
    }
}

void PID_SetOutputLimits(struct PID *self, float Min, float Max)
{
    if (Min > Max) return;
    self->outMin = Min;
    self->outMax = Max;

    clampOutput(self);
    clampIterm(self);
}

void PID_SetCtlMode(struct PID *self, enum PIDCtlMode Mode)
{
    bool newAuto = (Mode == PID_AUTOMATIC);
    if (newAuto == !self->ctlMode) {  /*we just went from manual to auto*/
        PID_Initialize(self);
    }
    self->ctlMode = newAuto;
}

void PID_Initialize(struct PID *self)
{
    self->lastInput = self->Input;
    self->ITerm = self->Output;

    clampIterm(self);
}

void PID_SetControllerDirection(struct PID *self, enum PIDDirection Direction)
{
    self->controllerDirection = Direction;
}