#include <QCoreApplication>


#include <iostream>
#include <sstream>
#include <fstream>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include "cuda_runtime.h"

#include "./params.h"
#include "./pointpillar.h"

#include "modulecomm.h"

std::string Model_File = "/home/nvidia/models/lidar/pointpillar.onnx";

PointPillar * gpPointPillar;

cudaEvent_t start, stop;
float elapsedTime = 0.0f;
cudaStream_t stream = NULL;
std::vector<Bndbox> nms_pred;

#define checkCudaErrors(status)                                   \
{                                                                 \
  if (status != 0)                                                \
  {                                                               \
    std::cout << "Cuda failure: " << cudaGetErrorString(status)   \
              << " at line " << __LINE__                          \
              << " in file " << __FILE__                          \
              << " error status: " << status                      \
              << std::endl;                                       \
              abort();                                            \
    }                                                             \
}

void Getinfo(void)
{
  cudaDeviceProp prop;

  int count = 0;
  cudaGetDeviceCount(&count);
  printf("\nGPU has cuda devices: %d\n", count);
  for (int i = 0; i < count; ++i) {
    cudaGetDeviceProperties(&prop, i);
    printf("----device id: %d info----\n", i);
    printf("  GPU : %s \n", prop.name);
    printf("  Capbility: %d.%d\n", prop.major, prop.minor);
    printf("  Global memory: %luMB\n", prop.totalGlobalMem >> 20);
    printf("  Const memory: %luKB\n", prop.totalConstMem  >> 10);
    printf("  SM in a block: %luKB\n", prop.sharedMemPerBlock >> 10);
    printf("  warp size: %d\n", prop.warpSize);
    printf("  threads in a block: %d\n", prop.maxThreadsPerBlock);
    printf("  block dim: (%d,%d,%d)\n", prop.maxThreadsDim[0], prop.maxThreadsDim[1], prop.maxThreadsDim[2]);
    printf("  grid dim: (%d,%d,%d)\n", prop.maxGridSize[0], prop.maxGridSize[1], prop.maxGridSize[2]);
  }
  printf("\n");
}


//void DectectOneBin(std::shared_ptr<float> & pbin_ptr,const int points_size)
void DectectOneBin(float * pdata,const int points_size)
{
    float *points_data = nullptr;
    unsigned int points_data_size = points_size * 4 * sizeof(float);
    checkCudaErrors(cudaMallocManaged((void **)&points_data, points_data_size));
 //   checkCudaErrors(cudaMemcpy(points_data, pbin_ptr.get(), points_data_size, cudaMemcpyDefault));
    checkCudaErrors(cudaMemcpy(points_data, pdata, points_data_size, cudaMemcpyDefault));
    checkCudaErrors(cudaDeviceSynchronize());

    cudaEventRecord(start, stream);

    gpPointPillar->doinfer(points_data, points_size, nms_pred);
    cudaEventRecord(stop, stream);
    cudaEventSynchronize(stop);
    cudaEventElapsedTime(&elapsedTime, start, stop);
    std::cout<<"TIME: pointpillar: "<< elapsedTime <<" ms." <<std::endl;

    checkCudaErrors(cudaFree(points_data));

    std::cout<<"Bndbox objs: "<< nms_pred.size()<<std::endl;
//    std::string save_file_name = Save_Dir + index_str + ".txt";
//    SaveBoxPred(nms_pred, save_file_name);

    for (const auto box : nms_pred) {

        if(box.id != 0)continue;
      std::cout << box.x << " ";
      std::cout << box.y << " ";
      std::cout << box.z << " ";
      std::cout << box.w << " ";
      std::cout << box.l << " ";
      std::cout << box.h << " ";
      std::cout << box.rt << " ";
      std::cout << box.id << " ";
      std::cout << box.score << " ";
      std::cout << std::endl;
    }

    nms_pred.clear();

    delete pdata;

    std::cout << ">>>>>>>>>>>" <<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;
    }


    int nNameSize;
    nNameSize = *pHeadSize - 4-4-8;
    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
    int i;
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);

    std::shared_ptr<float> pbin_ptr = std::shared_ptr<float>(new float[nPCount*4]);

    float * pdata = new float[nPCount * 4];// pbin_ptr.get();

    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZI xp;

        memcpy(&xp,p,sizeof(pcl::PointXYZI));
        xp.z = xp.z;

        pdata[4*i + 0]= static_cast<float>(xp.x);
        pdata[4*i + 1]= static_cast<float>(xp.y);
        pdata[4*i + 2]= static_cast<float>(xp.z);
        pdata[4*i + 3]= static_cast<float>(xp.intensity);

//        pdata[4*i + 0]= 0;
//        pdata[4*i + 1]= 0;
//        pdata[4*i + 2]= 0;
//        pdata[4*i + 3]= 0;

        p++;

    }

    DectectOneBin(pdata,nPCount);
 //   DectectOneBin(pbin_ptr,nPCount);



}


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

    Getinfo();



    checkCudaErrors(cudaEventCreate(&start));
    checkCudaErrors(cudaEventCreate(&stop));
    checkCudaErrors(cudaStreamCreate(&stream));

    Params params_;


    nms_pred.reserve(100);

    PointPillar pointpillar(Model_File, stream);

    gpPointPillar = &pointpillar;

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

    return a.exec();

    checkCudaErrors(cudaEventDestroy(start));
    checkCudaErrors(cudaEventDestroy(stop));
    checkCudaErrors(cudaStreamDestroy(stream));
}