#include "keyboard_control.hpp"
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <chrono>

using namespace std::chrono_literals;

// 键盘输入处理函数（非阻塞模式）
int kbhit()
{
    struct termios oldt, newt;
    int ch;
    int oldf;

    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
    fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);

    ch = getchar();

    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    fcntl(STDIN_FILENO, F_SETFL, oldf);

    if(ch != EOF)
    {
        ungetc(ch, stdin);
        return 1;
    }

    return 0;
}

KeyboardControlNode::KeyboardControlNode() : Node("keyboard_control_node")
{
    // 声明参数：移动速度、旋转速度
    this->declare_parameter("linear_speed", 0.2);   // 线速度（m/s）
    this->declare_parameter("angular_speed", 0.5);  // 角速度（rad/s）

    // 读取参数
    linear_speed_ = this->get_parameter("linear_speed").as_double();
    angular_speed_ = this->get_parameter("angular_speed").as_double();

    // 创建发布者（发送速度指令）
    twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
        "/cmd_vel", 10
    );

    // 打印控制说明
    print_help();

    // 创建定时器（周期性检查键盘输入）
    timer_ = this->create_wall_timer(
        50ms,  // 20Hz检查频率
        std::bind(&KeyboardControlNode::timer_callback, this)
    );
}


void KeyboardControlNode::print_help()
{
    RCLCPP_INFO(this->get_logger(), "键盘控制说明:");
    RCLCPP_INFO(this->get_logger(), "  w : 前进");
    RCLCPP_INFO(this->get_logger(), "  s : 后退");
    RCLCPP_INFO(this->get_logger(), "  a : 左转");
    RCLCPP_INFO(this->get_logger(), "  d : 右转");
    RCLCPP_INFO(this->get_logger(), "  x : 停止");
    RCLCPP_INFO(this->get_logger(), "  q : 退出");
}

void KeyboardControlNode::timer_callback()
{
    geometry_msgs::msg::Twist twist_msg;

    if(kbhit())
    {
        char key = getchar();
        switch(key)
        {
            case 'w':  // 前进
                twist_msg.linear.x = linear_speed_;
                twist_msg.angular.z = 0.0;
                RCLCPP_INFO(this->get_logger(), "前进");
                break;
            case 's':  // 后退
                twist_msg.linear.x = -linear_speed_;
                twist_msg.angular.z = 0.0;
                RCLCPP_INFO(this->get_logger(), "后退");
                break;
            case 'a':  // 左转
                twist_msg.linear.x = 0.0;
                twist_msg.angular.z = angular_speed_;
                RCLCPP_INFO(this->get_logger(), "左转");
                break;
            case 'd':  // 右转
                twist_msg.linear.x = 0.0;
                twist_msg.angular.z = -angular_speed_;
                RCLCPP_INFO(this->get_logger(), "右转");
                break;
            case 'x':  // 停止
                twist_msg.linear.x = 0.0;
                twist_msg.angular.z = 0.0;
                RCLCPP_INFO(this->get_logger(), "停止");
                break;
            case 'q':  // 退出
                RCLCPP_INFO(this->get_logger(), "退出控制");
                rclcpp::shutdown();
                return;
            default:
                twist_msg.linear.x = 0.0;
                twist_msg.angular.z = 0.0;
                print_help();
                break;
        }
        twist_pub_->publish(twist_msg);
    }
}

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<KeyboardControlNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
