#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <QCoreApplication>

#include "object.pb.h"
#include "objectarray.pb.h"

#include <thread>
#include <chrono>


#include "modulecomm.h"
#include "cluster.h"

#include <getopt.h>
#include <string>
#include <eigen3/Eigen/Eigen>
using namespace Eigen;
using namespace std;

char gstr_memname[256];


void print_useage()
{
    std::cout<<" -m --memname $mappath : share memory name. eq.  -m lidar_pc"<<std::endl;
    std::cout<<" -h --help print help"<<std::endl;
}

std::vector<Obj> Objresult;

void Listenlidarcnndectect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

//    std::vector<iv::lidar::object>  lidarobjvec;
//    strtolidarobj(lidarobjvec,strdata,nSize);

    iv::lidar::objectarray lidarobjvec;
    std::string in;
    in.append(strdata,nSize);
    lidarobjvec.ParseFromString(in);

    std::cout<<"lidar obj num: "<<lidarobjvec.obj_size()<<std::endl;
    Obj result;
    Objresult.clear();
    for(int i=0; i<lidarobjvec.obj_size();i++){
        iv::lidar::lidarobject lidarobj = lidarobjvec.obj(i);
        result.center_x = lidarobj.position().x();
        result.center_y = lidarobj.position().y();
        result.center_z = lidarobj.position().z();
        result.l = lidarobj.dimensions().x();
        result.w = lidarobj.dimensions().y();
        result.h = lidarobj.dimensions().z();
        result.yaw = lidarobj.tyaw();
        result.type_name = lidarobj.type_name();

        Eigen::AngleAxisf rotation_vector(-lidarobj.tyaw(), Eigen::Vector3f::UnitZ());
        result.Quaternion = Eigen::Quaternionf(rotation_vector);
        Objresult.push_back(result);
    }
}


int  GetOptLong(int argc, char *argv[]) {
    int nRtn = 0;
    int opt; // getopt_long() 的返回值
    int digit_optind = 0; // 设置短参数类型及是否需要参数

    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
    // 第几个元素的描述，即是long_opts的下标值
    int option_index = 0;
    // 设置短参数类型及是否需要参数
    const char *optstring = "m:h";

    // 设置长参数类型及其简写，比如 --reqarg <==>-r
    /*
    struct option {
             const char * name;  // 参数的名称
             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
        };
    其中：
        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
     */
    static struct option long_options[] = {
        {"memname", required_argument, NULL, 'm'},
        {"help",  no_argument,       NULL, 'h'},
 //       {"optarg", optional_argument, NULL, 'o'},
        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
    };

    while ( (opt = getopt_long(argc,
                               argv,
                               optstring,
                               long_options,
                               &option_index)) != -1) {
//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
//        printf("optarg = %s\n", optarg); // 参数内容
//        printf("optind = %d\n", optind); // 下一个被处理的下标值
//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
//        printf("\n");
        switch(opt)
        {
        case 'm':
            strncpy(gstr_memname,optarg,255);
            break;
        case 'h':
            print_useage();
            nRtn = 1; //because use -h
            break;
        default:
            break;
        }

    }

    return nRtn;
}


pcl::visualization::CloudViewer viewer("Cloud Viewer");//创建viewer对象


int user_data;

// // read source lidar_pointcloud
//void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * 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;
//    }

//    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;
//    pcl::PointXYZI * p;
//    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;
//        point_cloud->push_back(xp);
//        p++;

//    }

//    user_data++;
//    viewer.showCloud(point_cloud);
//}

// read segmentation_cnn output lidar_pointcloud
void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::lidar::objectarray lidarobjvec;
    std::string in;
    in.append(strdata,nSize);
    lidarobjvec.ParseFromString(in);

    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());
    for(int i=0; i<lidarobjvec.obj_size();i++){
        iv::lidar::lidarobject lidarobj = lidarobjvec.obj(i);
        if(lidarobj.type_name()!="car" && lidarobj.type_name()!="pedestrian")
            continue;
        for(int j=0;j<lidarobj.cloud_size();j++){
//            iv::lidar::PointXYZI Point = lidarobj.cloud(j);
            pcl::PointXYZI xp;
            xp.x = lidarobj.cloud(j).x();
            xp.y = lidarobj.cloud(j).y();
            xp.z = lidarobj.cloud(j).z();
            xp.intensity = lidarobj.cloud(j).i();
            point_cloud->push_back(xp);
        }
    }
    viewer.showCloud(point_cloud);
}


void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{

    viewer.setBackgroundColor(0.0,0.0,0.0);

}



void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
    static unsigned count = 0;
    std::stringstream ss;

    ss << "Point Cloud Count: " << user_data;
    viewer.removeShape ("text", 0);
    viewer.addText (ss.str(), 200, 300, "text", 0);

    viewer.removeAllShapes();
    viewer.addCube(-0.9,0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);

    for(int i=0;i<Objresult.size();i++){

        Eigen::Vector3f center(Objresult[i].center_x,Objresult[i].center_y,Objresult[i].center_z);
        std::string cube = "box"+std::to_string(i);
        viewer.addCube(center, Objresult[i].Quaternion,Objresult[i].w,Objresult[i].h,Objresult[i].l, cube);
        viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube);
        viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, cube);
        viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1, cube);
        std::string cubeFill = "boxFill"+std::to_string(i);
        viewer.addCube(center, Objresult[i].Quaternion,Objresult[i].w,Objresult[i].h,Objresult[i].l, cubeFill);
        viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cubeFill);
        viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, cubeFill);
        viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, cubeFill);
    }
}



int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    snprintf(gstr_memname,255,"lidar_pointpillar");

    int nRtn = GetOptLong(argc,argv);
    if(nRtn == 1)  //show help,so exit.
    {
        return 0;
    }


    //This will only get called once
    viewer.runOnVisualizationThreadOnce (viewerOneOff);

    //This will get called once per visualization iteration
    viewer.runOnVisualizationThread (viewerPsycho);

    //std::string gstrinput = "lidar_pointpillar";
    std::string gstrinput = "lidar_track";
    void * pa = iv::modulecomm::RegisterRecv(gstr_memname,ListenPointCloud);
    void * gpadetect = iv::modulecomm::RegisterRecv(gstrinput.data(),Listenlidarcnndectect);

    while(!viewer.wasStopped())
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    return 0;


    return a.exec();

}