#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include <cmath>

class MotionController : public rclcpp::Node
{
public:
    MotionController() : Node("motion_controller")
    {
        // 声明参数
        this->declare_parameter<double>("wheel_radius", 0.05);
        this->declare_parameter<double>("wheel_separation", 0.2);
        
        // 获取参数
        this->get_parameter("wheel_radius", wheel_radius_);
        this->get_parameter("wheel_separation", wheel_separation_);
        
        // 创建发布者和订阅者
        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10, 
            std::bind(&MotionController::odom_callback, this, std::placeholders::_1)
        );
        
        // 创建定时器用于控制循环
        control_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&MotionController::control_loop, this)
        );
        
        RCLCPP_INFO(this->get_logger(), "Motion controller initialized");
    }
    
private:
    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        // 保存当前里程计信息
        current_pose_ = msg->pose.pose;
        current_twist_ = msg->twist.twist;
    }
    
    void control_loop()
    {
        // 这里可以实现更复杂的控制算法
        // 简单示例：如果设置了目标位置，则向目标位置移动
        
        if (goal_set_)
        {
            // 计算当前位置与目标位置的差异
            double dx = goal_x_ - current_pose_.position.x;
            double dy = goal_y_ - current_pose_.position.y;
            double distance = std::sqrt(dx*dx + dy*dy);
            
            // 计算目标方向
            double target_yaw = std::atan2(dy, dx);
            
            // 从四元数获取当前偏航角
            double current_yaw = get_yaw_from_quaternion(current_pose_.orientation);
            
            // 计算角度差
            double angle_diff = target_yaw - current_yaw;
            // 归一化到[-π, π]
            angle_diff = std::atan2(std::sin(angle_diff), std::cos(angle_diff));
            
            // 控制参数
            const double angle_tolerance = 0.1;  // 弧度
            const double distance_tolerance = 0.05;  // 米
            
            geometry_msgs::msg::Twist cmd_vel;
            
            // 如果到达目标位置，停止
            if (distance < distance_tolerance)
            {
                cmd_vel.linear.x = 0.0;
                cmd_vel.angular.z = 0.0;
                goal_set_ = false;
                RCLCPP_INFO(this->get_logger(), "Reached goal position");
            }
            // 否则先旋转到目标方向
            else if (std::abs(angle_diff) > angle_tolerance)
            {
                cmd_vel.linear.x = 0.0;
                cmd_vel.angular.z = std::clamp(angle_diff, -0.5, 0.5);  // 限制最大角速度
            }
            // 然后前进
            else
            {
                cmd_vel.linear.x = std::clamp(distance, 0.0, 0.3);  // 限制最大线速度
                cmd_vel.angular.z = 0.0;
            }
            
            cmd_vel_pub_->publish(cmd_vel);
        }
    }
    
    // 从四元数计算偏航角
    double get_yaw_from_quaternion(const geometry_msgs::msg::Quaternion& quat)
    {
        double w = quat.w;
        double x = quat.x;
        double y = quat.y;
        double z = quat.z;
        
        return std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
    }
    
    // 设置目标位置（可以通过服务或动作接口实现）
    void set_goal(double x, double y)
    {
        goal_x_ = x;
        goal_y_ = y;
        goal_set_ = true;
        RCLCPP_INFO(this->get_logger(), "New goal set: (%f, %f)", x, y);
    }
    
    // 成员变量
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::TimerBase::SharedPtr control_timer_;
    
    geometry_msgs::msg::Pose current_pose_;
    geometry_msgs::msg::Twist current_twist_;
    
    double wheel_radius_;
    double wheel_separation_;
    
    double goal_x_ = 0.0;
    double goal_y_ = 0.0;
    bool goal_set_ = false;
};

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