#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"

class BaseController : public rclcpp::Node
{
public:
  BaseController() : Node("base_controller")
  {
    // 订阅参考速度
    cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
      "cmd_vel", 10, std::bind(&BaseController::cmdVelCallback, this, std::placeholders::_1));
    
    // 发布里程计
    odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
    
    // 创建TF广播器
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
    
    // 初始化状态
    x_ = 0.0;
    y_ = 0.0;
    theta_ = 0.0;
    
    // 创建定时器，定期更新控制和发布状态
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(20), std::bind(&BaseController::controlLoop, this));
  }

private:
  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
  {
    // 存储目标速度
    target_vx_ = msg->linear.x;
    target_vy_ = msg->linear.y;
    target_w_ = msg->angular.z;
  }

  void controlLoop()
  {
    // 简单PID控制器实现
    double dt = 0.02;  // 20ms控制周期
    
    // 模拟速度控制
    current_vx_ = target_vx_;  // 简化模型，实际应包含PID控制
    current_vy_ = target_vy_;
    current_w_ = target_w_;
    
    // 更新位姿
    double delta_x = (current_vx_ * cos(theta_) - current_vy_ * sin(theta_)) * dt;
    double delta_y = (current_vx_ * sin(theta_) + current_vy_ * cos(theta_)) * dt;
    double delta_theta = current_w_ * dt;
    
    x_ += delta_x;
    y_ += delta_y;
    theta_ += delta_theta;
    
    // 发布TF变换
    geometry_msgs::msg::TransformStamped transform_stamped;
    transform_stamped.header.stamp = this->now();
    transform_stamped.header.frame_id = "odom";
    transform_stamped.child_frame_id = "base_link";
    
    transform_stamped.transform.translation.x = x_;
    transform_stamped.transform.translation.y = y_;
    transform_stamped.transform.translation.z = 0.0;
    
    tf2::Quaternion q;
    q.setRPY(0, 0, theta_);
    transform_stamped.transform.rotation.x = q.x();
    transform_stamped.transform.rotation.y = q.y();
    transform_stamped.transform.rotation.z = q.z();
    transform_stamped.transform.rotation.w = q.w();
    
    tf_broadcaster_->sendTransform(transform_stamped);
    
    // 发布里程计消息
    auto odom = std::make_unique<nav_msgs::msg::Odometry>();
    odom->header.stamp = this->now();
    odom->header.frame_id = "odom";
    odom->child_frame_id = "base_link";
    
    odom->pose.pose.position.x = x_;
    odom->pose.pose.position.y = y_;
    odom->pose.pose.position.z = 0.0;
    odom->pose.pose.orientation.x = q.x();
    odom->pose.pose.orientation.y = q.y();
    odom->pose.pose.orientation.z = q.z();
    odom->pose.pose.orientation.w = q.w();
    
    odom->twist.twist.linear.x = current_vx_;
    odom->twist.twist.linear.y = current_vy_;
    odom->twist.twist.angular.z = current_w_;
    
    odom_pub_->publish(std::move(odom));
  }

  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  rclcpp::TimerBase::SharedPtr timer_;
  
  // 机器人状态
  double x_, y_, theta_;
  double target_vx_, target_vy_, target_w_;
  double current_vx_, current_vy_, current_w_;
};

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