//
// Created by Administrator on 2025/12/24.
//

#ifndef RTTHREAD_HEAD_SERVICE_IMPL_H
#define RTTHREAD_HEAD_SERVICE_IMPL_H

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

#include "erpc/proto/generated/servo_service_server.hpp"
#include "servo_manager/include/servo_manager.h"


#include <rtthread.h>


class ServoServiceImpl : public erpcShim::servo_service_interface
{
public:
    ServoServiceImpl();
    ~ServoServiceImpl() override;

    bool setConstraints(const char* id,
                        float max_velocity_rad,
                        float max_acceleration_rad,
                        float max_jerk_rad) override;
    bool setPositionGain(const char* id,
                         float position_gain) override;
    bool setMode(RpcMotionMode mode) override;
    bool setUpdatePeriodMs(uint32_t ms) override;
    int32_t move(const list_ServoCmd_1_t* cmds) override;
    int32_t moveJ(const list_float_1_t* angles_rad) override;

private:
    ServoManager manager_;
    rt_err_t init_err_;
    MotionMode mode_;
    rt_mutex_t mutex_;
};

#endif // RTTHREAD_HEAD_SERVICE_IMPL_H