#ifndef UNITREE_MOTOR_SERIAL_DRIVER_HPP
#define UNITREE_MOTOR_SERIAL_DRIVER_HPP

#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "serialPort/SerialPort.h"
#include "unitreeMotor/unitreeMotor.h"
#include "rc_msgs/msg/motor_cmds.hpp"
#include "rc_msgs/msg/data_motors.hpp"

namespace unitree_motor_serial_driver
{

struct Motor_t
{
    MotorCmd cmd;
    MotorData data;
};

Motor_t motor[3]; // 修改为数组以支持多个电机

class MotorControlNode : public rclcpp::Node
{
public:
    explicit MotorControlNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
    
private:
    void control_motor();
    void motor_cmd_callback(const rc_msgs::msg::MotorCmds::SharedPtr msg);
    void Motor_Ctrl_Offline(Motor_t *motor_s);
    SerialPort serial_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmd_sub_;
    rclcpp::Publisher<rc_msgs::msg::DataMotors>::SharedPtr data_motor_pub_;
    int timer_count_ = 0;
};

}

#endif