#include <QCoreApplication>

#include <yaml-cpp/yaml.h>

#include <lidar_centerpoint/centerpoint_config.hpp>
#include <lidar_centerpoint/centerpoint_trt.hpp>
#include <lidar_centerpoint/preprocess/pointcloud_densification.hpp>

#include "objectarray.pb.h"


#include "modulecomm.h"

void * gpa;
void * gpdetect;


using namespace centerpoint;

std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};

std::vector<std::string> class_names_;

void LoadYaml(std::string stryamlname,std::vector<int64_t> & allow_remapping_by_area_matrix
              )
{

}

void init()
{
    const float score_threshold = 0.35;
    const float circle_nms_dist_threshold =0.5;
  //    static_cast<float>(this->declare_parameter<double>("circle_nms_dist_threshold"));
    std::vector<double> yaw_norm_thresholds ;
    yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.0);
    const std::string densification_world_frame_id = "map";
    const int densification_num_past_frames = 1;
    const std::string trt_precision = "fp16";
    const std::string encoder_onnx_path = "/home/nvidia/models/pts_voxel_encoder_centerpoint.onnx";//this->declare_parameter<std::string>("encoder_onnx_path");
    const std::string encoder_engine_path ="/home/nvidia/models/pts_voxel_encoder_centerpoint.eng";//this->declare_parameter<std::string>("encoder_engine_path");
    const std::string head_onnx_path = "/home/nvidia/models/pts_backbone_neck_head_centerpoint.onnx";//this->declare_parameter<std::string>("head_onnx_path");
    const std::string head_engine_path ="/home/nvidia/models/pts_backbone_neck_head_centerpoint.eng" ;//this->declare_parameter<std::string>("head_engine_path");
    const std::size_t point_feature_size =4;
    const std::size_t max_voxel_size =40000;
    std::vector<double> point_cloud_range ;
    point_cloud_range.push_back(-76.8);point_cloud_range.push_back(-76.8);
    point_cloud_range.push_back(-4.0);point_cloud_range.push_back(76.8);
    point_cloud_range.push_back(76.8);point_cloud_range.push_back(6.0);
    std::vector<double>voxel_size ;
    voxel_size.push_back(0.32);voxel_size.push_back(0.32);
    voxel_size.push_back(10.0);
    const std::size_t downsample_factor =1;
    const std::size_t encoder_in_feature_size = 9;
    std::vector<int64_t> allow_remapping_by_area_matrix;
    std::vector<double> min_area_matrix ;
    std::vector<double> max_area_matrix ;

    class_names_.push_back("CAR");
    class_names_.push_back("TRUCK");
    class_names_.push_back("BUS");
    class_names_.push_back("BICYCLE");
    class_names_.push_back("PEDESTRIAN");





    NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision);
    NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision);
    DensificationParam densification_param(
      densification_world_frame_id, densification_num_past_frames);

    CenterPointConfig config(
      3, point_feature_size, max_voxel_size, point_cloud_range, voxel_size,
      downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold,
      yaw_norm_thresholds);
    detector_ptr_ =
      std::make_unique<CenterPointTRT>(encoder_param, head_param, densification_param, config);

    std::cout<<"init  complete."<<std::endl;

}


void box3DToDetectedObject(
  const Box3D & box3d, const std::vector<std::string> & class_names, const bool has_twist,
  iv::lidar::lidarobject & obj)
{
  // TODO(yukke42): the value of classification confidence of DNN, not probability.
//  obj.existence_probability = box3d.score;
  obj.set_score(box3d.score);

  // classification
  if (box3d.label >= 0 && static_cast<size_t>(box3d.label) < class_names.size()) {
    obj.set_type_name(class_names_[box3d.label]);
  } else {
    obj.set_type_name("UNKNOWN");
  }


  obj.set_mntype(box3d.label);
  obj.set_tyaw(box3d.yaw);
  iv::lidar::PointXYZ pos;
  pos.set_x(box3d.x);pos.set_y(box3d.y);pos.set_z(box3d.z);
  iv::lidar::Dimension dim;
  dim.set_x(box3d.length);dim.set_y(box3d.width);dim.set_z(box3d.height);

  iv::lidar::PointXYZ * ppos = obj.mutable_position();
  ppos->CopyFrom(pos);
  iv::lidar::PointXYZ * pcen = obj.mutable_centroid();
  pcen->CopyFrom(pos);
  iv::lidar::Dimension * pdim = obj.mutable_dimensions();
  pdim->CopyFrom(dim);


  // twist
  if (has_twist) {
//    float vel_x = box3d.vel_x;
//    float vel_y = box3d.vel_y;
//    geometry_msgs::msg::Twist twist;
//    twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2));
//    twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw);
//    obj.kinematics.twist_with_covariance.twist = twist;
//    obj.kinematics.has_twist = has_twist;
  }
}

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


    std::vector<Box3D> det_boxes3d;
    detector_ptr_ ->detect(point_cloud,det_boxes3d);
    std::cout<<" box size: "<<det_boxes3d.size()<<std::endl;

    iv::lidar::objectarray  lidarobjvec;

    int nsize = static_cast<int>(det_boxes3d.size());
    for(i=0;i<nsize;i++)
    {
        iv::lidar::lidarobject obj;
        box3DToDetectedObject(det_boxes3d[i],class_names_,false,obj);
        iv::lidar::lidarobject * pobj = lidarobjvec.add_obj();
        pobj->CopyFrom(obj);
    }

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


}


#include <pcl/io/pcd_io.h>
void testdet(std::string & path)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());

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

    std::vector<Box3D> det_boxes3d;
    int i;
    for(i=0;i<10;i++)
    {
    detector_ptr_ ->detect(point_cloud,det_boxes3d);
    std::cout<<" box size: "<<det_boxes3d.size()<<std::endl;
    }
}


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

    std::string path = "/home/nvidia/1.pcd";
//    testdet(path);

    gpa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);
    gpdetect = iv::modulecomm::RegisterSend("lidar_track",10000000,1);

    return a.exec();
}