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

#ifndef SERVO_MANAGER_H
#define SERVO_MANAGER_H

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

#include "servo_manager/include/servo_control.h"
#include "servo_manager/include/servo_config.h"

#include <rtthread.h>

class ServoManager
{
public:
    explicit ServoManager(const std::vector<ServoConfig>& cfg);
    explicit ServoManager(const std::string& json_file_path);
    ~ServoManager();

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

    ServoControl* get(size_t index);
    const ServoControl* get(size_t index) const;
    ServoControl* get(const std::string& servo_id);
    const ServoControl* get(const std::string& servo_id) const;

    size_t count() const;

    rt_err_t start();
    void stop();

    void setUpdatePeriodMs(rt_uint32_t update_period_ms);
    void wake();

private:
    static void threadEntry(void* parameter);
    void run();

    int findIndexByIdNoLock(const std::string& servo_id) const;

private:
    std::vector<ServoControl> controls_;

    rt_mutex_t mutex_;
    rt_sem_t wake_sem_;
    rt_thread_t thread_;

    rt_uint32_t update_period_ms_{10};
    volatile bool running_;
};

#endif // SERVO_MANAGER_H