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

namespace robot_control
{
    using namespace std::chrono_literals;

    RobotControlNode::RobotControlNode(): 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, 1, 2, 3};  // 腿部关节索引
        wheel_joint_indices_ = {4, 5, 6, 7}; 
        pitch_body_indices_ = {8};  // 身体关节索引
        arm_joint_indices_ = {9, 10, 11, 12, 13, 14};  // 手臂关节索引
       
        wheel_joint_directions_ = {1, -1, 1, -1};  // 轮子方向

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

        // 初始化关节状态
        joint_positions_.resize(total_joints_, 0.0);
        joint_velocities_.resize(total_joints_, 0.0);

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

        joint_states_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
            "joint_states", 10,
            std::bind(&RobotControlNode::joint_states_callback, this, std::placeholders::_1)
        );

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

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

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

    RobotControlNode::~RobotControlNode()
    {
        // 关闭节点时复位关节
        resetJoints();
    }

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


    void RobotControlNode::joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
    {
        // 保存当前关节角度
        joint_states_ = *msg;
        for (size_t i = 0; i < msg->position.size(); i++)
        {
            // joint_positions_[i] = msg->position[i];
            // joint_velocities_[i] = msg->velocity[i];
        }
    }

    void RobotControlNode::twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        if (!msg) {  // 检查消息是否有效
            RCLCPP_WARN(this->get_logger(), "收到空的Twist消息，忽略");
            return;
        }

        // 接收外部速度指令
        linear_x_ = msg->linear.x;
        angular_z_ = msg->angular.z;
    }

    void RobotControlNode::timer_callback()
    {
        // 计算时间差
        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);

        RCLCPP_INFO(this->get_logger(), "Joint 1 command: %f", cmd_msg.data[1]);

    }

    void RobotControlNode::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 RobotControlNode::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

        wheel_targets_[2] += left_omega * dt_sec;  // left_wheel2
        wheel_targets_[3] += right_omega * dt_sec; // right_wheel2

        // 赋值轮子目标角度到指令数组
        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];
        }
    }
}

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