/*
 * Copyright 2018-2019 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include "imm_ukf_pda.h"
#include "modulecomm.h"
#include "ivlog.h"
extern iv::Ivlog *givlog;

extern void * gpatrack;
ImmUkfPda::ImmUkfPda()
  : target_id_(0)
  ,  // assign unique ukf_id_ to each tracking targets
  init_(false),
  frame_count_(0),
  has_subscribed_vectormap_(false)
{
    tracking_frame_ = "world";
    life_time_thres_ = 8;
    gating_thres_ = 9.22;
    gate_probability_ = 0.99;
    detection_probability_ = 0.9;
    static_velocity_thres_ = 0.5;
    static_num_history_thres_ = 3;
    prevent_explosion_thres_ = 1000;
    merge_distance_threshold_ = 0.5;
    use_sukf_ = false;
    use_vectormap_ = false;
    is_benchmark_ = false;


//  private_nh_.param<std::string>("tracking_frame", tracking_frame_, "world");
//  private_nh_.param<int>("life_time_thres", life_time_thres_, 8);
//  private_nh_.param<double>("gating_thres", gating_thres_, 9.22);
//  private_nh_.param<double>("gate_probability", gate_probability_, 0.99);
//  private_nh_.param<double>("detection_probability", detection_probability_, 0.9);
//  private_nh_.param<double>("static_velocity_thres", static_velocity_thres_, 0.5);
//  private_nh_.param<int>("static_velocity_history_thres", static_num_history_thres_, 3);
//  private_nh_.param<double>("prevent_explosion_thres", prevent_explosion_thres_, 1000);
//  private_nh_.param<double>("merge_distance_threshold", merge_distance_threshold_, 0.5);
//  private_nh_.param<bool>("use_sukf", use_sukf_, false);

//  // for vectormap assisted tracking
//  private_nh_.param<bool>("use_vectormap", use_vectormap_, false);
//  private_nh_.param<double>("lane_direction_chi_thres", lane_direction_chi_thres_, 2.71);
//  private_nh_.param<double>("nearest_lane_distance_thres", nearest_lane_distance_thres_, 1.0);
//  private_nh_.param<std::string>("vectormap_frame", vectormap_frame_, "map");

//  // rosparam for benchmark
//  private_nh_.param<bool>("is_benchmark", is_benchmark_, false);
//  private_nh_.param<std::string>("kitti_data_dir", kitti_data_dir_, "");
//  if (is_benchmark_)
//  {
//    result_file_path_ = kitti_data_dir_ + "benchmark_results.txt";
//    std::remove(result_file_path_.c_str());
//  }
}

void ImmUkfPda::run()
{
//  pub_object_array_ = node_handle_.advertise<iv::lidar::objectarray >("/detection/objects", 1);
//  sub_detected_array_ = node_handle_.subscribe("/detection/fusion_tools/objects", 1, &ImmUkfPda::callback, this);

//  if (use_vectormap_)
//  {
//    vmap_.subscribe(private_nh_, vector_map::Category::POINT |
//                                 vector_map::Category::NODE  |
//                                 vector_map::Category::LANE, 1);
//  }
}


void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
{
    for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
    {
//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
//        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;

        if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
        {
            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
            if(xp == 0)xp=1;
            int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().y()/0.2)/2.0);
            if(yp == 0)yp=1;
            int ix,iy;
            for(ix = 0; ix<(xp*2); ix++)
            {
                for(iy = 0; iy<(yp*2); iy++)
                {
                    iv::fusion::NomalXYZ nomal_centroid;
                    iv::fusion::NomalXYZ *nomal_centroid_;
                    float nomal_x = ix*0.2 - xp*0.2;
                    float nomal_y = iy*0.2 - yp*0.2;
                    float nomal_z = 1.0;
                    float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
                            lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue;
                    else{
                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
                        nomal_centroid_ = fusion_obj.add_nomal_centroid();
                        nomal_centroid_->CopyFrom(nomal_centroid);
                    }
                }
            }

    }
}}


void ImmUkfPda::callback(const iv::fusion::fusionobjectarray & input)
{
//  input_header_ = input.header;

//  if(use_vectormap_)
//  {
//    checkVectormapSubscription();
//  }

//  bool success = updateNecessaryTransform();
//  if (!success)
//  {
//    ROS_INFO("Could not find coordiante transformation");
//    return;
//  }

//  iv::lidar::objectarray  transformed_input;
   iv::fusion::fusionobjectarray  fused_objects_output;
//  transformPoseToGlobal(input, transformed_input);
    tracker(input, fused_objects_output);
//  transformPoseToLocal(detected_objects_output);

//  pub_object_array_.publish(detected_objects_output);

//  if (is_benchmark_)
//  {
//    dumpResultText(detected_objects_output);
//  }
}

void ImmUkfPda::checkVectormapSubscription()
{
//  if (use_vectormap_ && !has_subscribed_vectormap_)
//  {
//    lanes_ = vmap_.findByFilter([](const vector_map_msgs::Lane& lane) { return true; });
//    if (lanes_.empty())
//    {
//      ROS_INFO("Has not subscribed vectormap");
//    }
//    else
//    {
//      has_subscribed_vectormap_ = true;
//    }
//  }
}

//bool ImmUkfPda::updateNecessaryTransform()
//{
//  bool success = true;
//  try
//  {
//    tf_listener_.waitForTransform(input_header_.frame_id, tracking_frame_, ros::Time(0), ros::Duration(1.0));
//    tf_listener_.lookupTransform(tracking_frame_, input_header_.frame_id, ros::Time(0), local2global_);
//  }
//  catch (tf::TransformException ex)
//  {
//    ROS_ERROR("%s", ex.what());
//    success = false;
//  }
//  if (use_vectormap_ && has_subscribed_vectormap_)
//  {
//    try
//    {
//      tf_listener_.waitForTransform(vectormap_frame_, tracking_frame_, ros::Time(0), ros::Duration(1.0));
//      tf_listener_.lookupTransform(vectormap_frame_, tracking_frame_, ros::Time(0), tracking_frame2lane_frame_);
//      tf_listener_.lookupTransform(tracking_frame_, vectormap_frame_, ros::Time(0), lane_frame2tracking_frame_);
//    }
//    catch (tf::TransformException ex)
//    {
//      ROS_ERROR("%s", ex.what());
//    }
//  }
//  return success;
//}

//void ImmUkfPda::transformPoseToGlobal(const iv::lidar::objectarray & input,
//                                      iv::lidar::objectarray & transformed_input)
//{
//  transformed_input.header = input_header_;
//  for (auto const &object: input.objects)
//  {
//    geometry_msgs::Pose out_pose = getTransformedPose(object.pose, local2global_);

//    iv::lidar::object   dd;
//    dd.header = input.header;
//    dd = object;
//    dd.pose = out_pose;

//    transformed_input.objects.push_back(dd);
//  }
//}

//void ImmUkfPda::transformPoseToLocal(iv::lidar::objectarray & detected_objects_output)
//{
//  detected_objects_output.header = input_header_;

//  tf::Transform inv_local2global = local2global_.inverse();
//  tf::StampedTransform global2local;
//  global2local.setData(inv_local2global);
//  for (auto& object : detected_objects_output.objects)
//  {
//    geometry_msgs::Pose out_pose = getTransformedPose(object.pose, global2local);
//    object.header = input_header_;
//    object.pose = out_pose;
//  }
//}

//geometry_msgs::Pose ImmUkfPda::getTransformedPose(const geometry_msgs::Pose& in_pose,
//                                                  const tf::StampedTransform& tf_stamp)
//{
//  tf::Transform transform;
//  geometry_msgs::PoseStamped out_pose;
//  transform.setOrigin(tf::Vector3(in_pose.position.x, in_pose.position().y(), in_pose.position.z));
//  transform.setRotation(
//      tf::Quaternion(in_pose.orientation.x, in_pose.orientation.y, in_pose.orientation.z, in_pose.orientation.w));
//  geometry_msgs::PoseStamped pose_out;
//  tf::poseTFToMsg(tf_stamp * transform, out_pose.pose);
//  return out_pose.pose;
//}

void ImmUkfPda::measurementValidation(const iv::fusion::fusionobjectarray & input, UKF& target,
                                      const bool second_init, const Eigen::VectorXd& max_det_z,
                                      const Eigen::MatrixXd& max_det_s,
                                      iv::fusion::fusionobjectarray & object_vec,
                                      std::vector<bool>& matching_vec)
{
  // alert: different from original imm-pda filter, here picking up most likely measurement
  // if making it allows to have more than one measurement, you will see non semipositive definite covariance
  bool exists_smallest_nis_object = false;
  double smallest_nis = std::numeric_limits<double>::max();
  int smallest_nis_ind = 0;
  for (size_t i = 0; i < input.obj_size(); i++)
  {
    double x = input.obj(i).centroid().x();
    double y = input.obj(i).centroid().y();

    Eigen::VectorXd meas = Eigen::VectorXd(2);
    meas << x, y;

    Eigen::VectorXd diff = meas - max_det_z;
    double nis = diff.transpose() * max_det_s.inverse() * diff;

    if (nis < gating_thres_)
    {
      if (nis < smallest_nis)
      {
        smallest_nis = nis;
        target.object_ = input.obj(i);
        smallest_nis_ind = i;
        exists_smallest_nis_object = true;
      }
    }
  }
  if (exists_smallest_nis_object)
  {
    matching_vec[smallest_nis_ind] = true;
    if (use_vectormap_ && has_subscribed_vectormap_)
    {
//      iv::lidar::object   direction_updated_object;
//      bool use_direction_meas =
//          updateDirection(smallest_nis, target.object_, direction_updated_object, target);
//      if (use_direction_meas)
//      {
//        object_vec.push_back(direction_updated_object);
//      }
//      else
//      {
//        object_vec.push_back(target.object_);
//      }
    }
    else
    {
        iv::fusion::fusionobject * px = object_vec.add_obj();
        px->CopyFrom(target.object_);
 //     object_vec.push_back(target.object_);
    }
  }
}

//bool ImmUkfPda::updateDirection(const double smallest_nis, const iv::lidar::object  & in_object,
//                                    iv::lidar::object  & out_object, UKF& target)
//{
//  bool use_lane_direction = false;
//  target.is_direction_cv_available_ = false;
//  target.is_direction_ctrv_available_ = false;
//  bool get_lane_success = storeObjectWithNearestLaneDirection(in_object, out_object);
//  if (!get_lane_success)
//  {
//    return use_lane_direction;
//  }
//  target.checkLaneDirectionAvailability(out_object, lane_direction_chi_thres_, use_sukf_);
//  if (target.is_direction_cv_available_ || target.is_direction_ctrv_available_)
//  {
//    use_lane_direction = true;
//  }
//  return use_lane_direction;
//}

//bool ImmUkfPda::storeObjectWithNearestLaneDirection(const iv::lidar::object  & in_object,
//                                                 iv::lidar::object  & out_object)
//{
//  geometry_msgs::Pose lane_frame_pose = getTransformedPose(in_object.pose, tracking_frame2lane_frame_);
//  double min_dist = std::numeric_limits<double>::max();

//  double min_yaw = 0;
//  for (auto const& lane : lanes_)
//  {
//    vector_map_msgs::Node node = vmap_.findByKey(vector_map::Key<vector_map_msgs::Node>(lane.bnid));
//    vector_map_msgs::Point point = vmap_.findByKey(vector_map::Key<vector_map_msgs::Point>(node.pid));
//    double distance = std::sqrt(std::pow(point.bx - lane_frame_pose.position().y(), 2) +
//                                std::pow(point.ly - lane_frame_pose.position.x, 2));
//    if (distance < min_dist)
//    {
//      min_dist = distance;
//      vector_map_msgs::Node front_node = vmap_.findByKey(vector_map::Key<vector_map_msgs::Node>(lane.fnid));
//      vector_map_msgs::Point front_point = vmap_.findByKey(vector_map::Key<vector_map_msgs::Point>(front_node.pid));
//      min_yaw = std::atan2((front_point.bx - point.bx), (front_point.ly - point.ly));
//    }
//  }

//  bool success = false;
//  if (min_dist < nearest_lane_distance_thres_)
//  {
//    success = true;
//  }
//  else
//  {
//    return success;
//  }

//  // map yaw in rotation matrix representation
//  tf::Quaternion map_quat = tf::createQuaternionFromYaw(min_yaw);
//  tf::Matrix3x3 map_matrix(map_quat);

//  // vectormap_frame to tracking_frame rotation matrix
//  tf::Quaternion rotation_quat = lane_frame2tracking_frame_.getRotation();
//  tf::Matrix3x3 rotation_matrix(rotation_quat);

//  // rotated yaw in matrix representation
//  tf::Matrix3x3 rotated_matrix = rotation_matrix * map_matrix;
//  double roll, pitch, yaw;
//  rotated_matrix.getRPY(roll, pitch, yaw);

//  out_object = in_object;
//  out_object.angle = yaw;
//  return success;
//}

void ImmUkfPda::updateTargetWithAssociatedObject(const iv::fusion::fusionobjectarray & object_vec,
                                                 UKF& target)
{
  target.lifetime_++;
//  if (!target.object_.label.empty() && target.object_.label !="unknown")
//  {
//    target.label_ = target.object_.label;
//  }
  updateTrackingNum(object_vec, target);
  if (target.tracking_num_ == TrackingState::Stable || target.tracking_num_ == TrackingState::Occlusion)
  {
    target.is_stable_ = true;
  }
}

void ImmUkfPda::updateBehaviorState(const UKF& target, iv::fusion::fusionobject  & object)
{
  if (target.mode_prob_cv_ > target.mode_prob_ctrv_ && target.mode_prob_cv_ > target.mode_prob_rm_)
  {
      object.set_behavior_state(MotionModel::CV);
//    object.behavior_state = MotionModel::CV;
  }
  else if (target.mode_prob_ctrv_ > target.mode_prob_cv_ && target.mode_prob_ctrv_ > target.mode_prob_rm_)
  {
      object.set_behavior_state(MotionModel::CTRV);
//    object.behavior_state = MotionModel::CTRV;
  }
  else
  {
      object.set_behavior_state(MotionModel::RM);
 //   object.behavior_state = MotionModel::RM;
  }
}

void ImmUkfPda::initTracker(const iv::fusion::fusionobjectarray & input, double timestamp)
{
  for (size_t i = 0; i < input.obj_size(); i++)
  {
    double px = input.obj(i).centroid().x();
    double py = input.obj(i).centroid().y();
    Eigen::VectorXd init_meas = Eigen::VectorXd(2);
    init_meas << px, py;

    UKF ukf;
    ukf.initialize(init_meas, timestamp, target_id_);
    targets_.push_back(ukf);
    target_id_++;
  }
  timestamp_ = timestamp;
  init_ = true;
}

void ImmUkfPda::secondInit(UKF& target, const iv::fusion::fusionobjectarray & object_vec, double dt)
{
  if (object_vec.obj_size() == 0)
  {
    target.tracking_num_ = TrackingState::Die;
    return;
  }
  // record init measurement for env classification
  target.init_meas_ << target.x_merge_(0), target.x_merge_(1);

  // state update
  double target_x = object_vec.obj(0).centroid().x();
  double target_y = object_vec.obj(0).centroid().y();
  double target_diff_x = target_x - target.x_merge_(0);
  double target_diff_y = target_y - target.x_merge_(1);
  double target_yaw = atan2(target_diff_y, target_diff_x);
  double dist = sqrt(target_diff_x * target_diff_x + target_diff_y * target_diff_y);
  double target_v = dist / dt;

  while (target_yaw > M_PI)
    target_yaw -= 2. * M_PI;
  while (target_yaw < -M_PI)
    target_yaw += 2. * M_PI;

  target.x_merge_(0) = target.x_cv_(0) = target.x_ctrv_(0) = target.x_rm_(0) = target_x;
  target.x_merge_(1) = target.x_cv_(1) = target.x_ctrv_(1) = target.x_rm_(1) = target_y;
  target.x_merge_(2) = target.x_cv_(2) = target.x_ctrv_(2) = target.x_rm_(2) = target_v;
  target.x_merge_(3) = target.x_cv_(3) = target.x_ctrv_(3) = target.x_rm_(3) = target_yaw;

  target.tracking_num_++;
  return;
}

void ImmUkfPda::updateTrackingNum(const iv::fusion::fusionobjectarray & object_vec, UKF& target)
{
  if (object_vec.obj_size() > 0)
  {
    if (target.tracking_num_ < TrackingState::Stable)
    {
      target.tracking_num_++;
    }
    else if (target.tracking_num_ == TrackingState::Stable)
    {
      target.tracking_num_ = TrackingState::Stable;
    }
    else if (target.tracking_num_ >= TrackingState::Stable && target.tracking_num_ < TrackingState::Lost)
    {
      target.tracking_num_ = TrackingState::Stable;
    }
    else if (target.tracking_num_ == TrackingState::Lost)
    {
      target.tracking_num_ = TrackingState::Die;
    }
  }
  else
  {
    if (target.tracking_num_ < TrackingState::Stable)
    {
      target.tracking_num_ = TrackingState::Die;
    }
    else if (target.tracking_num_ >= TrackingState::Stable && target.tracking_num_ < TrackingState::Lost)
    {
      target.tracking_num_++;
    }
    else if (target.tracking_num_ == TrackingState::Lost)
    {
      target.tracking_num_ = TrackingState::Die;
    }
  }

  return;
}

bool ImmUkfPda::probabilisticDataAssociation(const iv::fusion::fusionobjectarray & input, const double dt,
                                             std::vector<bool>& matching_vec,
                                             iv::fusion::fusionobjectarray & object_vec, UKF& target)
{
  double det_s = 0;
  Eigen::VectorXd max_det_z;
  Eigen::MatrixXd max_det_s;
  bool success = true;

  if (use_sukf_)
  {
    max_det_z = target.z_pred_ctrv_;
    max_det_s = target.s_ctrv_;
    det_s = max_det_s.determinant();
  }
  else
  {
    // find maxDetS associated with predZ
    target.findMaxZandS(max_det_z, max_det_s);
    det_s = max_det_s.determinant();
  }

  // prevent ukf not to explode
  if (std::isnan(det_s) || det_s > prevent_explosion_thres_)
  {
    target.tracking_num_ = TrackingState::Die;
    success = false;
    return success;
  }

  bool is_second_init;
  if (target.tracking_num_ == TrackingState::Init)
  {
    is_second_init = true;
  }
  else
  {
    is_second_init = false;
  }

  // measurement gating
  measurementValidation(input, target, is_second_init, max_det_z, max_det_s, object_vec, matching_vec);

  // second detection for a target: update v and yaw
  if (is_second_init)
  {
    secondInit(target, object_vec, dt);
    success = false;
    return success;
  }

  updateTargetWithAssociatedObject(object_vec, target);

  if (target.tracking_num_ == TrackingState::Die)
  {
    success = false;
    return success;
  }
  return success;
}

void ImmUkfPda::makeNewTargets(const double timestamp, const iv::fusion::fusionobjectarray & input,
                               const std::vector<bool>& matching_vec)
{
  for (size_t i = 0; i < input.obj_size(); i++)
  {
    if (matching_vec[i] == false)
    {
      double px = input.obj(i).centroid().x();
      double py = input.obj(i).centroid().y();
      Eigen::VectorXd init_meas = Eigen::VectorXd(2);
      init_meas << px, py;

      UKF ukf;
      ukf.initialize(init_meas, timestamp, target_id_);
      ukf.object_ = input.obj(i);
      targets_.push_back(ukf);
      target_id_++;
    }
  }
}

void ImmUkfPda::staticClassification()
{
  for (size_t i = 0; i < targets_.size(); i++)
  {
    // targets_[i].x_merge_(2) is referred for estimated velocity
    double current_velocity = std::abs(targets_[i].x_merge_(2));
    targets_[i].vel_history_.push_back(current_velocity);
    if (targets_[i].tracking_num_ == TrackingState::Stable && targets_[i].lifetime_ > life_time_thres_)
    {
      int index = 0;
      double sum_vel = 0;
      double avg_vel = 0;
      for (auto rit = targets_[i].vel_history_.rbegin(); index < static_num_history_thres_; ++rit)
      {
        index++;
        sum_vel += *rit;
      }
      avg_vel = double(sum_vel / static_num_history_thres_);

      if(avg_vel < static_velocity_thres_ && current_velocity < static_velocity_thres_)
      {
        targets_[i].is_static_ = true;
      }
    }
  }
}

bool
ImmUkfPda::arePointsClose(const iv::fusion::PointXYZ & in_point_a,
                                const iv::fusion::PointXYZ & in_point_b,
                                float in_radius)
{
  return (fabs(in_point_a.x() - in_point_b.x()) <= in_radius) && (fabs(in_point_a.y() - in_point_b.y()) <= in_radius);
}

bool
ImmUkfPda::arePointsEqual(const iv::fusion::PointXYZ & in_point_a,
                               const iv::fusion::PointXYZ & in_point_b)
{
  return arePointsClose(in_point_a, in_point_b, CENTROID_DISTANCE);
}

bool
ImmUkfPda::isPointInPool(const std::vector<iv::fusion::PointXYZ>& in_pool,
                          const iv::fusion::PointXYZ& in_point)
{
  for(size_t j=0; j<in_pool.size(); j++)
  {
    if (arePointsEqual(in_pool[j], in_point))
    {
      return true;
    }
  }
  return false;
}

iv::fusion::fusionobjectarray
ImmUkfPda::removeRedundantObjects(const iv::fusion::fusionobjectarray & in_detected_objects,
                            const std::vector<size_t> in_tracker_indices)
{
  if (in_detected_objects.obj_size() != in_tracker_indices.size())
    return in_detected_objects;

  iv::fusion::fusionobjectarray  resulting_objects;
//  resulting_objects.header = in_detected_objects.header;

  std::vector<iv::fusion::PointXYZ> centroids;
  //create unique points
  for(size_t i=0; i<in_detected_objects.obj_size(); i++)
  {
    if(!isPointInPool(centroids, in_detected_objects.obj(i).centroid()))
    {
      centroids.push_back(in_detected_objects.obj(i).centroid());
    }
  }
  //assign objects to the points
  std::vector<std::vector<size_t>> matching_objects(centroids.size());
  for(size_t k=0; k<in_detected_objects.obj_size(); k++)
  {
    const auto& object=in_detected_objects.obj(k);
    for(size_t i=0; i< centroids.size(); i++)
    {
      if (arePointsClose(object.centroid(), centroids[i], merge_distance_threshold_))
      {
        matching_objects[i].push_back(k);//store index of matched object to this point
      }
    }
  }
  //get oldest object on each point
  for(size_t i=0; i< matching_objects.size(); i++)
  {
    size_t oldest_object_index = 0;
    int oldest_lifespan = -1;
    std::string best_label;
    for(size_t j=0; j<matching_objects[i].size(); j++)
    {
      size_t current_index = matching_objects[i][j];
      int current_lifespan = targets_[in_tracker_indices[current_index]].lifetime_;
      if (current_lifespan > oldest_lifespan)
      {
        oldest_lifespan = current_lifespan;
        oldest_object_index = current_index;
      }
      if (!targets_[in_tracker_indices[current_index]].label_.empty() &&
        targets_[in_tracker_indices[current_index]].label_ != "unknown")
      {
        best_label = targets_[in_tracker_indices[current_index]].label_;
      }
    }
    // delete nearby targets except for the oldest target
    for(size_t j=0; j<matching_objects[i].size(); j++)
    {
      size_t current_index = matching_objects[i][j];
      if(current_index != oldest_object_index)
      {
        targets_[in_tracker_indices[current_index]].tracking_num_= TrackingState::Die;
      }
    }
    iv::fusion::fusionobject   best_object;
    best_object = in_detected_objects.obj(oldest_object_index);
    if (best_label != "unknown"
        && !best_label.empty())
    {
//      best_object.label = best_label;
    }

    iv::fusion::fusionobject * px = resulting_objects.add_obj();
    px->CopyFrom(best_object);
 //   resulting_objects.push_back(best_object);
  }

  return resulting_objects;

}

void ImmUkfPda::makeOutput(const iv::fusion::fusionobjectarray & input,
                           const std::vector<bool> &matching_vec,
                           iv::fusion::fusionobjectarray & fused_objects_output)
{
  iv::fusion::fusionobjectarray  tmp_objects;
//  tmp_objects.header = input.header;
  std::vector<size_t> used_targets_indices;
  for (size_t i = 0; i < targets_.size(); i++)
  {

    double tx = targets_[i].x_merge_(0);
    double ty = targets_[i].x_merge_(1);

    double tv = targets_[i].x_merge_(2);
    double tyaw = targets_[i].x_merge_(3);
    double tyaw_rate = targets_[i].x_merge_(4);
    if(tyaw_rate > 0.2)
    {
        tyaw_rate = 0.2;
    }

    while (tyaw > M_PI)
      tyaw -= 2. * M_PI;
    while (tyaw < -M_PI)
      tyaw += 2. * M_PI;

 //   tf::Quaternion q = tf::createQuaternionFromYaw(tyaw);

    iv::fusion::fusionobject   dd;
    dd = targets_[i].object_;
    dd.set_id(targets_[i].ukf_id_);
//    dd.id = targets_[i].ukf_id_;
    dd.set_velocity_linear_x(tv);
   // dd.velocity_linear_x = tv;
    dd.set_acceleration_linear_y(tyaw_rate);
   // dd.acceleration_linear_y = tyaw_rate;
    dd.set_velocity_reliable(targets_[i].is_stable_);
 //   dd.velocity_reliable = targets_[i].is_stable_;
    dd.set_pose_reliable(targets_[i].is_stable_);
 //   dd.pose_reliable = targets_[i].is_stable_;


    if (!targets_[i].is_static_ && targets_[i].is_stable_)
    {
      // Aligh the longest side of dimentions with the estimated orientation
      if(targets_[i].object_.dimensions().x() < targets_[i].object_.dimensions().y())
      {
          iv::fusion::Dimension * pd = dd.mutable_dimensions();
          pd->set_x(targets_[i].object_.dimensions().x());
          pd->set_y(targets_[i].object_.dimensions().y());
//        dd.dimensions.x = targets_[i].object_.dimensions.y;
//        dd.dimensions.y = targets_[i].object_.dimensions.x;
      } else {
          iv::fusion::Dimension * pd = dd.mutable_dimensions();
          pd->set_x(targets_[i].object_.dimensions().y());
          pd->set_y(targets_[i].object_.dimensions().x());
      }

      iv::fusion::PointXYZ * pp = dd.mutable_centroid();
      pp->set_x(tx);
      pp->set_y((ty));
//      dd.position.x = tx;
//      dd.position().y() = ty;

//      if (!std::isnan(q[0]))
//        dd.pose.orientation.x = q[0];
//      if (!std::isnan(q[1]))
//        dd.pose.orientation.y = q[1];
//      if (!std::isnan(q[2]))
//        dd.pose.orientation.z = q[2];
//      if (!std::isnan(q[3]))
//        dd.pose.orientation.w = q[3];
      dd.set_yaw(tyaw);
  //    dd.tyaw = tyaw;
    }


    updateBehaviorState(targets_[i], dd);
    int xp ,yp;
    if(targets_[i].object_.dimensions().x() < targets_[i].object_.dimensions().y())
    {
        xp = (int)((targets_[i].object_.dimensions().x()/0.2)/2.0);
        if(xp == 0)xp=1;
        yp = (int)((targets_[i].object_.dimensions().y()/0.2)/2.0);
        if(yp == 0)yp=1;
    } else {
        xp = (int)((targets_[i].object_.dimensions().y()/0.2)/2.0);
        if(xp == 0)xp=1;
        yp = (int)((targets_[i].object_.dimensions().x()/0.2)/2.0);
        if(yp == 0)yp=1;
    }


    int ix,iy;
    for(ix = 0; ix<(xp*2); ix++)
    {
//        std::cout<<"   add  normal  --------------------------  "<<std::endl;
        for(iy = 0; iy<(yp*2); iy++)
        {
            iv::fusion::NomalXYZ nomal_centroid;
            iv::fusion::NomalXYZ *nomal_centroid_;
            float nomal_x = ix*0.2 - xp*0.2;
            float nomal_y = iy*0.2 - yp*0.2;
            float nomal_z = 0.5;
                float s = nomal_x*cos(0) -
                        nomal_y*sin(0);
                float t = nomal_x*sin(0) +
                        nomal_y*cos(0);
            nomal_centroid.set_nomal_x(tx + s);
            nomal_centroid.set_nomal_y(ty + t);
            nomal_centroid_ = dd.add_nomal_centroid();
            nomal_centroid_->CopyFrom(nomal_centroid);
        }
    }

    if (targets_[i].is_stable_ || (targets_[i].tracking_num_ >= TrackingState::Init &&
                                   targets_[i].tracking_num_ < TrackingState::Stable))
    {
        iv::fusion::fusionobject * px = tmp_objects.add_obj();
        px->CopyFrom(dd);
//      tmp_objects.push_back(dd);
      used_targets_indices.push_back(i);
    }
  }
 // std::cout<<"run hear."<<std::endl;

  fused_objects_output = removeRedundantObjects(tmp_objects, used_targets_indices);


}

void ImmUkfPda::removeUnnecessaryTarget()
{
  std::vector<UKF> temp_targets;
  for (size_t i = 0; i < targets_.size(); i++)
  {
    if (targets_[i].tracking_num_ != TrackingState::Die)
    {
      temp_targets.push_back(targets_[i]);
    }
  }
  std::vector<UKF>().swap(targets_);
  targets_ = temp_targets;
}

void ImmUkfPda::dumpResultText(iv::fusion::fusionobjectarray & fused_objects)
{
  std::ofstream outputfile(result_file_path_, std::ofstream::out | std::ofstream::app);
  for (size_t i = 0; i < fused_objects.obj_size(); i++)
  {
 //   double yaw = tf::getYaw(detected_objects.objects[i].pose.orientation);
    double yaw = fused_objects.obj(i).yaw();
    // KITTI tracking benchmark data format:
    // (frame_number,tracked_id, object type, truncation, occlusion, observation angle, x1,y1,x2,y2, h, w, l, cx, cy,
    // cz, yaw)
    // x1, y1, x2, y2 are for 2D bounding box.
    // h, w, l, are for height, width, length respectively
    // cx, cy, cz are for object centroid

    // Tracking benchmark is based on frame_number, tracked_id,
    // bounding box dimentions and object pose(centroid and orientation) from bird-eye view
    outputfile << std::to_string(frame_count_) << " " << std::to_string(fused_objects.obj(i).id()) << " "
               << "Unknown"
               << " "
               << "-1"
               << " "
               << "-1"
               << " "
               << "-1"
               << " "
               << "-1 -1 -1 -1"
               << " " << std::to_string(fused_objects.obj(i).dimensions().x()) << " "
               << std::to_string(fused_objects.obj(i).dimensions().y()) << " "
               << "-1"
               << " " << std::to_string(fused_objects.obj(i).centroid().x()) << " "
               << std::to_string(fused_objects.obj(i).centroid().y()) << " "
               << "-1"
               << " " << std::to_string(yaw) << "\n";
  }
  frame_count_++;
}

void ImmUkfPda::tracker(const iv::fusion::fusionobjectarray & input,
                        iv::fusion::fusionobjectarray & fused_objects_output)
{
//  double timestamp = input.header.stamp.toSec();

  if(input.obj_size() < 1)
  {
      fused_objects_output.set_timestamp(input.timestamp());
      std::string out = fused_objects_output.SerializeAsString();
    //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
      iv::modulecomm::ModuleSendMsg(gpatrack,out.data(),out.length());
              return;
  }
  double timestamp =  input.timestamp();
//  qDebug("time stamp is %f",timestamp);
  timestamp = timestamp/1000.0;
  std::vector<bool> matching_vec(input.obj_size(), false);


  if (!init_)
  {
    initTracker(input, timestamp);

    makeOutput(input, matching_vec, fused_objects_output);
    return;
  }


  double dt = (timestamp - timestamp_);
  timestamp_ = timestamp;


  // start UKF process
  for (size_t i = 0; i < targets_.size(); i++)
  {
    targets_[i].is_stable_ = false;
    targets_[i].is_static_ = false;

    if (targets_[i].tracking_num_ == TrackingState::Die)
    {
      continue;
    }
    // prevent ukf not to explode
    if (targets_[i].p_merge_.determinant() > prevent_explosion_thres_ ||
        targets_[i].p_merge_(4, 4) > prevent_explosion_thres_)
    {
      targets_[i].tracking_num_ = TrackingState::Die;
      continue;
    }

    targets_[i].prediction(use_sukf_, has_subscribed_vectormap_, dt);

    iv::fusion::fusionobjectarray object_vec;
    bool success = probabilisticDataAssociation(input, dt, matching_vec, object_vec, targets_[i]);
    if (!success)
    {
      continue;
    }

    targets_[i].update(use_sukf_, detection_probability_, gate_probability_, gating_thres_, object_vec);
  }
  // end UKF process

  // making new ukf target for no data association objects
  makeNewTargets(timestamp, input, matching_vec);

  // static dynamic classification
  staticClassification();

  // making output for visualization
  makeOutput(input, matching_vec, fused_objects_output);

  fused_objects_output.set_timestamp(input.timestamp());
  if(fused_objects_output.obj_size()>0)
  {
      int j;
      for(j=0;j<fused_objects_output.obj_size();j++)
      {
      iv::fusion::fusionobject obj = fused_objects_output.obj(j);
      std::cout<<"obj "<<j<< " vel is "<<obj.velocity_linear_x()<<" id is "<<obj.id()<<std::endl;

      }
  }
  else
  {
      std::cout<<"no obj "<<std::endl;
  }
//  for(int k =0; k<fused_objects_output.obj_size(); k++ )
//  {
//     std::cout<<"  normal size   "<< fused_objects_output.obj(k).nomal_centroid_size()<<std::endl;
//  }

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

  int i;
  for(i=0;i<fused_objects_output.obj_size();i++)
  {
      iv::fusion::fusionobject * pobj = fused_objects_output.mutable_obj(i);
  }

  // remove unnecessary ukf object
  removeUnnecessaryTarget();
}

void ImmUkfPda::call(const iv::fusion::fusionobjectarray &input)
{
    callback(input);
}