#include "PID.h"
#include <Arduino.h>

PID::PID(float kp, float ki, float kd) {
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
    this->minOutput = 0;
    this->maxOutput = 255;
    this->lastOutput = 0;
    this->sampleTime = 100; // Default sample time in milliseconds
    this->lastTime = millis();
    this->integral = 0;
    this->lastInput = 0;
}

void PID::setTunings(float kp, float ki, float kd) {
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
}

void PID::setOutputLimits(float min, float max) {
    this->minOutput = min;
    this->maxOutput = max;
}

void PID::setSampleTime(int sampleTime) {
    this->sampleTime = sampleTime;
}

void PID::reset() {
    this->integral = 0;
    this->lastInput = 0;
    this->lastTime = millis();
}

float PID::compute(float setpoint, float input) {
    unsigned long now = millis();
    unsigned long timeChange = now - lastTime;

    if (timeChange >= sampleTime) {
        // Calculate error
        float error = setpoint - input;

        // Proportional term
        float Pout = kp * error;

        // Integral term
        integral += (ki * error * timeChange / 1000.0);
        integral = constrain(integral, minOutput, maxOutput);

        // Derivative term
        float derivative = (input - lastInput) / (timeChange / 1000.0);
        float Dout = kd * derivative;

        // Compute output
        float output = Pout + integral - Dout;
        output = constrain(output, minOutput, maxOutput);

        // Remember some variables for next time
        lastInput = input;
        lastTime = now;

        lastOutput = output

        return output;
    }

    return lastOutput;
}