//
// Created by Administrator on 2026/1/7.
//

#ifndef SERVO_MANAGER_H
#define SERVO_MANAGER_H

extern "C" {
#include <rtthread.h>
#include <rtdevice.h>
}

#include <stdint.h>
#include <stddef.h>
#include <vector>

class ServoManager
{
public:
    struct ServoConfig
    {
        const char* servo_id;     // 唯一ID
        const char* pwm_dev_name;
        int         pwm_channel;

        // ns
        rt_uint32_t period_ns;
        rt_uint32_t min_pulse_ns;
        rt_uint32_t max_pulse_ns;

        float       min_angle_deg;
        float       max_angle_deg;
        float       home_angle_deg;
    };

    struct ServoCmd
    {
        const char* id;
        float       angle_deg;
    };

public:

    explicit ServoManager(const std::vector<ServoConfig> &cfg);
    ~ServoManager();

    size_t count() const { return _cfg.size(); }

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

    rt_err_t enable(const char* servo_id, bool on);
    rt_err_t setAngle(const char* servo_id, float angle_deg);

    // 全部设置：两种形态（vector / 指针）
    rt_err_t setAllAngles(const std::vector<float>& angles_deg);
    rt_err_t setAllAngles(const float* angles_deg, size_t n);

    // 批量设置：两种形态（vector / 指针）
    rt_err_t setAngles(const std::vector<ServoCmd>& cmds);
    rt_err_t setAngles(const ServoCmd* cmds, size_t n);

    const char* idAt(size_t order) const;
    int orderOf(const char* servo_id) const;

private:
    rt_err_t enableNoLock(size_t order, bool on);
    rt_err_t setAngleNoLock(size_t order, float angle_deg);

    int findOrderByIdNoLock(const char* servo_id) const;

    static float clampf(float v, float lo, float hi);
    static rt_uint32_t angleToPulseNs(const ServoConfig& c, float angle_deg);

private:
    std::vector<ServoConfig>   _cfg;
    std::vector<rt_device_pwm*> _pwm_dev;
    std::vector<rt_uint32_t>    _last_pulse;

    rt_mutex_t _mutex;
};

#endif // SERVO_MANAGER_H