#include <thread>
#include <functional>
#include <future>
#include <sstream>
#include <iostream>
#include <chrono>
#include <mutex>
#include <string>
#include <fstream>
#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <dirent.h>
#include <linux/input.h>
#include "arm_keyboard_node.hpp"
#include "gjk_interface.hpp"

rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr ArmKeyboardNode::leftArmAimPub_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr ArmKeyboardNode::rightArmAimPub_;

std::thread inputThread_;
std::mutex armAimMutex_;
int64_t commandId = 0;
using namespace std::placeholders;
using namespace Eigen;

const std::unordered_map<int, int> keyToCommand = {
    {KEY_A, 0}, {KEY_S, 1}, {KEY_D, 2}, {KEY_F, 3}, {KEY_G, 4}, {KEY_H, 5},
    {KEY_Z, 6}, {KEY_X, 7}, {KEY_C, 8}, {KEY_V, 9}, {KEY_B, 10}, {KEY_N, 11},
    {KEY_W, 12}, {KEY_E, 13}, {KEY_R, 14}
};
enum {
    KEEP_ACTION = 0,
    STOP_ACTION = 1,
    NEW_ACTION = 2
};

void SetNonCanonicalMode(bool enable) {
    static struct termios oldt, newt;
    
    if (enable) {
        // 保存当前终端设置
        tcgetattr(STDIN_FILENO, &oldt);
        newt = oldt;
        
        // 禁用缓冲和回显
        newt.c_lflag &= ~(ICANON | ECHO);
        tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    } else {
        // 恢复终端设置
        oldt.c_lflag |= (ICANON | ECHO);
        tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    }
}

std::string to_lower(const std::string& str) {
    std::string res;
    for (char c : str) {
        res += tolower(c);
    }
    return res;
}

// 检查设备名称是否包含键盘关键词
bool is_keyboard_device(const std::string& event_path) {
    // 设备名称文件路径：/sys/class/input/eventX/device/name
    std::string name_path = "/sys/class/input/" + event_path + "/device/name";
    
    // 读取设备名称
    std::ifstream name_file(name_path);
    if (!name_file.is_open()) {
        return false;
    }
    
    std::string name;
    std::getline(name_file, name);
    name_file.close();
    
    // 检查名称是否包含键盘相关关键词
    std::string lower_name = to_lower(name);
    const std::vector<std::string> keywords = {"at translated set 2 keyboard"};
    for (const auto& kw : keywords) {
        if (lower_name.find(kw) != std::string::npos) {
            return true;
        }
    }
    return false;
}

// 查找所有输入设备并筛选出键盘
std::string find_keyboard_devices() {
    const std::string input_dir = "/sys/class/input/";  // 输入设备的sysfs目录
    
    // 打开sysfs输入设备目录
    DIR* dir = opendir(input_dir.c_str());
    if (!dir) {
        std::cerr << "无法打开目录: " << input_dir << std::endl;
        return "";
    }
    
    // 遍历所有event设备（event0, event1, ...）
    dirent* entry;
    while ((entry = readdir(dir)) != nullptr) {
        std::string entry_name = entry->d_name;
        
        // 只处理event开头的设备（输入设备通常以event命名）
        if (entry_name.substr(0, 5) != "event") {
            continue;
        }
        
        // 检查该设备是否为键盘
        if (is_keyboard_device(entry_name)) {
            // 转换为/dev/input/eventX格式的设备路径
            std::string device_path = "/dev/input/" + entry_name;
            
            // 打印设备信息（可选）
            std::ifstream name_file("/sys/class/input/" + entry_name + "/device/name");
            std::string name;
            std::getline(name_file, name);
            std::cout << "找到键盘设备: " << device_path << " (" << name << ")" << std::endl;
            return device_path;
        }
    }
    
    closedir(dir);
    return "";
}

void KeyboardInputAngleStepping(std::shared_ptr<ArmKeyboardNode> armNode) {
    // 设置终端为无缓冲模式
    std::cout << "q:退出,[a,s,d,f,g,h]:左臂控制,[z,x,c,v,b,n]:右臂控制" << std::endl;
    SetNonCanonicalMode(true);

    std::string device = find_keyboard_devices();
    std::cout << "找到键盘设备: " << device << std::endl;
    const char* device_path = device.c_str();  // 替换为你的键盘设备
    if (device_path == nullptr || strlen(device_path) == 0) {
        std::cerr << "未找到键盘设备，请确保键盘已连接。" << std::endl;
        SetNonCanonicalMode(false);
        return;
    }
    int fd = open(device_path, O_RDONLY);
    if (fd < 0) {
        std::cerr << "无法打开设备 " << device_path << "，请检查权限或路径是否正确" << std::endl;
        SetNonCanonicalMode(false);
        return;
    }

    struct input_event ev;
    char command[16] = {0};
    rclcpp::Time startTime = armNode->get_clock()->now();
    while (rclcpp::ok()) {
        int cancel_flag = 0;
        ssize_t n = read(fd, &ev, sizeof(ev));
        if (n < (ssize_t)sizeof(ev)) {
            continue;  // 事件不完整，跳过
        }
        // RCLCPP_INFO(armNode->get_logger(), "type: %d, code: %d, value: %d\n", ev.type, ev.code, ev.value);

        // 仅处理键盘事件
        if (ev.type == EV_KEY) {
            // ev.code：键码（如KEY_A=30），ev.value：0=释放，1=按下，2=重复（可忽略）
            if (ev.value == 1) {  // 按键按下
                if (keyToCommand.find(ev.code) == keyToCommand.end()) {
                    if (ev.code == KEY_Q) {  // 按下'q'退出
                        std::cout << "退出遥控模式。" << std::endl;
                        break;
                    }
                    continue;  // 非定义按键，忽略
                }
                command[keyToCommand.at(ev.code)] = 1;
            } else if (ev.value == 0) {  // 按键释放
                if (keyToCommand.find(ev.code) == keyToCommand.end()) {
                    continue;  // 非定义按键，忽略
                }
                command[keyToCommand.at(ev.code)] = 0;
                cancel_flag = 1;
            }
        }
        rclcpp::Time nowTime = armNode->get_clock()->now();
        int64_t duration = (nowTime - startTime).nanoseconds();
        if (duration >= 100000000 || cancel_flag) {  // 每100ms发送一次命令
            armNode->SendAngleSteppingGoal(command);
            startTime = nowTime;
        }
    }

    // 清理资源
    close(fd);
    SetNonCanonicalMode(false);
}

void KeyboardInputAngleMoving(std::shared_ptr<ArmKeyboardNode> armNode) {
    std::string line;
    while (rclcpp::ok()) {
        std::cout << "请输入: 整数(0(退出)/1(左臂)/2(右臂)),6个float,用','分割,以';'结尾:" << std::endl;
        std::getline(std::cin, line, ';');
        std::istringstream ss(line);
        std::string token;
        int mode;
        float values[USED_ARM_DOF];
        bool valid = true;

        // 解析整数
        if (!std::getline(ss, token, ',')) {
            valid = false;
        }
        else {
            try {
                mode = std::stoi(token);
                if (mode != 1 && mode != 2) valid = false;
            } catch (...) {
                valid = false;
            }
        }

        // 解析6个float
        for (int i = 0; i < USED_ARM_DOF && valid; ++i) {
            if (!std::getline(ss, token, ',')) { valid = false; break; }
            try {
                values[i] = std::stof(token);
            } catch (...) { valid = false; }
        }

        if (mode == 0) {
            std::cout << "退出输入角度模式。" << std::endl;
            return;
        }

        if (valid) {
            if (mode == 1)
                armNode->SendAngleMovingGoal(values, LEFT_ARM);
            else if (mode == 2)
                armNode->SendAngleMovingGoal(values, RIGHT_ARM);
        } else {
            std::cout << "输入格式错误,请重试。" << std::endl;
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
}

void KeyboardInputPoseMoving(std::shared_ptr<ArmKeyboardNode> armNode) {
    std::string line;
    while (rclcpp::ok()) {
        std::cout << "请输入: 整数(0(退出)/1(左臂)/2(右臂)),7个float,用','分割,以';'结尾:" << std::endl;
        std::getline(std::cin, line, ';');
        std::istringstream ss(line);
        std::string token;
        int mode;
        float values[POSE_DIMENSION];
        bool valid = true;

        // 解析整数
        if (!std::getline(ss, token, ',')) {
            valid = false;
        }
        else {
            try {
                mode = std::stoi(token);
                if (mode != 1 && mode != 2) valid = false;
            } catch (...) {
                valid = false;
            }
        }

        // 解析6个float
        for (int i = 0; i < POSE_DIMENSION && valid; ++i) {
            if (!std::getline(ss, token, ',')) { valid = false; break; }
            try {
                values[i] = std::stof(token);
            } catch (...) { valid = false; }
        }

        if (mode == 0) {
            std::cout << "退出输入pose模式。" << std::endl;
            return;
        }

        if (valid) {
            if (mode == 1)
                armNode->SendDirectMovingGoal(values, LEFT_ARM);
            else if (mode == 2)
                armNode->SendDirectMovingGoal(values, RIGHT_ARM);
        } else {
            std::cout << "输入格式错误,请重试。" << std::endl;
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
}

void KeyboardInputPoseStepping2(std::shared_ptr<ArmKeyboardNode> armNode) {
    std::string line;
    auto message = std_msgs::msg::Float64MultiArray();
    message.data.resize(ARM_DOF);
    while (rclcpp::ok()) {
        std::cout << "请输入: 整数(0(退出)/1(左臂)/2(右臂)/3(停止)),7个float,用','分割,以';'结尾:" << std::endl;
        std::getline(std::cin, line, ';');
        std::istringstream ss(line);
        std::string token;
        int mode;
        float values[POSE_DIMENSION];
        bool valid = true;

        // 解析整数
        if (!std::getline(ss, token, ',')) {
            valid = false;
        }
        else {
            try {
                mode = std::stoi(token);
                if (mode != 1 && mode != 2) valid = false;
            } catch (...) {
                valid = false;
            }
        }

        // 解析6个float
        for (int i = 0; i < POSE_DIMENSION && valid; ++i) {
            if (!std::getline(ss, token, ',')) { valid = false; break; }
            try {
                values[i] = std::stof(token);
            } catch (...) { valid = false; }
        }

        if (mode == 0) {
            std::cout << "退出输入角度模式。" << std::endl;
            return;
        }
        if (mode == 3) {
            armNode->StopGoal();
            continue;
        }

        if (valid) {
            if (mode == 1)
                armNode->SendDirectSteppingGoal2(values, LEFT_ARM);
            else if (mode == 2)
                armNode->SendDirectSteppingGoal2(values, RIGHT_ARM);
        } else {
            std::cout << "输入格式错误,请重试。" << std::endl;
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
}

void ArmKeyboardNode::StopGoal()
{
    std::cout << "取消当前目标。" << std::endl;
    std::lock_guard<std::mutex> lock(armAimMutex_);
    if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
        client_ptr_->async_cancel_goal(leftArmGoalHandle_);
        leftArmGoalHandle_.reset();
        std::cout << "左臂目标已取消。" << std::endl;
    }
    if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
        client_ptr_->async_cancel_goal(rightArmGoalHandle_);
        rightArmGoalHandle_.reset();
        std::cout << "右臂目标已取消。" << std::endl;
    }
}

void ArmKeyboardNode::SendDirectSteppingGoal2(const float *values, int armId)
{
    std::lock_guard<std::mutex> lock(armAimMutex_);
    int64_t timeStamp = get_clock()->now().nanoseconds();
    auto goal_msg = ArmAction::Goal();
    commandId++;
    goal_msg.body_id = armId;
    goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
    goal_msg.data_length = POSE_DIMENSION;
    goal_msg.frame_time_stamp = timeStamp;
    goal_msg.command_id = commandId;
    for (int i = 0; i < POSE_DIMENSION; ++i) {
        goal_msg.data_array.push_back(values[i]);
    }
    auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
    if (armId == LEFT_ARM) {
        leftCommandId_ = commandId;
        leftArmGoalHandle_ = future.get();
    } else {
        rightCommandId_ = commandId;
        rightArmGoalHandle_ = future.get();
    }
}

void ArmKeyboardNode::SendDirectMovingGoal(const float *values, int armId)
{
    std::lock_guard<std::mutex> lock(armAimMutex_);
    int64_t timeStamp = get_clock()->now().nanoseconds();
    auto goal_msg = ArmAction::Goal();
    commandId++;
    goal_msg.body_id = armId;
    goal_msg.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE; // 位置控制
    goal_msg.data_length = POSE_DIMENSION;
    goal_msg.frame_time_stamp = timeStamp;
    goal_msg.command_id = commandId;
    for (int i = 0; i < POSE_DIMENSION; ++i) {
        goal_msg.data_array.push_back(values[i]);
    }
    auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
    if (armId == LEFT_ARM) {
        leftCommandId_ = commandId;
        leftArmGoalHandle_ = future.get();
    } else {
        rightCommandId_ = commandId;
        rightArmGoalHandle_ = future.get();
    }
}

void ArmKeyboardNode::SendAngleMovingGoal(const float *values, int armId)
{
    std::lock_guard<std::mutex> lock(armAimMutex_);
    int64_t timeStamp = get_clock()->now().nanoseconds();
    auto goal_msg = ArmAction::Goal();
    commandId++;
    RCLCPP_INFO(this->get_logger(), "SendAngleMovingGoal id=%ld", commandId);
    goal_msg.body_id = armId;
    goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE; // 位置控制
    goal_msg.data_length = USED_ARM_DOF;
    goal_msg.frame_time_stamp = timeStamp;
    goal_msg.command_id = commandId;
    for (int i = 0; i < USED_ARM_DOF; ++i) {
        goal_msg.data_array.push_back(values[i]);
    }
    auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
    if (armId == LEFT_ARM) {
        leftCommandId_ = commandId;
        leftArmGoalHandle_ = future.get();
    } else {
        rightCommandId_ = commandId;
        rightArmGoalHandle_ = future.get();
    }
}

void KeyboardInputDirectStepping(std::shared_ptr<ArmKeyboardNode> armNode) {
    // 设置终端为无缓冲模式
    std::cout << "q:退出,w:左臂控制,e:右臂控制,r:负方向增量,a:x方向移动,s:y方向移动,"
        << "d:z方向移动,z:rx方向旋转,x:ry方向旋转,c:rz方向旋转,v:rw方向旋转" << std::endl;
    SetNonCanonicalMode(true);

    std::string device = find_keyboard_devices();
    std::cout << "找到键盘设备: " << device << std::endl;
    const char* device_path = device.c_str();  // 替换为你的键盘设备
    if (device_path == nullptr || strlen(device_path) == 0) {
        std::cerr << "未找到键盘设备，请确保键盘已连接。" << std::endl;
        SetNonCanonicalMode(false);
        return;
    }
    int fd = open(device_path, O_RDONLY);
    if (fd < 0) {
        std::cerr << "无法打开设备 " << device_path << "，请检查权限或路径是否正确" << std::endl;
        SetNonCanonicalMode(false);
        return;
    }

    struct input_event ev;
    char command[16] = {0};
    rclcpp::Time startTime = armNode->get_clock()->now();
    while (rclcpp::ok()) {
        int cancel_flag = 0;
        ssize_t n = read(fd, &ev, sizeof(ev));
        if (n < (ssize_t)sizeof(ev)) {
            continue;  // 事件不完整，跳过
        }
        // RCLCPP_INFO(armNode->get_logger(), "type: %d, code: %d, value: %d\n", ev.type, ev.code, ev.value);

        // 仅处理键盘事件
        if (ev.type == EV_KEY) {
            // ev.code：键码（如KEY_A=30），ev.value：0=释放，1=按下，2=重复（可忽略）
            if (ev.value == 1) {  // 按键按下
                if (keyToCommand.find(ev.code) == keyToCommand.end()) {
                    if (ev.code == KEY_Q) {  // 按下'q'退出
                        std::cout << "退出遥控模式。" << std::endl;
                        break;
                    }
                    continue;  // 非定义按键，忽略
                }
                command[keyToCommand.at(ev.code)] = 1;
            } else if (ev.value == 0) {  // 按键释放
                if (keyToCommand.find(ev.code) == keyToCommand.end()) {
                    continue;  // 非定义按键，忽略
                }
                command[keyToCommand.at(ev.code)] = 0;
                cancel_flag = 1;
            }
        }
        rclcpp::Time nowTime = armNode->get_clock()->now();
        int64_t duration = (nowTime - startTime).nanoseconds();
        if (duration >= 100000000 || cancel_flag) {  // 每100ms发送一次命令
            armNode->SendDirectSteppingGoal(command);
            startTime = nowTime;
        }
    }

    // 清理资源
    close(fd);
    SetNonCanonicalMode(false);
}

static void ListenInputThread(std::shared_ptr<ArmKeyboardNode> armNode) {
    std::string line;
    auto message = std_msgs::msg::Float64MultiArray();
    message.data.resize(ARM_DOF);
    while (rclcpp::ok()) {
        std::cout << "请输入选择控制模式,0:退出,1:移动至目标角度,2:移动至目标位置,3:持续旋转角度,4:持续往某个方向移动,以';'结尾:" << std::endl;
        std::getline(std::cin, line, ';');
        int mode = 0;
        try {
            mode = std::stoi(line);
        } catch (...) {
            std::cout << "输入格式错误,请重试。" << std::endl;
            continue;
        }

        switch (mode) {
            case 0:
                std::cout << "退出输入监听线程。" << std::endl;
                armNode->exit_flag = true;
                return;
            case 1:
                KeyboardInputAngleMoving(armNode);
                break;
            case 2:
                KeyboardInputPoseMoving(armNode);
                break;
            case 3:
                KeyboardInputAngleStepping(armNode);
                std::getline(std::cin, line, 'q');
                break;
            case 4:
                KeyboardInputPoseStepping2(armNode);
                break;
            case 5:
                KeyboardInputPoseStepping2(armNode);
                break;
            default:
                if (mode != 0) {
                    std::cout << "无效选择，请重试。" << std::endl;
                }
                break;
        }
    }
}

ArmKeyboardNode::ArmKeyboardNode() : Node("arm_keyboard_node")
{
    for (int i = 0; i < USED_ARM_DOF; ++i) {
        leftArmAngleAim_[i] = 0.0;
        rightArmAngleAim_[i] = 0.0;
    }
    leftCommandId_ = 0;
    rightCommandId_ = 0;
    leftArmAimPub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("left_arm_command", 10);
    rightArmAimPub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("right_arm_command", 10);
    exit_flag = false;
    this->client_ptr_ = rclcpp_action::create_client<ArmAction>(
      this,
      "ArmAction");
    goal_options_.goal_response_callback =
        std::bind(&ArmKeyboardNode::goal_response_callback, this, _1);
    goal_options_.feedback_callback =
        std::bind(&ArmKeyboardNode::feedback_callback, this, _1, _2);
    goal_options_.result_callback =
        std::bind(&ArmKeyboardNode::result_callback, this, _1);
}

ArmKeyboardNode::~ArmKeyboardNode()
{
    if (inputThread_.joinable()) {
        inputThread_.join(); // 或 join()，视你的需求
    }
    rightArmAimPub_.reset();
    leftArmAimPub_.reset();
}

void ArmKeyboardNode::goal_response_callback(GoalHandleArm::SharedPtr goal_handle)
{
    if (!goal_handle) {
        RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
        RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
    }
}

void ArmKeyboardNode::feedback_callback(
    GoalHandleArm::SharedPtr goal_handle,
    const std::shared_ptr<const ArmAction::Feedback> feedback)
{
    if (!goal_handle || !feedback) {
        RCLCPP_INFO(this->get_logger(), "feedback received err.");
    }
}

void ArmKeyboardNode::result_callback(const GoalHandleArm::WrappedResult & result)
{
    std::unique_lock lock(rw_mutex_);
    int64_t command_id = result.result->command_id;
    if (command_id == leftCommandId_) {
        leftCommandId_ = 0;
        leftArmGoalHandle_ = nullptr;
    } else if (command_id == rightCommandId_) {
        rightCommandId_ = 0;
        rightArmGoalHandle_ = nullptr;
    } else {
        RCLCPP_ERROR(this->get_logger(), "Unknown command id in result: %ld", command_id);
        return;
    }
    switch (result.code) {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "Goal was succeeded, command id: %ld", command_id);
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted, command id: %ld", command_id);
            return;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "Goal was canceled, command id: %ld", command_id);
            return;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code, command id: %ld", command_id);
            return;
    }
}

int directCommandMap[POSE_DIMENSION] = {0, 1, 2, 6, 7, 8, 9};

void ArmKeyboardNode::SendDirectSteppingGoal(const char *command)
{
    std::unique_lock lock(rw_mutex_);
    std::string commandStr = "command: ";
    for (int i = 0; i < 16; ++i) {
        if (command[i]) {
            commandStr += std::to_string(i) + " ";
        }
    }
    RCLCPP_INFO(this->get_logger(), "%s", commandStr.c_str());
    if (client_ptr_ == nullptr) {
        RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
        return;
    }
    if (!client_ptr_->action_server_is_ready()) {
        RCLCPP_ERROR(this->get_logger(), "Action server not ready");
        return;
    }
    auto goal_msg = ArmAction::Goal();
    int commandValueBitMap = 0;
    double speedadd = 0.01;
    if (command[14]) {
        speedadd = -0.01;
    }
    
    goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
    goal_msg.data_length = POSE_DIMENSION;
    goal_msg.command_id = commandId;
    for (int i = 0; i < POSE_DIMENSION; ++i) {
        if (command[directCommandMap[i]]) {
            goal_msg.data_array.push_back(speedadd);
            commandValueBitMap |= (1 << i);
        } else {
            goal_msg.data_array.push_back(0.0);
        }
    }

    if (command[12] && leftDirectCommand_!= commandValueBitMap) {
        commandId++;
        goal_msg.body_id = LEFT_ARM;
        leftDirectCommand_ = commandValueBitMap;
        if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
            RCLCPP_INFO(this->get_logger(),  "cancel left arm goal. command id: %ld", leftCommandId_);
            client_ptr_->async_cancel_goal(leftArmGoalHandle_);
            leftArmGoalHandle_.reset();
        } else if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
            RCLCPP_INFO(this->get_logger(),  "cancel right arm goal. command id: %ld", rightCommandId_);
            client_ptr_->async_cancel_goal(rightArmGoalHandle_);
            rightArmGoalHandle_.reset();
        }

        if (commandValueBitMap != 0) {
            auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
            leftCommandId_ = commandId;
            leftArmGoalHandle_ = future.get();
            RCLCPP_INFO(this->get_logger(), "send left arm goal, command id: %ld", commandId);
        }
    } else if (command[13] && rightDirectCommand_!= commandValueBitMap) {
        commandId++;
        goal_msg.body_id = RIGHT_ARM;
        rightDirectCommand_ = commandValueBitMap;
        if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
            RCLCPP_INFO(this->get_logger(),  "cancel right arm goal. command id: %ld", rightCommandId_);
            client_ptr_->async_cancel_goal(rightArmGoalHandle_);
            rightArmGoalHandle_.reset();
        } else if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
            RCLCPP_INFO(this->get_logger(),  "cancel left arm goal. command id: %ld", leftCommandId_);
            client_ptr_->async_cancel_goal(leftArmGoalHandle_);
            leftArmGoalHandle_.reset();
        }

        if (commandValueBitMap != 0) {
            auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
            rightCommandId_ = commandId;
            rightArmGoalHandle_ = future.get();
            RCLCPP_INFO(this->get_logger(), "send right arm goal, command id: %ld", commandId);
        }
    } else {
        RCLCPP_ERROR(this->get_logger(), "No arm selected");
        return;
    }
}

int actionChange[8] = {KEEP_ACTION, NEW_ACTION, KEEP_ACTION, NEW_ACTION,
                 STOP_ACTION, NEW_ACTION, NEW_ACTION, NEW_ACTION};

void ArmKeyboardNode::SendAngleSteppingGoal(const char *command) {
    std::unique_lock lock(rw_mutex_);
    std::string commandStr = "command: ";
    for (int i = 0; i < 16; ++i) {
        if (command[i]) {
            commandStr += std::to_string(i) + " ";
        }
    }
    RCLCPP_INFO(this->get_logger(), "%s", commandStr.c_str());
    if (client_ptr_ == nullptr) {
        RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
        return;
    }
    if (!client_ptr_->action_server_is_ready()) {
        RCLCPP_ERROR(this->get_logger(), "Action server not ready");
        return;
    }
    float speedadd = 0.02;
    if (command[12]) {
        speedadd = -0.02;
    }
    int rightArmMove = 0;
    int leftArmMove = 0;
    int rightArmStop = 0;
    int leftArmStop = 0;
    int rightArmNew = 0;
    int leftArmNew = 0;
    int rightArmKeep = 0;
    int leftArmKeep = 0;
    for (int i = 0; i < USED_ARM_DOF; ++i) {
        if (command[i]) {
            if (fabs(leftArmAngleAim_[i] + speedadd) < 0.1) {
                leftArmAngleAim_[i] += speedadd;
                leftArmNew = 1;
            } else {
                leftArmKeep = 1;
            }
        } else {
            if (leftArmAngleAim_[i] != 0.0f) {
                leftArmAngleAim_[i] = 0;
                leftArmStop = 1;
            }
        }
        if (command[i + USED_ARM_DOF]) {
            if (fabs(rightArmAngleAim_[i] + speedadd) < 0.1) {
                rightArmAngleAim_[i] += speedadd;
                rightArmNew = 1;
            } else {
                rightArmKeep = 1;
            }
        } else {
            if (rightArmAngleAim_[i] != 0.0f) {
                rightArmAngleAim_[i] = 0;
                rightArmStop = 1;
            }
        }
    }

    leftArmMove = actionChange[leftArmNew | (leftArmStop << 2) | (leftArmKeep << 1)];
    rightArmMove = actionChange[rightArmNew | (rightArmStop << 2) | (rightArmKeep << 1)];

    if (leftArmMove != KEEP_ACTION) {
        if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
            RCLCPP_INFO(this->get_logger(),  "cancel left arm goal. command id: %ld", leftCommandId_);
            client_ptr_->async_cancel_goal(leftArmGoalHandle_);
            leftArmGoalHandle_.reset();
        }
        if ((leftArmMove & NEW_ACTION) != 0) {
            auto goal_msg = ArmAction::Goal();
            commandId++;
            leftCommandId_ = commandId;
            goal_msg.body_id = LEFT_ARM;
            goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
            goal_msg.data_length = USED_ARM_DOF;
            goal_msg.command_id = commandId;
            for (int i = 0; i < USED_ARM_DOF; ++i) {
                goal_msg.data_array.push_back(leftArmAngleAim_[i]);
            }
            auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
            leftArmGoalHandle_ = future.get();
            RCLCPP_INFO(this->get_logger(), "send left arm goal, command id: %ld", commandId);
        }
    }
    if (rightArmMove != KEEP_ACTION) {
        if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
            RCLCPP_INFO(this->get_logger(), "cancel right arm goal. command id: %ld", rightCommandId_);
            client_ptr_->async_cancel_goal(rightArmGoalHandle_);
            rightArmGoalHandle_.reset();
        }
        if ((rightArmMove & NEW_ACTION) != 0) {
            auto goal_msg = ArmAction::Goal();
            commandId++;
            rightCommandId_ = commandId;
            goal_msg.body_id = RIGHT_ARM;
            goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
            goal_msg.data_length = USED_ARM_DOF;
            goal_msg.command_id = commandId;

            for (int i = 0; i < USED_ARM_DOF; ++i) {
                goal_msg.data_array.push_back(rightArmAngleAim_[i]);
            }
            auto future = client_ptr_->async_send_goal(goal_msg, goal_options_);
            rightArmGoalHandle_ = future.get();
            RCLCPP_INFO(this->get_logger(), "send right arm goal, command id: %ld", commandId);
        }
    }
}

int main(int argc, char *argv[])
{
    /*
obb1: center(-0.1883, 0.0863, 0.6624), axis1(-0.7700, 0.1477, -0.6207), axis2(-0.6119, 0.1045, 0.7840), axis3(0.1806, 0.9835, 0.0099), half_lengths(0.0316, 0.0330, 0.0145)
obb2: center(0.0095, 0.0001, 0.4868), axis1(-1.0000, -0.0071, -0.0000), axis2(0.0071, -1.0000, -0.0000), axis3(-0.0000, -0.0000, 1.0000), half_lengths(0.1505, 0.1795, 0.3083)
*/
    OBB obbA;
    OBB obbB;
    obbA.center = Vector3d(-0.1883, 0.0863, 0.6624);
    obbA.axis[0] = Vector3d(-0.7700, 0.1477, -0.6207);
    obbA.axis[1] = Vector3d(-0.6119, 0.1045, 0.7840);
    obbA.axis[2] = Vector3d(0.1806, 0.9835, 0.0099);
    obbA.halfExtent[0] = 0.0316;
    obbA.halfExtent[1] = 0.0330;
    obbA.halfExtent[2] = 0.0145;
    obbB.center = Vector3d(0.0095, 0.0001, 0.4868);
    obbB.axis[0] = Vector3d(-1.0000, -0.0071, -0.0000);
    obbB.axis[1] = Vector3d(0.0071, -1.0000, -0.0000);
    obbB.axis[2] = Vector3d(-0.0000, -0.0000, 1.0000);
    obbB.halfExtent[0] = 0.1505;
    obbB.halfExtent[1] = 0.1795;
    obbB.halfExtent[2] = 0.3083;

    AxisStats stats;
    bool inCollisionOBB = gjk_interface::checkOBBCollisionNew(obbA, obbB, &stats);
    if (inCollisionOBB) {
        printf("dect obb A and B collision\n");
    } else {
        printf("dect obb A and B safe\n");
    }

    inCollisionOBB = gjk_interface::checkOBBCollision(obbA, obbB);
    if (inCollisionOBB) {
        printf("dect obb A and B collision\n");
    } else {
        printf("dect obb A and B safe\n");
    }

    rclcpp::init(argc, argv);
    rclcpp::Node::SharedPtr node = std::make_shared<ArmKeyboardNode>();
    std::shared_ptr<ArmKeyboardNode> armNode = std::dynamic_pointer_cast<ArmKeyboardNode>(node);
    // 启动输入监听线程
    inputThread_ = std::thread(ListenInputThread, armNode);
    while (rclcpp::ok() && !armNode->exit_flag) {
        rclcpp::spin_some(node); // 处理所有待处理的事件
        std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 短暂休眠，降低CPU占用
    }
    rclcpp::shutdown();

    return 0;
}
