// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com

#include "Motor/BrushedMotor.hpp"
#include "SavedPidRegulator.hpp"
#include "esp_log.h"


class PIDController : public SavedPidRegulator {
public:
    PIDController(float p, float i, float d, BrushedMotor *rotor,
                  const char *name = "PID", uint16_t dt = 10, const bool isEnabled = false)
            : SavedPidRegulator(p, i, d, name, dt) {
        assert(rotor != nullptr);
        _rotor = rotor;
        _isEnabled = isEnabled;
        setLimits(-_rotor->maxDuty(), _rotor->maxDuty());
        setDirection(REVERSE);
        _pid_timer = millis();
        ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", name, Kp, Ki, Kd);
    }

    void enable() {
        _isEnabled = true;
        _rotor->setDuty(0);
        integral = 0;
    }

    void disable() {
        _isEnabled = false;
        _rotor->setDuty(0);
    }

    void update() {
        if (_isEnabled) {
            //ESP_LOGI(_tag, "Setpoint: %f", setpoint);
            //ESP_LOGI(_tag, "Input: %f", input);
            //ESP_LOGI(_tag, "Output: %d", (int16_t) round(getResultNow()));
            setRotorDuty((int16_t) round(getResultNow()));
        }
    }

    void setRotorDuty(int16_t duty) {
        /* Used only for test purposes */
        _rotor->setExtendedDuty(duty);
    }

private:
    //uint16_t _dt;
    BrushedMotor *_rotor = nullptr;
    uint32_t _pid_timer = 0;
    bool _isEnabled = false;
};