#include "TC.h"

class PIDController {
    private:
        double kp; // Proportional gain
        double ki; // Integral gain
        double kd; // Derivative gain

        double setpoint; // Desired value
        double input; // Current value

        double output; // Controller output

        double integral; // Integral term
        double previousError; // Previous error for derivative term

        unsigned long lastTime; // Last time the controller was updated

        bool autotuneEnabled; // Flag to enable autotuning
        double autotuneStep; // Step size for autotuning
        double autotuneNoiseBand; // Noise band for autotuning

        double autotuneSetpoint; // Setpoint for autotuning
        double autotuneOutput; // Output for autotuning

        enum AutotuneState {
            AUTOTUNE_INIT,
            AUTOTUNE_STEP_UP,
            AUTOTUNE_STEP_DOWN,
            AUTOTUNE_FINISHED
        };

        AutotuneState autotuneState; // Current state of autotuning

    public:
        PIDController(double kp, double ki, double kd) {
            this->kp = kp;
            this->ki = ki;
            this->kd = kd;

            setpoint = 0.0;
            input = 0.0;
            output = 0.0;

            integral = 0.0;
            previousError = 0.0;

            lastTime = 0;

            autotuneEnabled = false;
            autotuneStep = 0.1;
            autotuneNoiseBand = 0.5;

            autotuneSetpoint = 0.0;
            autotuneOutput = 0.0;

            autotuneState = AUTOTUNE_INIT;
        }

        void setSetpoint(double setpoint) {
            this->setpoint = setpoint;
        }

        void setInput(double input) {
            this->input = input;
        }

        double compute() {
            unsigned long currentTime = millis();
            double deltaTime = (currentTime - lastTime) / 1000.0;

            double error = setpoint - input;

            integral += error * deltaTime;
            double derivative = (error - previousError) / deltaTime;

            output = kp * error + ki * integral + kd * derivative;

            previousError = error;
            lastTime = currentTime;

            if (autotuneEnabled) {
                autotune();
            }

            return output;
        }

        void enableAutotune(double step, double noiseBand) {
            autotuneEnabled = true;
            autotuneStep = step;
            autotuneNoiseBand = noiseBand;

            autotuneState = AUTOTUNE_INIT;
        }

        void disableAutotune() {
            autotuneEnabled = false;
        }

        void autotune() {
            switch (autotuneState) {
                case AUTOTUNE_INIT:
                    autotuneSetpoint = input;
                    autotuneOutput = output;
                    autotuneState = AUTOTUNE_STEP_UP;
                    break;

                case AUTOTUNE_STEP_UP:
                    setSetpoint(autotuneSetpoint + autotuneStep);
                    if (input >= autotuneSetpoint + autotuneNoiseBand) {
                        autotuneState = AUTOTUNE_STEP_DOWN;
                    }
                    break;

                case AUTOTUNE_STEP_DOWN:
                    setSetpoint(autotuneSetpoint - autotuneStep);
                    if (input <= autotuneSetpoint - autotuneNoiseBand) {
                        autotuneState = AUTOTUNE_FINISHED;
                    }
                    break;

                case AUTOTUNE_FINISHED:
                    setSetpoint(autotuneSetpoint);
                    disableAutotune();
                    break;
            }
        }
};