#include "utils.h"

#include <cstddef>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>

pcl::PointCloud<pcl::PointXYZINormal>::Ptr Utils::livox2PCL(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg, int filter_num, double min_range, double max_range)
{
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZINormal>);
    int point_num = msg->point_num;
    cloud->reserve(point_num / filter_num + 1);
    for (int i = 0; i < point_num; i += filter_num)
    {
        if ((msg->points[i].line < 4) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
        {

            float x = msg->points[i].x;
            float y = msg->points[i].y;
            float z = msg->points[i].z;
            if (x * x + y * y + z * z < min_range * min_range || x * x + y * y + z * z > max_range * max_range)
                continue;
            pcl::PointXYZINormal p;
            p.x = x;
            p.y = y;
            p.z = z;
            p.intensity = msg->points[i].reflectivity;
            p.curvature = msg->points[i].offset_time / 1000000.0f;
            cloud->push_back(p);
        }
    }
    return cloud;
}

pcl::PointCloud<pcl::PointXYZINormal>::Ptr Utils::convertToXYZINormal(
    const sensor_msgs::msg::PointCloud2::SharedPtr& msg, int filter_num, double min_range, double max_range)
{
    // 先转成 PointXYZI（因为 ROS 点云通常带 intensity）
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_i(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*msg, *cloud_xyz_i);

    // (void)fileter_num; // 目前不使用 filter_num，直接拷贝所有点

    // 创建目标点云
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZINormal>);
    cloud_target->points.reserve(cloud_xyz_i->points.size());
    cloud_target->header = cloud_xyz_i->header;
    cloud_target->height = cloud_xyz_i->height;
    cloud_target->width = cloud_xyz_i->width;
    cloud_target->is_dense = cloud_xyz_i->is_dense;

    size_t used_point_count = 0;

    // 手动拷贝 + 法向量设为 0
    for (const auto& point : cloud_xyz_i->points)
    {
        pcl::PointXYZINormal p;
        p.x = point.x;
        p.y = point.y;
        p.z = point.z;

        if (p.x * p.x + p.y * p.y + p.z * p.z < min_range * min_range ||
            p.x * p.x + p.y * p.y + p.z * p.z > max_range * max_range)
          continue;

        used_point_count++;

        p.intensity = 0.0;
        p.normal_x = 0.0f;
        p.normal_y = 0.0f;
        p.normal_z = 0.0f;
        cloud_target->points.push_back(p);
    }

    cloud_target->points.resize(used_point_count);
    cloud_target->width = used_point_count;

    return cloud_target;
}

double Utils::getSec(std_msgs::msg::Header &header)
{
    return static_cast<double>(header.stamp.sec) + static_cast<double>(header.stamp.nanosec) * 1e-9;
}

builtin_interfaces::msg::Time Utils::getTime(const double &sec)
{
    builtin_interfaces::msg::Time time_msg;
    time_msg.sec = static_cast<int32_t>(sec);
    time_msg.nanosec = static_cast<uint32_t>((sec - time_msg.sec) * 1e9);
    return time_msg;
}
