#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h> 
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common.h>
#include <pcl/common/time.h>

class GroundPointCloudPublisher : public rclcpp::Node
{
public:
  GroundPointCloudPublisher() : Node("ground_point_cloud_publisher")
  {
    // 创建PointCloud2消息
    auto msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
    // 创建点云发布者
    point_cloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("ground_point_cloud", 10);


    // 初始化点云消息
    msg->header.stamp = now();
    msg->header.frame_id = "base_link";

    // 模拟生成地面点云
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    cloud->width = 100;
    uint32_t ground_length = 10; // 地面长度
    uint32_t ground_height = 1 ;// 地面高度

    cloud->height = 10;
    cloud->points.resize(cloud->width * cloud->height);

    // 填充点云数据
    for (uint32_t i = 0; i < cloud-> width / ground_length ; i ++)
    {
        for(uint32_t j = 0; j < ground_length; j ++){
            cloud->points[i * ground_length + j].x = i; // x坐标
            cloud->points[i * ground_length + j].y = j; // y坐标
            cloud->points[i * ground_length + j].z = ground_height;     // z坐标
        }
      
    }

    // 定义旋转角度（弧度）
    double rotation_angle_x = M_PI / 4; // X轴旋转45度
    double rotation_angle_y = M_PI / 6; // Y轴旋转45度

    // 创建旋转矩阵
    Eigen::AngleAxisd rotation_x(rotation_angle_x, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd rotation_y(rotation_angle_y, Eigen::Vector3d::UnitY());
    Eigen::Matrix3d rotation_matrix = (rotation_y * rotation_x).matrix();

    // 应用旋转变换
    for (auto &point : cloud->points)
    {
      Eigen::Vector3d point_vector(point.x, point.y, point.z);
      point_vector = rotation_matrix * point_vector;
      point.x = point_vector(0);
      point.y = point_vector(1);
      point.z = point_vector(2);
    }

    // 定时发布点云
    this->declare_parameter("publish_rate", 1.0);
    auto publish_rate = this->get_parameter("publish_rate").as_double();
    timer_ = this->create_wall_timer(
      std::chrono::duration<double>(1.0 / publish_rate),
      [cloud, this]() {
        // 将PCL点云转换为ROS 2消息
        sensor_msgs::msg::PointCloud2 ros_cloud;
        pcl::toROSMsg(*cloud, ros_cloud);
        ros_cloud.header.stamp = this->now();
        ros_cloud.header.frame_id = "base_link";

        // 发布点云
        point_cloud_publisher_->publish(ros_cloud);
      });


    // 创建可视化窗口对象
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));

    // 设置点云颜色为 intensity 值
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> fildColor(cloud, "intensity");
    
    // 将点云添加到可视化窗口中
    viewer->addPointCloud<pcl::PointXYZI>(cloud, fildColor, "cloud");

    // 设置点云大小
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");

    // 显示可视化窗口，直到用户关闭窗口为止
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(1000);  // 每隔100毫秒渲染一次
    }

  }

private:
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Time now() const { return rclcpp::Clock().now(); }
};

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