#include "GyverPID.h"
#include "Motor/BrushedMotor.hpp"
#include "Preferences.h"
#include "esp_log.h"
#include "mString.h"

class PIDController : public GyverPID {
  public:
    PIDController(float p, float i, float d, BrushedMotor *rotor, int rotorNumber = 1, uint16_t dt = 10)
      : GyverPID(p, i, d, dt) {
        assert(rotor != nullptr);
        _rotor = rotor;
        _rotorNumber = rotorNumber;
        setLimits(0, _rotor->maxDuty());
        _preferences.begin((String("r") + String(rotorNumber)).c_str());
        _loadPreferences(p, i, d);
        pid_timer = millis();
        ESP_LOGI(_tag, "PID №%d initialized with params (%f, %f, %f)", _rotorNumber, Kp, Ki, Kd);
    }

    void tick() {
        if (millis() - pid_timer >= _dt) {
            _rotor->setDuty(getResultTimer());
            pid_timer = millis();
        }
    }

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

    void save() {
        _preferences.putFloat("p", Kp);
        _preferences.putFloat("i", Ki);
        _preferences.putFloat("d", Kd);
        ESP_LOGI(_tag, "PID №%d saved with params (%f, %f, %f)", _rotorNumber, Kp, Ki, Kd);
    }

  private:
    Preferences _preferences;
    int _rotorNumber = 1;
    uint16_t _dt;
    BrushedMotor *_rotor = nullptr;
    uint32_t pid_timer = 0;
    static constexpr const char *_tag = "PIDController";

    void _loadPreferences(float pDefault = 0, float iDefault = 0, float dDefault = 0) {
        if (_preferences.isKey("p") and _preferences.isKey("i") and _preferences.isKey("d")) {
            Kp = _preferences.getFloat("p", pDefault);
            Ki = _preferences.getFloat("i", iDefault);
            Kd = _preferences.getFloat("d", dDefault);
        }
    }
};