#include <QCoreApplication>


#include <pcl/io/pcd_io.h>

#include <chrono>
#include <thread>

#include "lidar_apollo_instance_segmentation/detector.hpp"

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

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


void * gpa;
void * gpdetect;

LidarApolloInstanceSegmentation * gseg;

void test()
{

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


    pcl::PointCloud<pcl::PointXYZI>::Ptr xpcfile;

    xpcfile = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>>(new pcl::PointCloud<pcl::PointXYZI>);

    if(0 == pcl::io::loadPCDFile("/home/nvidia/share/middle.pcd",*xpc))
    {

    }
    else
    {
        std::cout<<"load pcd fail"<<std::endl;
        return;
    }

    int i;
    for(i=0;i<10;i++)
    {

    int64_t time1 =  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();



    std::vector<Obstacle>  objvec;
    gseg->detectDynamicObjects(
                xpc,
                objvec);


    int64_t time2 =  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

    std::cout<<"complete test. use "<<(time2 - time1)/1000<< std::endl;

//    std::this_thread::sleep_for(std::chrono::milliseconds(80));

    }
    std::cout<<"test complete."<<std::endl;
}

std::string gstrinput;
std::string gstroutput;

static iv::lidar::PointXYZ pcltoprotopoint(pcl::PointXYZ x)
{
    iv::lidar::PointXYZ lx;
    lx.set_x(x.x);
    lx.set_y(x.y);
    lx.set_z(x.z);
    return lx;
}

static void GetLidarObj(std::vector<Obstacle> objvec,iv::lidar::objectarray & lidarobjvec)
{
    int i;

    for(i=0;i<objvec.size();i++)
    {
        Obstacle x;
        iv::lidar::lidarobject lidarobj;
  //      iv::lidar::object xo;

        x = objvec.at(i);
        pcl::PointCloud<pcl::PointXYZI> in_cluster = *x.cloud_ptr;



        float min_x = std::numeric_limits<float>::max();
        float max_x = -std::numeric_limits<float>::max();
        float min_y = std::numeric_limits<float>::max();
        float max_y = -std::numeric_limits<float>::max();
        float min_z = std::numeric_limits<float>::max();
        float max_z = -std::numeric_limits<float>::max();
        float average_x = 0, average_y = 0, average_z = 0;
        float length, width, height;

        pcl::PointXYZ min_point;
        pcl::PointXYZ max_point;
  //      pcl::PointXYZ average_point;
        pcl::PointXYZ centroid;

        centroid.x = 0;
        centroid.y = 0;
        centroid.z = 0;

        for (auto pit = in_cluster.points.begin(); pit != in_cluster.points.end(); ++pit)
        {
            average_x += pit->x;
            average_y += pit->y;
            average_z += pit->z;
            centroid.x += pit->x;
            centroid.y += pit->y;
            centroid.z += pit->z;

            if (pit->x < min_x)
                min_x = pit->x;
            if (pit->y < min_y)
                min_y = pit->y;
            if (pit->z < min_z)
                min_z = pit->z;
            if (pit->x > max_x)
                max_x = pit->x;
            if (pit->y > max_y)
                max_y = pit->y;
            if (pit->z > max_z)
                max_z = pit->z;
        }

        // min, max points
        min_point.x = min_x;
        min_point.y = min_y;
        min_point.z = min_z;
        max_point.x = max_x;
        max_point.y = max_y;
        max_point.z = max_z;

        if (in_cluster.points.size() > 0)
        {
            centroid.x /= in_cluster.points.size();
            centroid.y /= in_cluster.points.size();
            centroid.z /= in_cluster.points.size();
        }
        length = max_point.x - min_point.x;
        width = max_point.y - min_point.y;
        height = max_point.z - min_point.z;

        iv::lidar::PointXYZ lx;
        iv::lidar::PointXYZ * plx;
        lx = pcltoprotopoint(centroid);
        plx = lidarobj.mutable_centroid();
        plx->CopyFrom(lx);
        lx = pcltoprotopoint(min_point);
        plx = lidarobj.mutable_min_point();
        plx->CopyFrom(lx);
        lx = pcltoprotopoint(max_point);
        plx = lidarobj.mutable_max_point();
        plx->CopyFrom(lx);




//        lidarobj.centroid = centroid;
//        lidarobj.min_point = min_point;
//        lidarobj.max_point = max_point;


        lx.set_x(min_point.x + length / 2);
        lx.set_y(min_point.y + width / 2);
        lx.set_z(min_point.z + height / 2);

        plx = lidarobj.mutable_position();
        plx->CopyFrom(lx);

//        lidarobj.position.x = min_point.x + length / 2;
//        lidarobj.position.y = min_point.y + width / 2;
//        lidarobj.position.z = min_point.z + height / 2;
        iv::lidar::Dimension ld;
        iv::lidar::Dimension * pld;
        ld.set_x((length < 0) ? -1 * length : length);
        ld.set_y((width < 0) ? -1 * width : width);
        ld.set_z((height < 0) ? -1 * height : height);
//        lidarobj.dimensions.x = ((length < 0) ? -1 * length : length);
//        lidarobj.dimensions.y = ((width < 0) ? -1 * width : width);
//        lidarobj.dimensions.z = ((height < 0) ? -1 * height : height);
        pld = lidarobj.mutable_dimensions();
        pld->CopyFrom(ld);

        lidarobj.set_mntype(x.meta_type);
        lidarobj.set_score(x.score);


        lidarobj.set_type_name(x.GetTypeString());

        int j;
        for(j=0;j<x.meta_type_probs.size();j++)
        {
            lidarobj.add_type_probs(x.meta_type_probs.at(j));
        }

        for(j=0;j<x.cloud_ptr->size();j++)
        {
            iv::lidar::PointXYZI * pcp = lidarobj.add_cloud();
            pcl::PointXYZI pp = x.cloud_ptr->at(j);
            pcp->set_x(pp.x);
            pcp->set_y(pp.y);
            pcp->set_z(pp.z);
            pcp->set_i(pp.intensity);
        }

        iv::lidar::lidarobject * po = lidarobjvec.add_obj();
        po->CopyFrom(lidarobj);

//        givlog->verbose("CNNOBJ","    object %d type:%s x:%6.3f y:%6.3f",i,
//                        lidarobj.type_name().data(),lidarobj.position().x(),
//                        lidarobj.position().y());


//        lidarobj.mnType = x.meta_type;

//        lidarobj.score = x.score;
//        lidarobj.type_probs = x.meta_type_probs;
//        lidarobj.cloud_ptr = x.cloud_ptr;

//        int ndatalen;
//        char * strx = lidarobj.serialize(ndatalen);

//        lidarobj.unserialize(strx,ndatalen);
//        delete strx;

//        lidarobjvec.push_back(lidarobj);

    }
}


void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
{
    std::cout<<"pcd size is "<<pc_ptr->size()<<std::endl;
    pcl::PointIndices valid_idx;
    auto &indices = valid_idx.indices;
    indices.resize(pc_ptr->size());
    std::iota(indices.begin(), indices.end(), 0);

    std::vector<Obstacle> objvec;

    gseg->detectDynamicObjects(
                pc_ptr,
                objvec);

    std::cout<<"obj size is "<<objvec.size()<<std::endl;

//    std::vector<iv::lidar::lidarobject> lidarobjvec;
    iv::lidar::objectarray lidarobjvec;
    GetLidarObj(objvec,lidarobjvec);

    double timex = pc_ptr->header.stamp;
    timex = timex/1000.0;
    lidarobjvec.set_timestamp(pc_ptr->header.stamp);

    int ntlen;
    std::string out = lidarobjvec.SerializeAsString();
 //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
    iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
  //  delete strout;

//    int i;
//    for(i=0;i<objvec.size();i++)
//    {
//        Obstacle x;
//        x = objvec.at(i);

//        std::cout<<"obj "<<i<<" x is "<<x.GetTypeString()<<std::endl;

//        iv::lidar::lidarobject y;
//        y = lidarobjvec.at(i);
//        std::cout<<"     "<<" x is"<<y.position.x<<" y is "<<y.position.y<<" len x is "<<y.dimensions.x<<" len y is "<<y.dimensions.y<<std::endl;
//        std::cout<<"     "<<" type is "<<y.mnType<<std::endl;
//    }
}

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;
    }

    QTime xTime;
    xTime.start();

    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;
//        if((p->x<-30)||(p->x>30)||(p->y>50)||(p->y<-50))
//        {

//        }
//        else
//        {
            memcpy(&xp,p,sizeof(pcl::PointXYZI));
            xp.z = xp.z + (-1.0) ;
            point_cloud->push_back(xp);

//        }
        p++;
//        xp.x = p->x;
//        xp.y = p->y;
//        xp.z = p->z;
//        xp.intensity = p->intensity;
//        point_cloud->push_back(xp);
//        p++;
    }

//    std::cout<<"pcl time is "<<xTime.elapsed()<<std::endl;

    DectectOnePCD(point_cloud);
    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" "<<xTime.elapsed()<<std::endl;

}



//LidarApolloInstanceSegmentation

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

    gseg = new LidarApolloInstanceSegmentation();

 //   test();

    QString strpath = QCoreApplication::applicationDirPath();
    if(argc < 2)
        strpath = strpath + "/detection_lidar_cnn_segmentation.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

    iv::xmlparam::Xmlparam xparam(strpath.toStdString());

    gstrinput = xparam.GetParam("input","lidarpc_center");
    gstroutput = xparam.GetParam("output","lidar_cnn_dectect");


    gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
    gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(),10000000,1);

    return a.exec();
}