//
// Created by Administrator on 2026/4/22.
//

#ifndef RTTHREAD_SERVO_DRIVER_H
#define RTTHREAD_SERVO_DRIVER_H


#include <rtdevice.h>
#include <rtthread.h>

#include <stdint.h>
#include <string>

class ServoDriver
{
public:
    struct Config
    {
        std::string servo_id;
        std::string pwm_dev_name;
        int pwm_channel;

        rt_uint32_t period_ns;
        rt_uint32_t min_pulse_ns;
        rt_uint32_t max_pulse_ns;

        float min_angle_rad;
        float max_angle_rad;
        float home_angle_rad;
    };

public:
    explicit ServoDriver(const Config& cfg);
    ~ServoDriver();

    rt_err_t init(bool enable_after_init = true, bool go_home = true);

    rt_err_t enable(bool on);
    rt_err_t setAngle(float angle_rad);

    const std::string& id() const;
    const Config& config() const;

private:
    rt_err_t enableNoLock(bool on);
    rt_err_t setAngleNoLock(float angle_rad);

    static float clampf(float v, float lo, float hi);
    static rt_uint32_t angleToPulseNs(const Config& cfg, float angle_rad);

private:
    Config cfg_;
    rt_device_pwm* pwm_dev_;
    rt_uint32_t last_pulse_;
    bool inited_;

    rt_mutex_t mutex_;
};

#endif //RTTHREAD_SERVO_DRIVER_H