#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include <cmath>
#include <chrono>

using namespace std::chrono_literals;

class ArmSineMotionNode : public rclcpp::Node
{
public:
    ArmSineMotionNode() : Node("arm_sine_motion_node")
    {
        // 1. 声明并获取参数（振幅默认50度，频率默认0.5Hz）
        this->declare_parameter("amplitude_deg", 50.0);  // 振幅（度）
        this->declare_parameter("frequency", 0.5);       // 频率（Hz）
        this->declare_parameter("control_period_ms", 1); // 控制周期（毫秒，默认20ms=50Hz）

        // 2. 读取参数并转换单位（度→弧度）
        double amplitude_deg = this->get_parameter("amplitude_deg").as_double();
        amplitude_rad_ = amplitude_deg * M_PI / 180.0;  // 50度→弧度
        frequency_ = this->get_parameter("frequency").as_double();
        int period_ms = this->get_parameter("control_period_ms").as_int();

        // 3. 手臂关节在控制器指令数组中的索引（对应joints配置顺序）
        // 控制器joints顺序：[0-3腿部, 4-9手臂]
        arm_joint_indices_ = {4, 5, 6, 7, 8, 9};  // 6个手臂关节索引
        total_joints_ = 10;  // 控制器总关节数（4腿部+6手臂）

        // 4. 创建发布者（发送位置指令到位置控制器）
        cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
            "/bot0_position_controller/commands", 
            10
        );

        // 5. 创建定时器（周期性发送指令）
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(period_ms),
            std::bind(&ArmSineMotionNode::timer_callback, this)
        );

        // 6. 记录起始时间（用于计算正弦运动相位）
        start_time_ = this->now();
        RCLCPP_INFO(this->get_logger(), "手臂正弦运动节点启动（振幅50度，频率%.2fHz）", frequency_);
    }

    void RestJoint()
    {
        auto reset_msg = std_msgs::msg::Float64MultiArray();
        reset_msg.data.resize(10, 0.0);
        cmd_pub_->publish(reset_msg);
    }

private:
    void timer_callback()
    {
        // 1. 计算当前运动时间（秒）
        rclcpp::Duration elapsed = this->now() - start_time_;
        double current_time = elapsed.seconds();

        // 2. 计算正弦位置：y = A*sin(2πft)
        // A=振幅（弧度），f=频率（Hz），t=时间（秒）
        double sine_pos = amplitude_rad_ * std::sin(2 * M_PI * frequency_ * current_time);

        // 3. 构建指令数组（10个关节：腿部保持0，手臂按正弦运动）
        auto cmd_msg = std_msgs::msg::Float64MultiArray();
        cmd_msg.data.resize(total_joints_, 0.0);  // 初始化所有关节为0

        // 4. 为手臂关节赋值正弦位置
        // for (size_t i = 0; i < arm_joint_indices_.size(); ++i) {
        //     int joint_idx = arm_joint_indices_[i];
        //     cmd_msg.data[joint_idx] = 0;//sine_pos;  // 所有手臂关节同步运动
        // }

        for (int i = 0; i < total_joints_; ++i) {
            // int joint_idx = arm_joint_indices_[i];
            if (i == 1 || i == 3)
            {
                cmd_msg.data[i] = sine_pos * 2;
            }
            else
            {
                cmd_msg.data[i] = sine_pos;  // 所有手臂关节同步运动
            }
        }
        
        cmd_msg.data.resize(total_joints_, 0.0);  // 初始化所有关节为0

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

    // 成员变量
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time start_time_;  // 起始时间
    double amplitude_rad_;     // 振幅（弧度）
    double frequency_;         // 频率（Hz）
    std::vector<int> arm_joint_indices_;  // 手臂关节索引
    int total_joints_;         // 总关节数
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ArmSineMotionNode>();

    // 运行节点（按Ctrl+C退出）
    rclcpp::spin(node);

    // 退出时发送复位指令（所有关节回0）
    node->RestJoint();
    RCLCPP_INFO(node->get_logger(), "节点退出，发送复位指令");

    rclcpp::shutdown();
    return 0;
}
