//
// Created by Johnathon Slightham on 2025-07-15.
//

// 180 deg servo

#ifndef SERVO1ACTUATOR_H
#define SERVO1ACTUATOR_H

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

#define SERVO_MAX_SPEED_DEG_PER_SEC 180.0f

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

  private:
    static void servo_task(void *args);
    static void write_pwm(float angle);

    std::atomic<float> m_target{90.0f};
    float m_current{90.0f};
    std::atomic<bool> m_running{true};
    TaskHandle_t m_servo_task{nullptr};
};

#endif //SERVO1ACTUATOR_H