#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <cmath>
#include <chrono>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>

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;
}

class RobotPhaseMotionNode : public rclcpp::Node
{
public:
    RobotPhaseMotionNode() : Node("robot_phase_motion_node")
    {
        // 声明并获取参数
        this->declare_parameter("leg_target_deg", 0.0);       // 腿部目标角度（度）
        this->declare_parameter("arm_amplitude_deg", 30.0);    // 手臂摆动振幅（度）
        this->declare_parameter("arm_frequency", 0.5);         // 手臂摆动频率（Hz）
        this->declare_parameter("control_period_ms", 10);      // 控制周期（毫秒）
        this->declare_parameter("wheel_radius", 0.05);         // 轮子半径（米）
        this->declare_parameter("wheel_separation", 0.3);      // 轮距（米）
        this->declare_parameter("linear_speed", 0.2);          // 线速度（m/s）
        this->declare_parameter("angular_speed", 0.5);         // 角速度（rad/s）

        // 读取参数并转换单位
        double leg_target_deg = this->get_parameter("leg_target_deg").as_double();
        leg_target_rad_ = leg_target_deg * M_PI / 180.0;
        
        double arm_amplitude_deg = this->get_parameter("arm_amplitude_deg").as_double();
        arm_amplitude_rad_ = arm_amplitude_deg * M_PI / 180.0;
        
        arm_frequency_ = this->get_parameter("arm_frequency").as_double();
        int period_ms = this->get_parameter("control_period_ms").as_int();
        wheel_radius_ = this->get_parameter("wheel_radius").as_double();
        wheel_separation_ = this->get_parameter("wheel_separation").as_double();
        linear_speed_ = this->get_parameter("linear_speed").as_double();
        angular_speed_ = this->get_parameter("angular_speed").as_double();
        joint_directions_ = {1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};  // 关节方向

        total_joints_ = 15;  // 总关节数
        
        // 关节索引定义
        leg_joint_indices_ = {0, 2, 4, 6};  // 腿部关节索引
        arm_joint_indices_ = {9, 10, 11, 12, 13};  // 手臂关节索引
        wheel_joint_indices_ = {1,  5};  // 轮子关节索引（left_wheel1, right_wheel1 ）两个主动轮
        wheel_joint_directions_ = {-1, 1};  // 轮子方向

        // 初始化轮子目标角度
        wheel_targets_.resize(2, 0.0);

        // 创建发布者
        cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
            "/bot1_position_controller/commands", 10
        );

        // 创建订阅者（接收来自速度指令）
        twist_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "/cmd_vel", 10,
            std::bind(&RobotPhaseMotionNode::twist_callback, this, std::placeholders::_1)
        );

        // 创建定时器
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(period_ms),
            std::bind(&RobotPhaseMotionNode::timer_callback, this)
        );

        // 记录起始时间
        start_time_ = this->now();
        last_time_ = this->now();

        // 打印控制说明
        print_help();
        RCLCPP_INFO(this->get_logger(), "机器人分阶段运动节点启动（含轮子控制）");
    }

    // 关节复位
    void resetJoints()
    {
        auto reset_msg = std_msgs::msg::Float64MultiArray();
        reset_msg.data.resize(total_joints_, 0.0);
        cmd_pub_->publish(reset_msg);
    }

private:
    void 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 twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        // 接收外部速度指令
        linear_x_ = msg->linear.x;
        angular_z_ = msg->angular.z;
    }

    void timer_callback()
    {
        // 处理键盘输入
        handle_keyboard_input();

        // 计算时间差
        rclcpp::Time current_time = this->now();
        rclcpp::Duration dt = current_time - last_time_;
        double dt_sec = dt.seconds();
        last_time_ = current_time;

        // 构建指令数组
        auto cmd_msg = std_msgs::msg::Float64MultiArray();
        cmd_msg.data.resize(total_joints_, 0.0);

        // 分阶段控制腿部和手臂
        double phase_time = (current_time - start_time_).seconds();
        handle_phase_motion(cmd_msg, phase_time);

        // 计算并设置轮子角度
        handle_wheel_control(cmd_msg, dt_sec);

        // 发送指令
        cmd_pub_->publish(cmd_msg);
    }

    void handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time)
    {
        // 第一阶段：前5秒，腿部关节缓慢张开
        if (current_time <= 5.0)
        {
            double progress = current_time / 5.0;
            double current_leg_angle = leg_target_rad_ * progress;
            
            // 设置腿部关节角度
            for (int idx : leg_joint_indices_)
            {
                if (idx == 0 || idx == 4)  // 左腿1、左腿2关节
                {
                    cmd_msg.data[idx] = -current_leg_angle;
                }
                else
                {
                    cmd_msg.data[idx] = current_leg_angle;
                }
            }
            
            // 手臂保持初始位置
            for (int idx : arm_joint_indices_)
            {
                cmd_msg.data[idx] = 0.0;
            }
            
            if (current_time >= 4.9 && !phase1_notified_)
            {
                RCLCPP_INFO(this->get_logger(), "第一阶段即将结束，准备进入手臂摆动阶段");
                phase1_notified_ = true;
            }
        }
        // 第二阶段：5秒后，双臂做摆手动作
        else
        {
            // 腿部保持在目标角度
            for (int idx : leg_joint_indices_)
            {
                if (idx == 0 || idx == 4)  // 左腿1、左腿2关节
                {
                    cmd_msg.data[idx] = -leg_target_rad_;
                }
                else
                {
                    cmd_msg.data[idx] = leg_target_rad_;
                }
            }
            
            // 计算手臂摆动角度
            double arm_phase_time = current_time - 5.0;
            double arm_angle = arm_amplitude_rad_ * std::sin(2 * M_PI * arm_frequency_ * arm_phase_time);
            
            // 设置手臂关节角度（左右臂对称）
            for (size_t i = 0; i < arm_joint_indices_.size(); ++i)
            {
                if (i >= arm_joint_indices_.size() / 2)
                {
                    cmd_msg.data[arm_joint_indices_[i]] = -arm_angle;
                }
                else
                {
                    cmd_msg.data[arm_joint_indices_[i]] = arm_angle;
                }

                cmd_msg.data[arm_joint_indices_[i]] = 0;
            }
        }
    }

    void handle_wheel_control(std_msgs::msg::Float64MultiArray& cmd_msg, double dt_sec)
    {
        // 计算左右轮目标线速度
        double left_speed = linear_x_ + (angular_z_ * wheel_separation_) / 2.0;
        double right_speed = linear_x_ - (angular_z_ * wheel_separation_) / 2.0;

        // 计算轮子角速度（rad/s）
        double left_omega = left_speed / wheel_radius_;
        double right_omega = right_speed / wheel_radius_;

        // 积分计算目标角度（弧度）
        wheel_targets_[0] += left_omega * dt_sec;  // left_wheel1
        wheel_targets_[1] += right_omega * dt_sec; // right_wheel1

        // 赋值轮子目标角度到指令数组
        for (size_t i = 0; i < wheel_joint_indices_.size(); ++i)
        {
            cmd_msg.data[wheel_joint_indices_[i]] = wheel_joint_directions_[i] * wheel_targets_[i];
        }
    }

    void handle_keyboard_input()
    {
        if(kbhit())
        {
            char key = getchar();
            switch(key)
            {
                case 'w':  // 前进
                    linear_x_ = linear_speed_;
                    angular_z_ = 0.0;
                    RCLCPP_INFO(this->get_logger(), "前进");
                    break;
                case 's':  // 后退
                    linear_x_ = -linear_speed_;
                    angular_z_ = 0.0;
                    RCLCPP_INFO(this->get_logger(), "后退");
                    break;
                case 'a':  // 左转
                    linear_x_ = 0.0;
                    angular_z_ = angular_speed_;
                    RCLCPP_INFO(this->get_logger(), "左转");
                    break;
                case 'd':  // 右转
                    linear_x_ = 0.0;
                    angular_z_ = -angular_speed_;
                    RCLCPP_INFO(this->get_logger(), "右转");
                    break;
                case 'x':  // 停止
                    linear_x_ = 0.0;
                    angular_z_ = 0.0;
                    RCLCPP_INFO(this->get_logger(), "停止");
                    break;
                case 'q':  // 退出
                    RCLCPP_INFO(this->get_logger(), "退出控制");
                    resetJoints();
                    rclcpp::shutdown();
                    exit(0);
                    break;
                default:
                    print_help();
                    break;
            }
        }
    }

    // 成员变量
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time start_time_;
    rclcpp::Time last_time_;
    
    // 运动参数
    double leg_target_rad_;       // 腿部目标角度（弧度）
    double arm_amplitude_rad_;    // 手臂摆动振幅（弧度）
    double arm_frequency_;        // 手臂摆动频率（Hz）
    int total_joints_;            // 总关节数
    bool phase1_notified_ = false; // 第一阶段结束提示标记
    
    // 关节索引
    std::vector<int> leg_joint_indices_;  // 腿部关节索引
    std::vector<int> arm_joint_indices_;  // 手臂关节索引
    std::vector<int> wheel_joint_indices_; // 轮子关节索引
    std::vector<int> joint_directions_;   // 关节方向
    std::vector<int> wheel_joint_directions_; // 轮子关节方向
    
    // 轮子控制相关
    double wheel_radius_;         // 轮子半径（米）
    double wheel_separation_;     // 轮距（米）
    std::vector<double> wheel_targets_;   // 轮子目标角度（弧度）
    double linear_x_ = 0.0;       // 线速度（m/s）
    double angular_z_ = 0.0;      // 角速度（rad/s）
    double linear_speed_;         // 最大线速度（m/s）
    double angular_speed_;        // 最大角速度（rad/s）
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<RobotPhaseMotionNode>();
    rclcpp::spin(node);
    node->resetJoints();
    RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令");
    rclcpp::shutdown();
    return 0;
}
