#include "lidarmerge.h"

#include <QDateTime>

#include <QMutex>

extern QMutex gMutex;

static int g_seq = 0;

pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvectorlidar)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());

    point_cloud->header.frame_id = "velodyne";
    point_cloud->height = 1;
    point_cloud->header.stamp = QDateTime::currentMSecsSinceEpoch();
    point_cloud->width = 0;
    point_cloud->header.seq =g_seq;
    g_seq++;


    int i,j;
    for(i=0;i<xvectorlidar.size();i++)
    {
        Eigen::Matrix3d rotation_matrix;
        Eigen::Vector3d trans_matrix;
        Eigen::AngleAxisd r_z ( xvectorlidar[i].fang_z/180.0*M_PI, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw
        Eigen::AngleAxisd r_y ( xvectorlidar[i].fang_y/180.0*M_PI, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll
        Eigen::AngleAxisd r_x ( xvectorlidar[i].fang_x/180.0*M_PI, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch
        Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序
        // 四元数-->>旋转矩阵
        rotation_matrix = q_zyx.toRotationMatrix();
        trans_matrix << xvectorlidar[i].foff_x, xvectorlidar[i].foff_y, xvectorlidar[i].foff_z;//x,y,z

        int nsize;

        pcl::PointCloud<pcl::PointXYZI>::Ptr partpoint_cloud;
        int64_t curtime = QDateTime::currentMSecsSinceEpoch();
        if((xvectorlidar[i].bUpdate)||((curtime -xvectorlidar[i].mupdatetime)<150))
        {
            gMutex.lock();
            partpoint_cloud = xvectorlidar[i].mpoint_cloud;
            gMutex.unlock();
            nsize = partpoint_cloud->width;
            qDebug("size is %d ",nsize);
            for(j=0;j<nsize;j++)
            {
                pcl::PointXYZI point;
                point = partpoint_cloud->at(j);
                Eigen::Vector3d new_point, old_point;
                old_point<<point.x, point.y, point.z;
                new_point = rotation_matrix * (old_point) + trans_matrix;
                point.x = new_point[0];
                point.y = new_point[1];
                point.z = new_point[2];

                if(point.x>xvectorlidar[i].fmax_x)continue;
                if(point.y>xvectorlidar[i].fmax_y)continue;
                if(point.z>xvectorlidar[i].fmax_z)continue;
                if(point.z > xvectorlidar[i].fignore_zmax) continue;
                if(point.z < xvectorlidar[i].fignore_zmin) continue;
//                std::cout<<"  fignore  zmin  "<<xvectorlidar[i].fignore_zmin<<std::endl;

                if(point.x<xvectorlidar[i].fmin_x)continue;
                if(point.y<xvectorlidar[i].fmin_y)continue;
                if(point.z<xvectorlidar[i].fmin_z)continue;

                if((point.x>xvectorlidar[i].fignore_xmin)&&(point.x<xvectorlidar[i].fignore_xmax)&&(point.y>xvectorlidar[i].fignore_ymin)&&(point.y<xvectorlidar[i].fignore_ymax))
                {
                    continue;
                }
                if(!pcl_isfinite(point.x)||!pcl_isfinite(point.y)||!pcl_isfinite(point.z)||!pcl_isfinite(point.intensity)) continue;

                point_cloud->push_back(point);
                ++point_cloud->width;
            }


        }
    }
    qDebug("merge size is %d ",point_cloud->width);
    return point_cloud;
}