#include "mypcl.h"

namespace ai_matrix
{
    MyPcl *MyPcl::ins = nullptr;
    MyPcl::GarbageCollector MyPcl::gc;
    std::mutex MyPcl::mx;

    MyPcl *MyPcl::getins()
    {
        //双层锁，确保线程安全
        if (ins == nullptr)
        {
            std::lock_guard<std::mutex> guard(mx); //防止异常发生不能解锁
            if (ins == nullptr)
            {
                ins = new MyPcl();
            }
        }
        return ins;
    }

    void MyPcl::compressPCD(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, std::stringstream &compressedData)
    {
        bool showStatistics = false; //设置在标准设备上输出打印出压缩结果信息
        // 压缩选项详见 /io/include/pcl/compression/compression_profiles.h
        pcl::io::compression_Profiles_e compressionProfile = pcl::io::LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR;
        // 初始化压缩与解压缩对象，其中压缩对象需要设定压缩参数选项，解压缩按照数据源自行判断
        pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB> *PointCloudEncoder;
        PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB>(compressionProfile, showStatistics);
        PointCloudEncoder->encodePointCloud(cloud.makeShared(), compressedData); // 压缩点云
        delete (PointCloudEncoder);
    }

    void MyPcl::insert_plane_line(pcl::PointCloud<pcl::PointXYZRGB> &line, float x_min, float x_max, float y, float z, unsigned char r, unsigned char g, unsigned char b, float step)
    {
        int num = 0;
        for (float x = x_min; x <= x_max; x += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x;
            pt.y = y;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        line.width += num;
        line.height = 1;
    }

    void MyPcl::insert_load_line(pcl::PointCloud<pcl::PointXYZRGB> &line, float x_min, float x_max, float y_min, float y_max, float z, unsigned char r, unsigned char g, unsigned char b, float step)
    {
        int num = 0;
        for (float x = x_min; x <= x_max; x += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x;
            pt.y = y_min;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        for (float x = x_min; x <= x_max; x += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x;
            pt.y = y_max;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        for (float y = y_min; y <= y_max; y += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x_min;
            pt.y = y;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        for (float y = y_min; y <= y_max; y += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x_max;
            pt.y = y;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        line.width += num;
        line.height = 1;
    }

    void MyPcl::insert_bound_line(pcl::PointCloud<pcl::PointXYZRGB> &line, float y_min, float y_max, float z_min, float z_max, float x, unsigned char r, unsigned char g, unsigned char b, float step)
    {
        int num = 0;

        for (float y = y_min; y <= y_max; y += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x;
            pt.y = y;
            pt.z = z_max;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        for (float z = z_min; z <= z_max; z += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x;
            pt.y = y_min;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        for (float z = z_min; z <= z_max; z += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x;
            pt.y = y_max;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        line.width += num;
        line.height = 1;
    }

    void MyPcl::insert_object_line(pcl::PointCloud<pcl::PointXYZRGB> &line, float x_min, float x_max, float y, float z_min, float z_max, unsigned char r, unsigned char g, unsigned char b, float step)
    {
        int num = 0;

        for (float x = x_min; x <= x_max; x += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x;
            pt.y = y;
            pt.z = z_min;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        for (float x = x_min; x <= x_max; x += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x;
            pt.y = y;
            pt.z = z_max;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        for (float z = z_min; z <= z_max; z += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x_min;
            pt.y = y;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        for (float z = z_min; z <= z_max; z += step)
        {
            pcl::PointXYZRGB pt;
            pt.x = x_max;
            pt.y = y;
            pt.z = z;
            pt.r = r;
            pt.g = g;
            pt.b = b;

            line.points.push_back(pt);
            num++;
        }

        line.width += num;
        line.height = 1;
    }

    void MyPcl::coords_convert(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out, const Eigen::Matrix4f &tf, float thetaz)
    {
        Eigen::Matrix4f tf_use;

        //沿z轴旋转
        Eigen::Matrix4f tfz = Eigen::Matrix4f::Identity();
        tfz(0, 0) = cos(thetaz);
        tfz(0, 1) = -sin(thetaz);
        tfz(1, 0) = sin(thetaz);
        tfz(1, 1) = cos(thetaz);

        //y坐标乘以-1
        Eigen::Matrix4f tfy = Eigen::Matrix4f::Identity();
        tfy(0, 0) = -1;

        tf_use = tfz * tfy * tf; //一起旋转，效率更高

        pcl::transformPointCloud(cloud_in, cloud_out, tf_use);
    }

    void MyPcl::coords_convert(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out, float thetax, float thetay, float thetaz, float radarz, bool reverse)
    {
        //绕x轴旋转
        Eigen::Matrix4f tfx = Eigen::Matrix4f::Identity();
        tfx(1, 1) = cos(thetax);
        tfx(1, 2) = -sin(thetax);
        tfx(2, 1) = sin(thetax);
        tfx(2, 2) = cos(thetax);

        //绕y轴旋转
        Eigen::Matrix4f tfy = Eigen::Matrix4f::Identity();
        tfy(0, 0) = cos(thetay);
        tfy(0, 2) = sin(thetay);
        tfy(2, 0) = -sin(thetay);
        tfy(2, 2) = cos(thetay);

        //沿z轴旋转
        Eigen::Matrix4f tfz = Eigen::Matrix4f::Identity();
        tfz(0, 0) = cos(thetaz);
        tfz(0, 1) = -sin(thetaz);
        tfz(1, 0) = sin(thetaz);
        tfz(1, 1) = cos(thetaz);

        Eigen::Matrix4f tf_use = tfz * tfy * tfx; //一起旋转，效率更高
        tf_use(2, 3) = radarz;                    //雷达高度

        Eigen::Matrix4f tf_use_inv = tf_use.inverse(); //求逆矩阵

        if (reverse)
        {
            pcl::transformPointCloud(cloud_in, cloud_out, tf_use_inv);
        }
        else
        {
            pcl::transformPointCloud(cloud_in, cloud_out, tf_use);
        }
    }

    void MyPcl::cube_cloud(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, std::vector<int> &indices, const Eigen::Vector3f &min, const Eigen::Vector3f &max)
    {
        //pcl::CropBox<pcl::PointXYZ> crop;
        //crop.setMin(Eigen::Vector4f(min(0), min(1), min(2), 1.0));
        //crop.setMax(Eigen::Vector4f(max(0), max(1), max(2), 1.0));
        //crop.setInputCloud(cloud_in.makeShared());
        //crop.setNegative(false); //默认为false，代表取立方体内的点
        //crop.filter(indices);
        for(int i = 0 ; i < cloud_in.points.size(); i ++)
        {
            if (min[0] < cloud_in.points[i].x && cloud_in.points[i].x < max[0]
                && min[1] < cloud_in.points[i].y && cloud_in.points[i].y < max[1]
                && min[2] < cloud_in.points[i].z && cloud_in.points[i].z < max[2])
            {
                indices.push_back(i);
            }
        }
    }

    void MyPcl::cube_cloud(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out, const Eigen::Vector3f &min, const Eigen::Vector3f &max)
    {
        //pcl::CropBox<pcl::PointXYZ> crop;
        //crop.setMin(Eigen::Vector4f(min(0), min(1), min(2), 1.0));
        //crop.setMax(Eigen::Vector4f(max(0), max(1), max(2), 1.0));
        //crop.setInputCloud(cloud_in.makeShared());
        //crop.setNegative(false); //默认为false，代表取立方体内的点
        //crop.filter(cloud_out);

        cloud_out.height = 1;       //无序点云
        cloud_out.is_dense = false; //加上，防止出错
        for (int i = 0; i < cloud_in.points.size(); i++)
        {
            if (min[0] < cloud_in.points[i].x && cloud_in.points[i].x < max[0]
                && min[1] < cloud_in.points[i].y && cloud_in.points[i].y < max[1]
                && min[2] < cloud_in.points[i].z && cloud_in.points[i].z < max[2])
            {
                cloud_out.points.push_back(cloud_in.points[i]);
            }
        }
    }

    void MyPcl::cube_cloud(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out, std::vector<int> &indices, const Eigen::Vector3f &min, const Eigen::Vector3f &max)
    {
        //pcl::CropBox<pcl::PointXYZ> crop;
        //crop.setMin(Eigen::Vector4f(min(0), min(1), min(2), 1.0));
        //crop.setMax(Eigen::Vector4f(max(0), max(1), max(2), 1.0));
        //crop.setInputCloud(cloud_in.makeShared());
        //crop.setNegative(false); //默认为false，代表取立方体内的点
        //crop.filter(cloud_out);
        //crop.filter(indices);
        cloud_out.height = 1;       //无序点云
        cloud_out.is_dense = false; //加上，防止出错
        for (int i = 0; i < cloud_in.points.size(); i++)
        {
            if (min[0] < cloud_in.points[i].x && cloud_in.points[i].x < max[0]
                && min[1] < cloud_in.points[i].y && cloud_in.points[i].y < max[1]
                && min[2] < cloud_in.points[i].z && cloud_in.points[i].z < max[2])
            {
                cloud_out.points.push_back(cloud_in.points[i]);
                indices.push_back(i);
            }
        }
    }
}