#include "transfusion_call.h"

transfusion_call::transfusion_call()
{

    std::string engine_path ="/home/nvidia/models/transfusion.eng";
    std::string onnx_path = "/home/nvidia/models/transfusion.onnx";
    std::string trt_precision ="fp16" ;

    std::string densification_world_frame_id = "world";
    int densification_num_past_frames = 0;

    std::size_t cloud_capacity = 2000000;
    std::vector<int64_t> voxels_num;
    std::vector<double>  point_cloud_range;
    std::vector<double>  voxel_size;
    std::size_t num_proposals = 500;
    float circle_nms_dist_threshold = 0.5;
    std::vector<double>  yaw_norm_thresholds;
    float score_threshold = 0.2;

    voxels_num.push_back(5000);voxels_num.push_back(30000);voxels_num.push_back(60000);
    point_cloud_range.push_back(-76.8);point_cloud_range.push_back(-76.8);point_cloud_range.push_back(-3.0);
    point_cloud_range.push_back(76.8);point_cloud_range.push_back(76.8);point_cloud_range.push_back(5.0);
    voxel_size.push_back(0.3);voxel_size.push_back(0.3);voxel_size.push_back(8.0);
    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.3);yaw_norm_thresholds.push_back(0.0);

    NetworkParam network_param(onnx_path, engine_path, trt_precision);
    DensificationParam densification_param(
      densification_world_frame_id, densification_num_past_frames);
    TransfusionConfig config(
      cloud_capacity, voxels_num, point_cloud_range, voxel_size, num_proposals,
      circle_nms_dist_threshold, yaw_norm_thresholds, score_threshold);

    detector_ptr_ = std::make_unique<TransfusionTRT>(network_param, densification_param, config);
}

bool transfusion_call::detect(
  const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr,
  std::vector<Box3D> & det_boxes3d)
{
    std::unordered_map<std::string, double> proc_timing;
    return detector_ptr_->detect(pc_ptr,det_boxes3d,proc_timing);
}