// DC motor with quatrature hall encoder

#ifndef DCMOTORACTUATOR_H
#define DCMOTORACTUATOR_H

#include "IActuator.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <atomic>

class DCMotorActuator final : public IActuator {
  public:
    DCMotorActuator();
    ~DCMotorActuator() override;
    void actuate(uint8_t *cmd) override;
    std::vector<Flatbuffers::sensor_value> get_sensor_data() override;

  private:
    void setup_encoder();
    static void pid_task(void *args);

    volatile int32_t m_encoder_ticks{0};

    double m_current_angle{0.0};
    std::atomic<int64_t> m_target_angle{0};
    TaskHandle_t m_pid_task{nullptr};

    double m_integral{0.0};
    double m_last_error{0.0};
};

#endif //DCMOTORACTUATOR_H