#ifndef ROBOT_CONTROL__HPP_
#define ROBOT_CONTROL__HPP_

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

namespace robot_control
{
    class RobotControlNode : public rclcpp::Node
    {
        public:
            RobotControlNode();
            ~RobotControlNode();

            // 关节复位
            void resetJoints();

        private:
            void print_help();

            void joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg);
            void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
            void timer_callback();
            void handle_phase_motion(std_msgs::msg::Float64MultiArray& cmd_msg, double current_time);
            void handle_wheel_control(std_msgs::msg::Float64MultiArray& cmd_msg, double dt_sec);

            // 成员变量
            rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
            rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
            rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_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> pitch_body_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）

            // 运动状态
            std::vector<double> joint_positions_; // 关节位置（弧度）
            std::vector<double> joint_velocities_; // 关节速度（弧度/秒）
            std::vector<double> joint_accelerations_; // 关节加速度（弧度/秒^2）
            std::vector<double> joint_efforts_; // 关节力矩（牛顿米）

            sensor_msgs::msg::JointState joint_states_;
    };

} // namespace robot_control

#endif // ROBOT_CONTROL__HPP_
