#pragma once
#include "Arduino.h"

class PIDController {
private:
    double kp;
    double ki;
    double kd;
    
    double input;
    double output;
    double setpoint;
    
    unsigned long lastTime;
    double outputSum;
    double lastInput;
    
    double outMin;
    double outMax;
    int sampleTime;
    bool inAuto;

public:
    PIDController(double Kp, double Ki, double Kd) :
        kp(Kp),
        ki(Ki),
        kd(Kd),
        input(0),
        output(0),
        setpoint(0),
        lastTime(0),
        outputSum(0),
        lastInput(0),
        outMin(0),
        outMax(255),
        sampleTime(100),
        inAuto(false)
    {
    }
    
    bool Compute() {
        if(!inAuto) return false;
        
        unsigned long now = millis();
        unsigned long timeChange = (now - lastTime);
        if(timeChange >= sampleTime) {
            double error = setpoint - input;
            
            outputSum += (ki * error);
            if(outputSum > outMax) outputSum = outMax;
            else if(outputSum < outMin) outputSum = outMin;
            
            double dInput = (input - lastInput);
            
            output = kp * error + outputSum - kd * dInput;
            
            if(output > outMax) output = outMax;
            else if(output < outMin) output = outMin;
            
            lastInput = input;
            lastTime = now;
            
            return true;
        }
        return false;
    }
    
    void SetTunings(double Kp, double Ki, double Kd) {
        if (Kp < 0 || Ki < 0 || Kd < 0) return;
        
        double SampleTimeInSec = ((double)sampleTime)/1000;
        kp = Kp;
        ki = Ki * SampleTimeInSec;
        kd = Kd / SampleTimeInSec;
    }
    
    void SetSampleTime(int NewSampleTime) {
        if (NewSampleTime > 0) {
            double ratio = (double)NewSampleTime / (double)sampleTime;
            ki *= ratio;
            kd /= ratio;
            sampleTime = (unsigned long)NewSampleTime;
        }
    }
    
    void SetOutputLimits(double Min, double Max) {
        if(Min >= Max) return;
        outMin = Min;
        outMax = Max;
        
        if(inAuto) {
            if(output > outMax) output = outMax;
            else if(output < outMin) output = outMin;
            
            if(outputSum > outMax) outputSum = outMax;
            else if(outputSum < outMin) outputSum = outMin;
        }
    }
    
    void SetMode(bool Mode) {
        bool newAuto = Mode;
        if(newAuto && !inAuto) {
            Initialize();
        }
        inAuto = newAuto;
    }
    
    void Initialize() {
        outputSum = output;
        lastInput = input;
        if(outputSum > outMax) outputSum = outMax;
        else if(outputSum < outMin) outputSum = outMin;
    }

    // Методы для установки и получения значений
    void SetInput(double val) { input = val; }
    void SetSetpoint(double val) { setpoint = val; }
    double GetOutput() const { return output; }
};