#include <QCoreApplication>


//sudo apt install libopen3d-dev

#include <open3d/Open3D.h>
#include <open3d/core/TensorFunction.h>
#include <open3d/visualization/visualizer/RenderOption.h>

#include <pcl/io/pcd_io.h>
#include <pcl/common/io.h>

#include <thread>

#include <QFile>

#include "modulecomm.h"
#include "ivversion.h"
#include "xmlparam.h"

char gstr_memname[256];

std::shared_ptr<open3d::geometry::PointCloud> gcloud_ptr = nullptr;
open3d::visualization::VisualizerWithKeyCallback gvis;

bool readpcd(std::string strpcdpath,open3d::t::geometry::PointCloud &cloud)
{
    open3d::core::Dtype dtype_f{open3d::core::Dtype::Float32};
    open3d::core::Device device_type{open3d::core::Device::DeviceType::CPU, 0};
    std::vector<float> intensities_buffer;
    std::vector<Eigen::Vector3d> points_buffer;

    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());

    pcl::io::loadPCDFile<pcl::PointXYZI>(strpcdpath,*point_cloud);

    int i;
    int nsize = static_cast<int>(point_cloud->width);
    for(i=0;i<nsize;i++)
    {
        float x,y,z,intens;
        x = point_cloud->points[i].x;
        y = point_cloud->points[i].y;
        z = point_cloud->points[i].z;
        intens = point_cloud->points[i].intensity;
        points_buffer.push_back(Eigen::Vector3d(x, y, z));
        intensities_buffer.push_back(intens);
    }

    std::vector<float> times_buffer(points_buffer.size(), 0.0f);
    open3d::core::Tensor positions =
        open3d::core::eigen_converter::EigenVector3dVectorToTensor(
            points_buffer, dtype_f, device_type);
    cloud.SetPointPositions(positions);
    auto intensity = open3d::core::Tensor(
        intensities_buffer, {1, static_cast<int>(intensities_buffer.size())},
        open3d::core::Dtype::Float32);
    auto time = open3d::core::Tensor(times_buffer,
                                     {1, static_cast<int>(times_buffer.size())},
                                     open3d::core::Dtype::Float32);
    cloud.SetPointAttr("intensity", intensity);
    cloud.SetPointAttr("time", time);
    return true;
}

void test()
{
    auto cloud_ptr = std::make_shared<open3d::t::geometry::PointCloud>();
    if (readpcd(std::string("/home/nvidia/1.pcd"), *cloud_ptr)) {
        auto positions = cloud_ptr->GetPointPositions();
        auto intensities = cloud_ptr->GetPointAttr("intensity").Reshape({-1, 1});
        auto times = cloud_ptr->GetPointAttr("time").Reshape({-1, 1});
        auto concat = open3d::core::Concatenate({positions, intensities, times}, 1);
        float *d_points = nullptr;
        int num_points = cloud_ptr->ToLegacy().points_.size();
        float *tmp = (float *)concat.GetDataPtr();
//        open3d::visualization::VisualizerWithKeyCallback vis;
//        vis.CreateVisualizerWindow("Point Cloud Viewer", 1600, 900);
//        vis.GetRenderOption().background_color_ = {0.0, 0.0, 0.0};
//        vis.GetRenderOption().point_color_option_ =
//            open3d::visualization::RenderOption::PointColorOption::Color;
//        vis.GetRenderOption().point_size_ = 1.0;
        gvis.AddGeometry(std::make_shared<open3d::geometry::PointCloud>(
            cloud_ptr->ToLegacy().PaintUniformColor({1.0, 1.0, 1.0})));



 //       gvis.Run();
 //       gvis.DestroyVisualizerWindow();
    }
}


void threadvis()
{
    test();
    std::cout<<" vis close."<<std::endl;
}


void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    if(nSize <=16)return;
    unsigned int * pHeadSize = (unsigned int *)strdata;
    if(*pHeadSize > nSize)
    {
        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
    }

    open3d::core::Dtype dtype_f{open3d::core::Dtype::Float32};
    open3d::core::Device device_type{open3d::core::Device::DeviceType::CPU, 0};
    std::vector<float> intensities_buffer;
    std::vector<Eigen::Vector3d> points_buffer;
    open3d::t::geometry::PointCloud cloud;

    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());
    int nNameSize;
    nNameSize = *pHeadSize - 4-4-8;
    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
    point_cloud->header.frame_id = strName;
    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
    int i;
    point_cloud->height = 1;
    point_cloud->width = 0;
    pcl::PointXYZI * p;
    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<"ms size : "<<nPCount<<std::endl;
    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZI xp;
        xp.x = p->x;
        xp.y = p->y;
        xp.z = p->z;
        xp.intensity = p->intensity;

        float x,y,z,intens;
        x = p->x;
        y = p->y;
        z = p->z;
        intens = p->intensity;
        points_buffer.push_back(Eigen::Vector3d(x, y, z));
        intensities_buffer.push_back(intens);

//        if(xp.intensity>10)
//        {
        point_cloud->push_back(xp);

        ++point_cloud->width;
//        }
         p++;

  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
    }

    std::vector<float> times_buffer(points_buffer.size(), 0.0f);
    open3d::core::Tensor positions =
        open3d::core::eigen_converter::EigenVector3dVectorToTensor(
            points_buffer, dtype_f, device_type);
    cloud.SetPointPositions(positions);
    auto intensity = open3d::core::Tensor(
        intensities_buffer, {1, static_cast<int>(intensities_buffer.size())},
        open3d::core::Dtype::Float32);
    auto time = open3d::core::Tensor(times_buffer,
                                     {1, static_cast<int>(times_buffer.size())},
                                     open3d::core::Dtype::Float32);
    cloud.SetPointAttr("intensity", intensity);
    cloud.SetPointAttr("time", time);

    if(gcloud_ptr != nullptr)
    {
//        gvis.RemoveGeometry(gcloud_ptr);
        gcloud_ptr = std::make_shared<open3d::geometry::PointCloud>(
                    cloud.ToLegacy().PaintUniformColor({1.0, 1.0, 1.0}));
        gvis.AddGeometry(gcloud_ptr);

    }
    else
    {
        gvis.RemoveGeometry(gcloud_ptr);
        gcloud_ptr = std::make_shared<open3d::geometry::PointCloud>(
                    cloud.ToLegacy().PaintUniformColor({1.0, 1.0, 1.0}));
        gvis.AddGeometry(gcloud_ptr);//gvis.UpdateGeometry(gcloud_ptr);
        gvis.UpdateRender();
    }





 //   return;


 //   gw->UpdatePointCloud(point_cloud);
 //   DBSCAN_PC(point_cloud);


}




int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    snprintf(gstr_memname,255,"lidar_pc");


    void * pa = iv::modulecomm::RegisterRecv(gstr_memname,ListenPointCloud);

    gvis.CreateVisualizerWindow("Point Cloud Viewer", 1600, 900);

    open3d::visualization::gui::SceneWidget;
    gvis.GetRenderOption().background_color_ = {0.0, 0.0, 0.0};
    gvis.GetRenderOption().point_color_option_ =
        open3d::visualization::RenderOption::PointColorOption::Color;
    gvis.GetRenderOption().point_size_ = 1.0;

//    std::thread * pt = new std::thread(&threadvis);

    gvis.Run();
    gvis.DestroyVisualizerWindow();

    return 0;

    while(1)
    {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        std::cout<<"main."<<std::endl;
    }


//    test();
    return a.exec();
}