#include "rclcpp/rclcpp.hpp"
#include "interface/msg/lidar.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/crop_box.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <climits>
#include <cmath>
#include <memory>
#include <atomic>

// 定义优化的点云处理节点
class LidarNode : public rclcpp::Node
{
public:
    LidarNode(const std::string &name) : Node(name)
    {
        // 创建发布者
        command_publisher_ = this->create_publisher<interface::msg::Lidar>(
            "lidar_message",
            rclcpp::QoS(10).transient_local());

        auto qos = rclcpp::QoS(rclcpp::KeepLast(5))
                       .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)
                       .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE)
                       .history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);

        // 创建点云订阅者
        points_subscriber_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/rslidar_points",
            qos,
            std::bind(&LidarNode::points_callback, this, std::placeholders::_1));

        // 创建定时器，定期发布消息
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100), // 10Hz发布频率
            std::bind(&LidarNode::publish_message, this));

        // 声明参数 - 针对小巴车优化
        this->declare_parameter<double>("lidar_pitch_deg", 12.0);
        this->declare_parameter<bool>("use_ransac_ground_filter", false);
        this->declare_parameter<double>("ground_threshold", 0.3);
        this->declare_parameter<double>("angle_threshold_deg", 5.0);
        this->declare_parameter<double>("vehicle_height", 2.4);

        // 获取参数
        double pitch_deg = this->get_parameter("lidar_pitch_deg").as_double();
        lidar_pitch_rad_ = pitch_deg * M_PI / 180.0;

        use_ransac_ground_filter_ = this->get_parameter("use_ransac_ground_filter").as_bool();
        ground_threshold_ = this->get_parameter("ground_threshold").as_double();

        double angle_threshold_deg = this->get_parameter("angle_threshold_deg").as_double();
        angle_threshold_rad_ = angle_threshold_deg * M_PI / 180.0;

        vehicle_height_ = this->get_parameter("vehicle_height").as_double();

        // RCLCPP_INFO(this->get_logger(), "地面过滤参数: RANSAC=%s, 阈值=%.2fm, 角度阈值=%.1f°, 车辆高度=%.2fm",
        //             use_ransac_ground_filter_ ? "启用" : "禁用",
        //             ground_threshold_, angle_threshold_deg, vehicle_height_);
    }

private:
    std::atomic<double> min_distance_{INFINITY};
    interface::msg::Lidar cached_message_;
    std::mutex msg_mutex_;

    // 检测范围参数 (单位：米) - 针对小巴车调整
    static constexpr double Y_RANGE = 1.1; // 左右检测范围 ±1.1m
    static constexpr double X_MIN = 0.5;   // 前方最小检测距离 (避免车头干扰)
    static constexpr double X_MAX = 20.0;  // 前方最大检测距离 (雷达位置高，检测更远)
    static constexpr double Z_MIN = -0.5;  // 高度最小值 (考虑2.4m高度)
    static constexpr double Z_MAX = 6.0;   // 高度最大值 (检测高架桥等)

    // 地面过滤参数
    double lidar_pitch_rad_;
    bool use_ransac_ground_filter_;
    double ground_threshold_;
    double angle_threshold_rad_;
    double vehicle_height_;

    void points_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        try
        {
            // 转换ROS2消息到PCL点云
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
            pcl::fromROSMsg(*msg, *cloud);

            // 检查点云是否为空
            if (cloud->empty())
            {
                RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                                     "接收到空点云数据");
                return;
            }

            // 首先进行区域裁剪
            pcl::CropBox<pcl::PointXYZ> crop_filter;
            crop_filter.setInputCloud(cloud);
            crop_filter.setMin(Eigen::Vector4f(X_MIN, -Y_RANGE, Z_MIN, 1.0f));
            crop_filter.setMax(Eigen::Vector4f(X_MAX, Y_RANGE, Z_MAX, 1.0f));

            pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>());
            crop_filter.filter(*cropped_cloud);

            if (cropped_cloud->empty())
            {
                handle_empty_cloud();
                return;
            }

            // 地面过滤
            pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud = filter_ground_points(cropped_cloud);

            process_filtered_cloud(filtered_cloud);
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                                  "点云处理错误: %s", e.what());
        }
    }

    // 地面点过滤函数
    pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_points(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud)
    {
        if (use_ransac_ground_filter_)
        {
            return filter_ground_ransac(input_cloud);
        }
        else
        {
            return filter_ground_height_angle(input_cloud);
        }
    }

    // 基于RANSAC的地面点过滤
    pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_ransac(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud)
    {
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

        // 创建RANSAC分割器
        pcl::SACSegmentation<pcl::PointXYZ> seg;
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setDistanceThreshold(ground_threshold_);
        seg.setMaxIterations(100);

        seg.setInputCloud(input_cloud);
        seg.segment(*inliers, *coefficients);

        // 提取非地面点
        pcl::PointCloud<pcl::PointXYZ>::Ptr non_ground_cloud(new pcl::PointCloud<pcl::PointXYZ>());

        if (inliers->indices.size() > 0)
        {
            pcl::ExtractIndices<pcl::PointXYZ> extract;
            extract.setInputCloud(input_cloud);
            extract.setIndices(inliers);
            extract.setNegative(true); // 提取非地面点
            extract.filter(*non_ground_cloud);

            // RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
            //                       "RANSAC地面过滤: 总点数=%zu, 地面点数=%zu, 障碍物点数=%zu",
            //                       input_cloud->size(), inliers->indices.size(), non_ground_cloud->size());
        }
        else
        {
            *non_ground_cloud = *input_cloud; // 如果没有找到地面，保留所有点
        }

        return non_ground_cloud;
    }

    // 基于高度和角度的地面点过滤
    pcl::PointCloud<pcl::PointXYZ>::Ptr filter_ground_height_angle(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr non_ground_cloud(new pcl::PointCloud<pcl::PointXYZ>());

        size_t ground_points_filtered = 0;

        for (const auto &point : *input_cloud)
        {
            if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z))
                continue;

            // 坐标修正：绕Y轴旋转补偿俯仰角
            double x_corr = point.x * std::cos(lidar_pitch_rad_) + point.z * std::sin(lidar_pitch_rad_);
            double z_corr = -point.x * std::sin(lidar_pitch_rad_) + point.z * std::cos(lidar_pitch_rad_);

            // 计算点到激光雷达的水平距离
            double horizontal_distance = std::sqrt(x_corr * x_corr + point.y * point.y);

            // 避免除零，小巴车雷达高度调整最小距离
            if (horizontal_distance < 0.5)
            {
                non_ground_cloud->push_back(point);
                continue;
            }

            // 计算相对于水平面的角度（考虑车辆高度）
            double height_diff = z_corr + vehicle_height_; // 调整高度基准
            double angle_to_point = std::atan2(std::abs(height_diff), horizontal_distance);

            // 地面过滤条件 - 针对2.4m高度优化
            bool is_ground = false;

            // 条件1：高度过滤 - 考虑2.4m雷达高度，地面点应该在-2.1m以下
            if (z_corr < -2.1)
            {
                is_ground = true;
            }
            // 条件2：角度过滤 - 小巴车雷达高，角度阈值可以更严格
            else if (angle_to_point < angle_threshold_rad_)
            {
                is_ground = true;
            }
            // 条件3：对于较远距离，适当放宽高度限制（路面起伏）
            else if (horizontal_distance > 10.0 && z_corr < -1.8)
            {
                is_ground = true;
            }

            if (!is_ground)
            {
                non_ground_cloud->push_back(point);
            }
            else
            {
                ground_points_filtered++;
            }
        }

        // RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
        //                       "高度角度地面过滤: 总点数=%zu, 过滤地面点数=%zu, 障碍物点数=%zu",
        //                       input_cloud->size(), ground_points_filtered, non_ground_cloud->size());

        return non_ground_cloud;
    }

    void handle_empty_cloud()
    {
        // 无障碍物时的处理
        min_distance_.store(INFINITY);

        std::lock_guard<std::mutex> lock(msg_mutex_);
        cached_message_.obstacle_m = INT_MAX;

        RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
                              "检测范围内无点云数据");
    }

    void process_filtered_cloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &filtered_cloud)
    {
        if (filtered_cloud->empty())
        {
            handle_empty_cloud();
            return;
        }

        double current_min_distance = INFINITY;
        for (const auto &point : *filtered_cloud)
        {
            if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z))
                continue;

            // 坐标修正：绕Y轴旋转补偿俯仰角
            double x_corr = point.x * std::cos(lidar_pitch_rad_) + point.z * std::sin(lidar_pitch_rad_);

            // 只关心水平方向的距离，确保是前方的障碍物
            if (x_corr > 0 && x_corr < current_min_distance)
            {
                current_min_distance = x_corr;
            }
        }

        // 更新最近距离和缓存消息
        if (current_min_distance != INFINITY)
        {
            min_distance_.store(current_min_distance);

            std::lock_guard<std::mutex> lock(msg_mutex_);
            cached_message_.obstacle_m = static_cast<int>(current_min_distance * 10); // 转换为分米
        }
        else
        {
            // 过滤后无有效障碍物
            min_distance_.store(INFINITY);
            std::lock_guard<std::mutex> lock(msg_mutex_);
            cached_message_.obstacle_m = INT_MAX;
        }
    }

    void publish_message()
    {
        std::lock_guard<std::mutex> lock(msg_mutex_);
        command_publisher_->publish(cached_message_);

        if (cached_message_.obstacle_m == INT_MAX)
        {
            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
                                 "无障碍物检测");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "检测到障碍物: 距离=%d分米", cached_message_.obstacle_m);
        }
    }

    // 成员变量声明
    rclcpp::Publisher<interface::msg::Lidar>::SharedPtr command_publisher_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr points_subscriber_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
    // 初始化ROS2
    rclcpp::init(argc, argv);

    // 创建并运行节点
    auto node = std::make_shared<LidarNode>("lidar_node");
    rclcpp::spin(node);

    // 关闭ROS2
    rclcpp::shutdown();
    return 0;
}