#include "unitree_motor_serial_driver/unitree_motor_serial_driver.hpp"
#include "rclcpp_components/register_node_macro.hpp"

namespace unitree_motor_serial_driver
{

    MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
        : Node("unitree_motor_serial_driver", options),
          serial_(this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值
    {
        // 声明其他参数
        auto retry_count = this->declare_parameter("serial_retry_count", 5);  // 默认重试5次
        auto retry_delay_ms = this->declare_parameter("serial_retry_delay_ms", 1000);  // 默认延迟1秒
    
        bool connection_success = false;
        int attempts = 0;
    
        // 其余代码保持不变
        timer_ = this->create_wall_timer(
            std::chrono::microseconds(1000), std::bind(&MotorControlNode::control_motor, this));
        data_control_sub_ = this->create_subscription<rc_msgs::msg::DataLeg>(
            "data_leg_control", 10, std::bind(&MotorControlNode::data_control_callback, this, std::placeholders::_1));
        data_motor_pub_ = this->create_publisher<rc_msgs::msg::DataLeg>("data_leg_motor", 10);
    }
        

void MotorControlNode::data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg)
{
    // 将接收到的 DataLeg 消息分别赋值给 motor_t 数组
    for (int i = 0; i < 3; ++i)
    {
        motor[i].cmd.kp = msg->data_control_0.kp;
        motor[i].cmd.kd = msg->data_control_0.kd;
        motor[i].cmd.q = msg->data_control_0.pos * queryGearRatio(MotorType::GO_M8010_6);
        motor[i].cmd.dq = msg->data_control_0.vw * queryGearRatio(MotorType::GO_M8010_6);
        motor[i].cmd.tau = msg->data_control_0.tau;
    }
    timer_count_ = 0;
}

void MotorControlNode::control_motor()
{
    rc_msgs::msg::DataLeg motor_msg;

    for (int i = 0; i < 3; ++i)
    {
        MotorCmd cmd;
        MotorData data;
        cmd.motorType = MotorType::GO_M8010_6;
        data.motorType = MotorType::GO_M8010_6;
        cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
        cmd.id = i;
        cmd.kp = motor[i].cmd.kp;
        cmd.kd = motor[i].cmd.kd;
        cmd.q = motor[i].cmd.q;
        cmd.dq = motor[i].cmd.dq;
        cmd.tau = motor[i].cmd.tau;

        if (timer_count_ > 100)
        {
            Motor_Ctrl_Offline(&motor[i]);
            timer_count_ = 0;
        }
        else
        {
            timer_count_++;
        }

        // 添加异常处理以防止程序崩溃
        try {
            // 优化串口通信，减少阻塞
            if (!serial_.sendRecv(&cmd, &data))
            {
                RCLCPP_WARN(this->get_logger(), "Failed to communicate with motor %d", i);
                continue;
            }

            // 填充 DataLeg 消息
            if (i == 0)
            {
                motor_msg.data_motor_0.tau = data.tau;
                motor_msg.data_motor_0.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
                motor_msg.data_motor_0.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
            }
            else if (i == 1)
            {
                motor_msg.data_motor_1.tau = data.tau;
                motor_msg.data_motor_1.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
                motor_msg.data_motor_1.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
            }
            else if (i == 2)
            {
                motor_msg.data_motor_2.tau = data.tau;
                motor_msg.data_motor_2.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
                motor_msg.data_motor_2.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
            }
        } catch (const std::exception &e) {
            RCLCPP_ERROR(this->get_logger(), 
                "Exception during communication with motor %d: %s", i, e.what());
            
            // 尝试重新初始化串口连接
            try {
                auto serial_port = this->get_parameter("serial_port").as_string();
                RCLCPP_INFO(this->get_logger(), "Attempting to reopen serial port: %s", serial_port.c_str());
                serial_ = SerialPort(serial_port);
                RCLCPP_INFO(this->get_logger(), "Successfully reopened serial port: %s", serial_port.c_str());
            } catch (const std::exception &e2) {
                RCLCPP_ERROR(this->get_logger(), 
                    "Failed to reopen serial port: %s", e2.what());
            }
            
            // 将电机设置为安全状态
            Motor_Ctrl_Offline(&motor[i]);
        }
    }

    // 发布 DataLeg 消息
    data_motor_pub_->publish(motor_msg);
}

void MotorControlNode::Motor_Ctrl_Offline(Motor_t *motor_s)
{
    motor_s->cmd.kp = 0.0;
    motor_s->cmd.kd = 0.01;
    motor_s->cmd.q = 0.0;
    motor_s->cmd.dq = 0.0;
    motor_s->cmd.tau = 0.0;
}

} // namespace unitree_motor_serial_driver

// 注册组件
RCLCPP_COMPONENTS_REGISTER_NODE(unitree_motor_serial_driver::MotorControlNode)