#include <mutex>
#include <memory>
#include <iostream>
#include <boost/circular_buffer.hpp>

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/MarkerArray.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <pcl/filters/voxel_grid.h>

#include <pclomp/ndt_omp.h>
#include <pclomp/gicp_omp.h>

#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <visualization_msgs/MarkerArray.h>

#include<pcl/segmentation/sac_segmentation.h>
#include<pcl/search/search.h>
#include<pcl/search/kdtree.h>
#include<pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>

#include <hdl_localization/pose_estimator.hpp>

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>

#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
using namespace gtsam;

#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseActionResult.h>
#include <actionlib_msgs/GoalStatusArray.h>

#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Path.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <tf/transform_datatypes.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

#include <cmath>

namespace hdl_localization {

class HdlLocalizationNodelet : public nodelet::Nodelet {
public:
  using PointT = pcl::PointXYZI;

  HdlLocalizationNodelet():ac_(nh,"move_base", true){

  }
  virtual ~HdlLocalizationNodelet() {
  }

  void onInit() override {

    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    processing_time.resize(16);
    imuAccNoise = private_nh.param<double>("imuAccNoise", 0.01);
    imuGyrNoise = private_nh.param<double>("imuGyrNoise", 0.001);
    odomLinearNoise = private_nh.param<double>("odomLinearNoise", 0.01);
    odomAngularNoise = private_nh.param<double>("odomAngularNoise", 0.01);
    gravity= private_nh.param<double>("gravity", 10.04);
    opt_frames = private_nh.param<int>("opt_frames", 20);//图优化窗口的最大值
    odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "velodyne");
    use_imu = private_nh.param<bool>("use_imu", true);
    invert_imu = private_nh.param<bool>("invert_imu", false);
    key_interval = private_nh.param<float>("key_interval", 0.1);
    imu_sub = mt_nh.subscribe("/imu/data_raw", 256, &HdlLocalizationNodelet::imu_callback, this);
    points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this);
    globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this);
    initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this);
    pose_pub = nh.advertise<nav_msgs::Odometry>("/ndt/global_odom", 20, false);

    z_axis_min_ =  private_nh.param<float>("z_axis_min_", 0.1);
    z_axis_max_ = private_nh.param<float>("z_axis_max_", 0.4);  //直通滤波z轴方向的最大值
    cluster_size_min_ = private_nh.param<int>("cluster_size_min_", 50);
    cluster_size_max_ = private_nh.param<int>("cluster_size_max_", 300);//聚类过程中每一类的最大点云数量
    cylinderSize= private_nh.param<int>("cylinderSize", 5); //RANSAC 拟合圆柱时inliner的数量
    withdetection= private_nh.param<bool>("withdetection", false);//确定是否需要进行树干检测并发送目标
    trunkCentermin=private_nh.param<float>("trunkCentermin", 0.1);
    trunkCentermax=private_nh.param<float>("trunkCentermax",1.1);//拟合的树干的中心点的高度，通过高度限制滤掉地面上的圆柱型土坡
//    pose_array_pub_ = nh.advertise<geometry_msgs::PoseArray>("poses", 100); //发布所有类的位置
    marker_array_pub_ = nh.advertise<visualization_msgs::MarkerArray>("markers", 100);//标记所有类

      getPoseArray(PoseArray);
      piter=PoseArray.begin();

      initialize_params();

  }

private:
  void initialize_params() {

    // intialize scan matching method
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    std::string ndt_neighbor_search_method = private_nh.param<std::string>("ndt_neighbor_search_method", "DIRECT7");

    double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    downsample_filter = voxelgrid;

    pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
    pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>());

    ndt->setTransformationEpsilon(0.01);
    ndt->setResolution(ndt_resolution);
    if(ndt_neighbor_search_method == "DIRECT1") {
      NODELET_INFO("search_method DIRECT1 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
      registration = ndt;
//        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
//      localregistration=ndt;
    } else if(ndt_neighbor_search_method == "DIRECT7") {
      NODELET_INFO("search_method DIRECT7 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
      registration = ndt;
//      localregistration=ndt;

    } else if(ndt_neighbor_search_method == "GICP_OMP"){
      NODELET_INFO("search_method GICP_OMP is selected");
      registration = gicp;
    }
     else {
      if(ndt_neighbor_search_method == "KDTREE") {
        NODELET_INFO("search_method KDTREE is selected");
      } else {
        NODELET_WARN("invalid search method was given");
        NODELET_WARN("default method is selected (KDTREE)");
      }
      ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
      registration = ndt;

    }

    // initialize pose estimator
    if(private_nh.param<bool>("specify_init_pose", true))
    {
      NODELET_INFO("initialize pose estimator with specified parameters!!");
      pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
        ros::Time::now(),
        Eigen::Vector3d(private_nh.param<double>("init_pos_x", 0.0), private_nh.param<double>("init_pos_y", 0.0), private_nh.param<double>("init_pos_z", 0.0)),
        Eigen::Quaterniond(private_nh.param<double>("init_ori_w", 1.0), private_nh.param<double>("init_ori_x", 0.0), private_nh.param<double>("init_ori_y", 0.0), private_nh.param<double>("init_ori_z", 0.0)),
        private_nh.param<double>("cool_time_duration", 0.5)
      ));
    }
//    poseFrom = gtsam::Pose3(gtsam::Rot3::Quaternion(1.0,0.0,0.0,0.0),gtsam::Point3(0.0,0.0,0.0));
      /*****************factor graph********************/
      boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(gravity);
      p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
      p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
      p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
      gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); // assume zero initial bias
      priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
      priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/s
      priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
      correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1); // meter
      noiseModelBetweenBias = (gtsam::Vector(6) << 3.99395e-03, 3.99395e-03, 3.99395e-03, 1.56363e-03, 1.56363e-03, 1.56363e-03).finished();
      imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias);
      key_count=0;
      systemInitialized= false;
/********************cluster******************************/
      regions_[0] = 2; regions_[1] = 3; regions_[2] = 3; regions_[3] = 3; regions_[4] = 3;
      regions_[5] = 3; regions_[6] = 3; regions_[7] = 2; regions_[8] = 3; regions_[9] = 3;
      regions_[10]= 3; regions_[11]= 3; regions_[12]= 3; regions_[13]= 3;
      //参考论文《Online Learning for 3D LiDAR-based Human Detection: Experimental
      //Analysis of Point Cloud Clustering and Classification Methods》
/*******************path planning**movebase********************/
      while(!ac_.waitForServer(ros::Duration(5.0)));  //启动movebase

  }

private:
  /**
   * @brief callback for imu data
   * @param imu_msg
   */
  void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) {
      std::lock_guard<std::mutex> lock(imu_data_mutex);
      imu_data.push_back(imu_msg);
//      ROS_INFO("hahahahahahah");
  }
  /**
   * @brief callback for point cloud data
   * @param points_msg
   */
  void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {

      points_curr_time=points_msg->header.stamp;

      if(firstkey)
      {
          points_pre_time=points_msg->header.stamp;
          firstkey= false;
      }

      auto time_interval= points_curr_time-points_pre_time;

      if(time_interval.toSec()<key_interval) return;// select key frames according to time interval

      std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);

      if(!pose_estimator) {
          NODELET_ERROR("waiting for initial pose input!!");
          return;
      }

      if(!globalmap) {
          NODELET_ERROR("globalmap has not been received!!");
          return;
      }

      const auto& stamp = points_msg->header.stamp;
      pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
      pcl::fromROSMsg(*points_msg, *pcl_cloud);

      if(pcl_cloud->empty()) {
          NODELET_ERROR("cloud is empty!!");
          return;
      }

      // transform pointcloud into odom_child_frame_id
      pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
      if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_listener)) {
          NODELET_ERROR("point cloud cannot be transformed into target frame!!");
          return;
      }

      auto filtered = downsample(cloud);

      auto t1 = ros::WallTime::now();

      if(!systemInitialized){

          resetOptimization();

          newgraph.add(PriorFactor<Pose3>(X(0), prevPose_, priorPoseNoise));
//          std::cout<<"initial value:"<<prevPose_.x()<<" "<<prevPose_.y()<<" "<<prevPose_.z()<<" "<<prevPose_.rotation().roll()<<" "
//                   <<prevPose_.rotation().pitch()<<" "<<prevPose_.rotation().yaw()<<std::endl;

          //initial velocity
          prevVel_ = gtsam::Vector3(0, 0, 0);
          gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
          newgraph.add(priorVel);

          //initial bias
          prevBias_ = gtsam::imuBias::ConstantBias();
          gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
          newgraph.add(priorBias);

          //add values
          initialEstimate.insert(X(0), prevPose_);
          initialEstimate.insert(V(0), prevVel_);
          initialEstimate.insert(B(0), prevBias_);

          optimizer->update(newgraph, initialEstimate);

          newgraph.resize(0);
          initialEstimate.clear();
          imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
          systemInitialized=true;
          key_count=1;
          return;
      }
      else{

          if(key_count==opt_frames)
          {
              gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(X(key_count-1)));
              gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(V(key_count-1)));
              gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(B(key_count-1)));
              // reset graph
              resetOptimization();
              // add pose
              gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
              newgraph.add(priorPose);
              // add velocity
              gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
              newgraph.add(priorVel);
              // add bias
              gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
              newgraph.add(priorBias);
              // add values
              initialEstimate.insert(X(0), prevPose_);
//              Eigen::Vector3d vel(prevVel_(0),prevVel_(1),0);// test in indoor,limit the z velocity to 0
//              initialEstimate.insert(V(0), vel);
              initialEstimate.insert(V(0), prevVel_);

              initialEstimate.insert(B(0), prevBias_);
              // optimize once
              optimizer->update(newgraph, initialEstimate);
              newgraph.resize(0);
              initialEstimate.clear();

              key_count=1;
          }
          gtsam::Pose3 poseFrom = gtsam::Pose3(gtsam::Rot3::Quaternion(pose_estimator->quat().w(),pose_estimator->quat().x(),pose_estimator->quat().y(),pose_estimator->quat().z()),
                                               gtsam::Point3(pose_estimator->pos()(0),pose_estimator->pos()(1),pose_estimator->pos()(2)));
//          std::cout<<"prior value:"<<poseFrom.x()<<" "<<poseFrom.y()<<" "<<poseFrom.z()<<" "<<poseFrom.rotation().roll()<<" "
//                   <<poseFrom.rotation().pitch()<<" "<<poseFrom.rotation().yaw()<<std::endl;

          auto aligned = pose_estimator->correct(filtered);

          gtsam::Pose3 poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(pose_estimator->quat().w(),pose_estimator->quat().x(),pose_estimator->quat().y(),pose_estimator->quat().z()),
                                             gtsam::Point3(pose_estimator->pos()(0),pose_estimator->pos()(1),pose_estimator->pos()(2)));
//          std::cout<<"update value:"<<poseTo.x()<<" "<<poseTo.y()<<" "<<poseTo.z()<<" "<<poseTo.rotation().roll()<<" "
//                   <<poseTo.rotation().pitch()<<" "<<poseTo.rotation().yaw()<<std::endl;

          std::lock_guard<std::mutex> lock(imu_data_mutex);
          auto imu_iter = imu_data.begin();
          for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
              if(stamp < (*imu_iter)->header.stamp) {
                  break;
              }
              const auto& acc = (*imu_iter)->linear_acceleration;
              const auto& gyro = (*imu_iter)->angular_velocity;
              imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(acc.x, acc.y, acc.z),gtsam::Vector3(gyro.x, gyro.y, gyro.z), 0.01);
          }
          imu_data.erase(imu_data.begin(),imu_iter);

          gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key_count), poseTo, correctionNoise);
          newgraph.add(pose_factor);

          const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorImu_);
          gtsam::ImuFactor imu_factor(X(key_count - 1), V(key_count - 1), X(key_count), V(key_count), B(key_count - 1), preint_imu);

          newgraph.add(imu_factor);
          newgraph.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key_count - 1), B(key_count), gtsam::imuBias::ConstantBias(),
                                                                          gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorImu_->deltaTij()) * noiseModelBetweenBias)));

          gtsam::NavState propState_ = imuIntegratorImu_->predict(prevState_, prevBias_);

//          std::cout<<" imu integre propState:"<<propState_.pose().x()<<" "<<propState_.pose().y()<<" "<<propState_.pose().z()<<" "<<
//                   propState_.pose().rotation().roll()<<" "<<propState_.pose().rotation().pitch()<<" "<<propState_.pose().rotation().yaw()<<std::endl;

          if(propState_.pose().z()<-0.05)
          {
              gtsam::Quaternion quat(propState_.quaternion());
              quat.normalized();
              poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(propState_.pose().x(),propState_.pose().y(),0));
          }
          else if (propState_.pose().z()>0.05)
          {

              gtsam::Quaternion quat(propState_.quaternion());
              quat.normalized();
              poseTo = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(propState_.pose().x(),propState_.pose().y(),0));
          }

//          std::cout<<"propState_.pose().z():"<<propState_.pose().z()<<std::endl;

          noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << odomLinearNoise, odomLinearNoise, odomLinearNoise, odomAngularNoise, odomAngularNoise, odomAngularNoise).finished());

          newgraph.add(BetweenFactor<Pose3>(X(key_count-1), X(key_count), poseFrom.between(poseTo), odometryNoise));

//          Eigen::Vector3d vel(propState_.v()(0),propState_.v()(1),0);// test in indoor,limit the z velocity to 0
//          initialEstimate.insert(V(key_count), vel); //test in indoor,limit the z velocity to 0

          initialEstimate.insert(X(key_count), poseTo);
          initialEstimate.insert(V(key_count), propState_.v());
          initialEstimate.insert(B(key_count), prevBias_);
      }

      optimizer->update(newgraph, initialEstimate);
      newgraph.resize(0);
      initialEstimate.clear();
      gtsam::Pose3 latestEstimate;
      isamCurrentEstimate = optimizer->calculateEstimate();

      prevPose_ = isamCurrentEstimate.at<Pose3>(X(key_count));

      prevVel_ =isamCurrentEstimate.at<Vector3>(V(key_count));
      prevState_ = gtsam::NavState(prevPose_, prevVel_);
      prevBias_=isamCurrentEstimate.at<imuBias::ConstantBias>(B(key_count));
      imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);

      pose_estimator->update(prevState_,prevBias_);

      Eigen::Quaterniond quat(prevPose_.rotation().toQuaternion().w(),prevPose_.rotation().toQuaternion().x(),prevPose_.rotation().toQuaternion().y(),prevPose_.rotation().toQuaternion().z());
      quat.normalize();
      geometry_msgs::Quaternion odom_quat;
      odom_quat.w = quat.w();
      odom_quat.x = quat.x();
      odom_quat.y = quat.y();
      odom_quat.z = quat.z();

      geometry_msgs::TransformStamped odom_trans;
      odom_trans.header.stamp = stamp;
      odom_trans.header.frame_id = "map";
      odom_trans.child_frame_id = odom_child_frame_id;

      odom_trans.transform.translation.x = prevPose_.x();
      odom_trans.transform.translation.y = prevPose_.y();
      odom_trans.transform.translation.z = prevPose_.z();
      odom_trans.transform.rotation = odom_quat;

      pose_broadcaster.sendTransform(odom_trans);

      nav_msgs::Odometry laserOdometryROS;
      laserOdometryROS.header.stamp = points_msg->header.stamp;
      laserOdometryROS.header.frame_id = "map";
      laserOdometryROS.child_frame_id = odom_child_frame_id;
      laserOdometryROS.pose.pose.position.x = prevPose_.x();
      laserOdometryROS.pose.pose.position.y = prevPose_.y();
      laserOdometryROS.pose.pose.position.z = prevPose_.z();
      laserOdometryROS.pose.pose.orientation = odom_quat;
      pose_pub.publish(laserOdometryROS);

      points_pre_time = points_curr_time;
      ++key_count;

      if(withdetection)        //确定是否需要做树干检测并发布目标点
      {
          cur_pose[0]=prevPose_.x();
          cur_pose[1]=prevPose_.y();
          cur_pose[2]=prevPose_.rotation().yaw();

          if(comparepose(target,cur_pose)) {setTarget=true;}     //  make sure if it is need to set new target

          if(1){

              /***************pass through filter *****************/
              pcl::IndicesPtr pc_indices(new std::vector<int>);
              pcl::PassThrough<pcl::PointXYZI> pt;
              pt.setInputCloud(filtered);
              pt.setFilterFieldName("z");
              pt.setFilterLimits(z_axis_min_, z_axis_max_);
              pt.setFilterFieldName("x");
              pt.setFilterLimits(-1, 8);
              pt.setFilterFieldName("y");
              pt.setFilterLimits(-1.5, 3);
              pt.filter(*pc_indices);

              /*** Divide the point cloud into nested circular regions ***/
              boost::array<std::vector<int>, 8> indices_array;
              for(int i = 0; i < pc_indices->size(); i++) {
                  float range = 0.0;
                  for(int j = 0; j < region_max_; j++) {
                      float d2 = filtered->points[(*pc_indices)[i]].x * filtered->points[(*pc_indices)[i]].x +
                                 filtered->points[(*pc_indices)[i]].y * filtered->points[(*pc_indices)[i]].y +
                                 filtered->points[(*pc_indices)[i]].z * filtered->points[(*pc_indices)[i]].z;
                      if(d2 > range * range && d2 <= (range+regions_[j]) * (range+regions_[j])) {
                          indices_array[j].push_back((*pc_indices)[i]);
                          break;
                      }
                      range += regions_[j];
                  }
              }
              /*** Euclidean clustering ***/
              float tolerance = 0.0;
              std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr, Eigen::aligned_allocator<pcl::PointCloud<pcl::PointXYZI>::Ptr > > clusters;

              for(int i = 0; i < region_max_; i++) {
                  tolerance += 0.1;
                  if(indices_array[i].size() > cluster_size_min_)
                  {
                      boost::shared_ptr<std::vector<int> > indices_array_ptr(new std::vector<int>(indices_array[i]));
                      pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
                      tree->setInputCloud(filtered, indices_array_ptr);

                      std::vector<pcl::PointIndices> cluster_indices;
                      pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
                      ec.setClusterTolerance(tolerance);
                      ec.setMinClusterSize(cluster_size_min_);
                      ec.setMaxClusterSize(cluster_size_max_);
                      ec.setSearchMethod(tree);
                      ec.setInputCloud(filtered);
                      ec.setIndices(indices_array_ptr);
                      ec.extract(cluster_indices);

                      for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++)
                      {

                          pcl::PointCloud<pcl::PointXYZI>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZI>);
                          for(std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
                          {
                              cluster->points.push_back(filtered->points[*pit]);
                          }
                          /****************cylinder fitting*****************/
                          pcl::SACSegmentationFromNormals<pcl::PointXYZI, pcl::Normal> seg;
                          pcl::NormalEstimation<PointT, pcl::Normal> ne;
                          pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
                          pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
                          pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
                          pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
                          pcl::ExtractIndices<PointT> extract;
                          pcl::ExtractIndices<pcl::Normal> extract_normals;
                          pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
                          pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
                          ne.setSearchMethod (tree);
                          ne.setInputCloud (cluster);
                          ne.setKSearch (5);
                          ne.compute (*cloud_normals);
                          seg.setOptimizeCoefficients (true);
                          seg.setModelType (pcl::SACMODEL_CYLINDER);
                          seg.setMethodType (pcl::SAC_RANSAC);
                          seg.setNormalDistanceWeight (0.1);
                          seg.setMaxIterations (30);
                          seg.setDistanceThreshold (0.05);
                          seg.setRadiusLimits (0.07, 0.2);
                          seg.setInputCloud (cluster);
                          seg.setInputNormals (cloud_normals);
                          seg.segment (*inliers_cylinder, *coefficients_cylinder);
                          extract.setInputCloud (cluster);
                          extract.setIndices (inliers_cylinder);
                          extract.setNegative (false);
                          pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
                          extract.filter (*cloud_cylinder);
                          if (cloud_cylinder->size()>cylinderSize)
                          {
                              /************************************************/
                              Eigen::Vector4f centroid;
                              pcl::compute3DCentroid(*cloud_cylinder,centroid);
                              if (centroid[2]>trunkCentermin && centroid[2]<trunkCentermax)
                              {
//                                  std::cout<<"centroid:"<<centroid<<std::endl;
                                  cloud_cylinder->width = cloud_cylinder->size();
                                  cloud_cylinder->height = 1;
                                  cloud_cylinder->is_dense = true;
                                  clusters.push_back(cloud_cylinder);
                              }

                          }
//                              cluster->width = cluster->size();
//                              cluster->height = 1;
//                              cluster->is_dense = true;
//                              clusters.push_back(cluster);
                      }
                  }
              }

              geometry_msgs::PoseArray pose_array;
              visualization_msgs::MarkerArray marker_array;

              for(int i = 0; i < clusters.size(); i++) {
                  Eigen::Vector4f centroid;
                  pcl::compute3DCentroid(*clusters[i], centroid);

                  geometry_msgs::Pose pose;
                  pose.position.x = centroid[0];
                  pose.position.y = centroid[1];
                  pose.position.z = centroid[2];
                  pose.orientation.w = 1;

                  pose_array.poses.push_back(pose);

                  Eigen::Vector4f min, max;
                  pcl::getMinMax3D(*clusters[i], min, max);

                  visualization_msgs::Marker marker;
                  marker.header = points_msg->header;
                  marker.ns = "adaptive_clustering";
                  marker.id = i;
                  marker.type = visualization_msgs::Marker::LINE_LIST;
                  geometry_msgs::Point p[24];
                  p[0].x = max[0];  p[0].y = max[1];  p[0].z = max[2];
                  p[1].x = min[0];  p[1].y = max[1];  p[1].z = max[2];
                  p[2].x = max[0];  p[2].y = max[1];  p[2].z = max[2];
                  p[3].x = max[0];  p[3].y = min[1];  p[3].z = max[2];
                  p[4].x = max[0];  p[4].y = max[1];  p[4].z = max[2];
                  p[5].x = max[0];  p[5].y = max[1];  p[5].z = min[2];
                  p[6].x = min[0];  p[6].y = min[1];  p[6].z = min[2];
                  p[7].x = max[0];  p[7].y = min[1];  p[7].z = min[2];
                  p[8].x = min[0];  p[8].y = min[1];  p[8].z = min[2];
                  p[9].x = min[0];  p[9].y = max[1];  p[9].z = min[2];
                  p[10].x = min[0]; p[10].y = min[1]; p[10].z = min[2];
                  p[11].x = min[0]; p[11].y = min[1]; p[11].z = max[2];
                  p[12].x = min[0]; p[12].y = max[1]; p[12].z = max[2];
                  p[13].x = min[0]; p[13].y = max[1]; p[13].z = min[2];
                  p[14].x = min[0]; p[14].y = max[1]; p[14].z = max[2];
                  p[15].x = min[0]; p[15].y = min[1]; p[15].z = max[2];
                  p[16].x = max[0]; p[16].y = min[1]; p[16].z = max[2];
                  p[17].x = max[0]; p[17].y = min[1]; p[17].z = min[2];
                  p[18].x = max[0]; p[18].y = min[1]; p[18].z = max[2];
                  p[19].x = min[0]; p[19].y = min[1]; p[19].z = max[2];
                  p[20].x = max[0]; p[20].y = max[1]; p[20].z = min[2];
                  p[21].x = min[0]; p[21].y = max[1]; p[21].z = min[2];
                  p[22].x = max[0]; p[22].y = max[1]; p[22].z = min[2];
                  p[23].x = max[0]; p[23].y = min[1]; p[23].z = min[2];

                  for(int i = 0; i < 24; i++) {
                      marker.points.push_back(p[i]);
                  }
                  marker.scale.x = 0.02;
                  marker.color.a = 1.0;
                  marker.color.r = 0.0;
                  marker.color.g = 1.0;
                  marker.color.b = 0.5;
                  marker.lifetime = ros::Duration(0.1);
                  marker_array.markers.push_back(marker);
              }

//              if(pose_array.poses.size()) {
//                  pose_array.header = points_msg->header;
//                  pose_array_pub_.publish(pose_array);
//
//              } //这段是发布所有类的位置

              if(marker_array.markers.size()) {
                  marker_array_pub_.publish(marker_array);
              }

              if(setTarget)
              {
                  setTarget=false;
                  if(PoseArray.size()>0 && piter!=PoseArray.end())
                  {
                      target.clear();
                      target.assign((*piter).begin(),(*piter).end());
                      std::cout<<""<<target[0]<<" "<<target[1]<<" "<<target[2]<<std::endl;
                      setGoal(target[0],target[1],target[2]);
                      ac_.sendGoal(goal_);
                      ROS_INFO("New goal set");
                      piter++;
                  }
              }

          }
      }

      auto t2 = ros::WallTime::now();
      NODELET_INFO_STREAM("processing_time: " <<(t2 - t1).toSec()* 1000.0 << "[msec]");

  }

  /**
   * @brief callback for globalmap input
   * @param points_msg
   */
  void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
    NODELET_INFO("globalmap received!");
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    globalmap = cloud;
    registration->setInputTarget(globalmap);
  }

  /**
   * @brief callback for initial pose input ("2D Pose Estimate" on rviz)
   * @param pose_msg
   */
  void initialpose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg) {
    NODELET_INFO("initial pose received!!");
    std::lock_guard<std::mutex> lock(pose_estimator_mutex);
    const auto& p = pose_msg->pose.pose.position;
    const auto& q = pose_msg->pose.pose.orientation;
    pose_estimator.reset(
          new hdl_localization::PoseEstimator(
            registration,
            ros::Time::now(),
            Eigen::Vector3d(p.x, p.y, p.z),
            Eigen::Quaterniond(q.w, q.x, q.y, q.z),
            private_nh.param<double>("cool_time_duration", 0.5))
    );
  }

  /**
   * @brief downsampling
   * @param cloud   input cloud
   * @return downsampled cloud
   */
  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    if(!downsample_filter) {
      return cloud;
    }
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*filtered);
    filtered->header = cloud->header;
    return filtered;
  }

  /**
   * @brief publish odometry
   * @param stamp  timestamp
   * @param pose   odometry pose to be published
   */

  void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4d& pose) {
    // broadcast the transform over tf
    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", odom_child_frame_id);
    pose_broadcaster.sendTransform(odom_trans);

    // publish the transform
    nav_msgs::Odometry odom;
    odom.header.stamp = stamp;
    odom.header.frame_id = "map";

    odom.pose.pose.position.x = pose(0, 3);
    odom.pose.pose.position.y = pose(1, 3);
    odom.pose.pose.position.z = pose(2, 3);
    odom.pose.pose.orientation = odom_trans.transform.rotation;

    odom.child_frame_id = odom_child_frame_id;
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    pose_pub.publish(odom);
  }

  /**
   * @brief convert a Eigen::Matrix to TransformedStamped
   * @param stamp           timestamp
   * @param pose            pose matrix
   * @param frame_id        frame_id
   * @param child_frame_id  child_frame_id
   * @return transform
   */
  geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4d& pose, const std::string& frame_id, const std::string& child_frame_id) {
    Eigen::Quaterniond quat(pose.block<3, 3>(0, 0));
    quat.normalize();
    geometry_msgs::Quaternion odom_quat;
    odom_quat.w = quat.w();
    odom_quat.x = quat.x();
    odom_quat.y = quat.y();
    odom_quat.z = quat.z();

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = stamp;
    odom_trans.header.frame_id = frame_id;
    odom_trans.child_frame_id = child_frame_id;

    odom_trans.transform.translation.x = pose(0, 3);
    odom_trans.transform.translation.y = pose(1, 3);
    odom_trans.transform.translation.z = pose(2, 3);
    odom_trans.transform.rotation = odom_quat;

    return odom_trans;
  }

    void resetOptimization()
    {

        std::cout<<"resetOptimization"<<std::endl;
        gtsam::ISAM2Params optParameters;
        optParameters.relinearizeThreshold = 0.1;
        optParameters.relinearizeSkip = 1;
        optimizer = new gtsam::ISAM2(optParameters);
        gtsam::NonlinearFactorGraph newGraphFactors;
        newgraph = newGraphFactors;
        gtsam::Values NewGraphValues;
        initialEstimate = NewGraphValues;

//        gtsam::LevenbergMarquardtParams optParameters;
//        optParameters.setVerbosity("ERROR");
//        optParameters.setOrderingType("METIS");
//        optParameters.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
//        gtsam::NonlinearFactorGraph newGraphFactors;
//        newgraph = newGraphFactors;
//        gtsam::Values NewGraphValues;
//        initialEstimate = NewGraphValues;
//        optimizer= new gtsam::LevenbergMarquardtOptimizer optimizer(newgraph,initialEstimate,optParameters);
    }

    bool comparepose(std::vector<double>& pos1, std::vector<double>& pos2)
    {
      if (sqrt(pow((pos1[0]-pos2[0]),2)+pow((pos1[1]-pos2[1]),2))<transOffset && sqrt(pow((pos1[2]-pos2[2]),2))<angleOffset)
        {
            std::cout<<"transOffset:"<<sqrt(pow((pos1[0]-pos2[0]),2)+pow((pos1[1]-pos2[1]),2)+pow((pos1[2]-pos2[2]),2))<<std::endl;
            std::cout<<"angleOffset:"<<sqrt(pow((pos1[2]-pos2[2]),2))<<std::endl;
            return true;
        }
        return false;
    }
    int sendGoal(const move_base_msgs::MoveBaseGoal& goal)
    {
        ac_.sendGoal(goal);
        ROS_INFO("New goal set");
        return 1;
    }
    void setGoal(double& goal_position_x, double& goal_position_y, double& goal_pose_yaw)
    {
        goal_.target_pose.header.frame_id = "map";
        goal_.target_pose.header.stamp = ros::Time::now();
        goal_.target_pose.pose.position.x = goal_position_x;
        goal_.target_pose.pose.position.y = goal_position_y;
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0, 0, goal_pose_yaw);
        goal_.target_pose.pose.orientation = odom_quat;
    }
    void setInitialPose(double initial_pose_x, double initial_pose_y, double initial_pose_yaw)
    {
        initial_pose_.header.stamp = ros::Time::now();
        initial_pose_.header.frame_id = "map";
        initial_pose_.pose.pose.position.x = initial_pose_x;
        initial_pose_.pose.pose.position.y = initial_pose_y;
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0, 0, initial_pose_yaw);
        initial_pose_.pose.pose.orientation = odom_quat;
    }

    void getPoseArray(std::vector<std::vector<double>> & targetPoseArray)
    {
        const std::string pose_addr{"/home/xcy/graph_sw_localization/src/graph_localization/config/target.txt"};
        std::ifstream input_file(pose_addr.c_str());
        targetPoseArray.clear();
        if(input_file.is_open())
        {
            std::string str;
            geometry_msgs::Pose pose3d;
            std::vector<double> pose2d(3);

            while(getline(input_file,str)&&!str.empty())
            {
                std::istringstream stringGet(str);
                stringGet>>pose3d.position.x>>pose3d.position.y>>pose3d.position.z>>pose3d.orientation.x
                >>pose3d.orientation.y>>pose3d.orientation.z>>pose3d.orientation.w;
                double siny_cosp = 2 * (pose3d.orientation.w * pose3d.orientation.z + pose3d.orientation.x * pose3d.orientation.y);
                double cosy_cosp = 1 - 2 * (pose3d.orientation.y * pose3d.orientation.y + pose3d.orientation.z * pose3d.orientation.z);
                double yaw = std::atan2(siny_cosp, cosy_cosp);
                pose2d[0]=pose3d.position.x;
                pose2d[1]=pose3d.position.y;
                pose2d[2]=yaw;
                targetPoseArray.push_back(pose2d);
            }
            input_file.close();
        }
    }

private:
  // ROS
  ros::NodeHandle nh;
  ros::NodeHandle mt_nh;
  ros::NodeHandle private_nh;
  ros::Publisher pose_incre_pub;
  ros::Publisher pose_pub;

  std::string odom_child_frame_id;

  bool use_imu;
  bool invert_imu;

  float key_interval;
  int opt_frames;
  ros::Subscriber imu_sub;
  ros::Subscriber points_sub;
  ros::Subscriber globalmap_sub;
  ros::Subscriber initialpose_sub;
  ros::Subscriber updateGlobalPose_sub;

  ros::Time points_pre_time,points_curr_time;

  ros::Publisher aligned_pub;
  tf::TransformBroadcaster pose_broadcaster;
  tf::TransformBroadcaster pose_incre_broadcaster;
  tf::TransformListener tf_listener;

  // imu input buffer
  std::mutex imu_data_mutex;
  std::vector<sensor_msgs::ImuConstPtr> imu_data;

  // globalmap and registration method
  pcl::PointCloud<PointT>::Ptr globalmap;
  pcl::Filter<PointT>::Ptr downsample_filter;
  pcl::Registration<PointT, PointT>::Ptr registration;
  pcl::Registration<PointT, PointT>::Ptr localregistration;

  // pose estimator
  std::mutex pose_estimator_mutex;
  std::unique_ptr<hdl_localization::PoseEstimator> pose_estimator;

  // processing time buffer
  boost::circular_buffer<double> processing_time;

    // ********************the followings are for graph optimization*********************
public:
  bool firstkey=true;
  NonlinearFactorGraph newgraph;
  gtsam::ISAM2 *optimizer;
//  gtsam::LevenbergMarquardtOptimizer *optimizer;
  gtsam::Values initialEstimate;
  gtsam::Values isamCurrentEstimate;
  gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
  bool systemInitialized;
  int key_count;
  double imuAccNoise,imuGyrNoise,gravity;
  double odomLinearNoise,odomAngularNoise;

  gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
  gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
  gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
  gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
  gtsam::Vector noiseModelBetweenBias;

//  pcl::PointCloud<PointT>::Ptr pre_cloud;  //registration for local odometry
//  pcl::PointCloud<PointT>::Ptr cur_cloud;  //registration for local odometry
  gtsam::Vector3 prevVel_;
  gtsam::imuBias::ConstantBias prevBias_;
  gtsam::NavState prevState_;
  gtsam::Pose3 prevPose_;
//  gtsam::Pose3 poseFrom;

//  pub trunk pose
bool setTarget;
float transOffset=0.1;
float angleOffset=0.01;
std::vector<double> target{0,0,0};
std::vector<double> cur_pose{0,0,0};
float z_axis_min_, z_axis_max_;
const int region_max_ = 8; // Change this value to match how far you want to detect.
int regions_[100];
int cluster_size_min_;
int cluster_size_max_;
ros::Publisher pose_array_pub_,pubTarget;
ros::Publisher marker_array_pub_;
int cylinderSize;
bool withdetection;
float trunkCentermin,trunkCentermax;

std::vector<std::vector<double>> PoseArray{};
std::vector<std::vector<double>>::iterator piter;

private:
   MoveBaseClient ac_;
   move_base_msgs::MoveBaseGoal goal_;
   geometry_msgs::PoseWithCovarianceStamped initial_pose_;

};

}

PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet)


/*
Copyright 2017 Cleuton Sampaio de Melo Junior - O Bom Programador
http://obomprogramador.com

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.

To run this lib you will need two files:
- shape_predictor_5_face_landmarks.dat
- dlib_face_recognition_resnet_model_v1.dat

You can get them from:
- "http://dlib.net/files/shape_predictor_5_face_landmarks.dat.bz2"
- "http://dlib.net/files/dlib_face_recognition_resnet_model_v1.dat.bz2"

And you must set an environment variable with the path where you extract them:
- export HOGCOMPARATOR_PATH="<path>"

THIS CODE IS BASED ON
http://dlib.net/dnn_face_recognition_ex.cpp.html

*/
#include <jni.h>
#include <iostream>
#include <cstdlib>
#include "com_obomprogramador_hog_HogComparator.h"

//------------------------------------------------------------------
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <dlib/image_processing.h>
#include <dlib/opencv/cv_image.h>
#include <dlib/dnn.h>
#include <dlib/gui_widgets.h>
#include <dlib/clustering.h>
#include <dlib/string.h>
#include <dlib/image_io.h>
#include <dlib/image_processing/frontal_face_detector.h>

using namespace dlib;
using namespace std;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
                            alevel0<
                            alevel1<
                            alevel2<
                            alevel3<
                            alevel4<
                            max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
                            input_rgb_image_sized<150>
                            >>>>>>>>>>>>;

// ----------------------------------------------------------------------------------------

std::vector<matrix<rgb_pixel>> jitter_image(
    const matrix<rgb_pixel>& img
);

//------------------------------------------------------------------

typedef unsigned char BYTE;
/*
    This is the native method.
    You should avoid these prints (cout). Use exceptions to signal errors!
*/
JNIEXPORT jboolean JNICALL Java_com_obomprogramador_hog_HogComparator_compareFaces
    (JNIEnv * env, jobject obj, jlong addFoto1, jlong addFoto2) {
        const char* pPath = getenv ("HOGCOMPARATOR_PATH");
        std::string path(pPath);
        cv::Mat* pInputImage = (cv::Mat*)addFoto1;
        cv::Mat* pInputImage2 = (cv::Mat*)addFoto2;
        dlib::array2d<rgb_pixel> dlibImage;
        dlib::array2d<rgb_pixel> dlibImage2;
        dlib::assign_image(dlibImage, dlib::cv_image<bgr_pixel>(*pInputImage));
        dlib::assign_image(dlibImage2, dlib::cv_image<bgr_pixel>(*pInputImage2));
        dlib::frontal_face_detector detector = dlib::get_frontal_face_detector();
        dlib::shape_predictor sp;
        dlib::deserialize(path + "/shape_predictor_5_face_landmarks.dat") >> sp;
        anet_type net;
        dlib::deserialize(path + "/dlib_face_recognition_resnet_model_v1.dat") >> net;
        std::vector<matrix<rgb_pixel>> faces1;
        for (auto face : detector(dlibImage))
        {
            auto shape = sp(dlibImage, face);
            matrix<rgb_pixel> face_chip;
            dlib::extract_image_chip(dlibImage, dlib::get_face_chip_details(shape,150,0.25), face_chip);
            faces1.push_back(move(face_chip));
            break;
        }

        // Check whether there is any face in image 1:
        if (faces1.size() == 0)
        {
            cout << "No faces found in image 1!" << endl;
            return false;
        }

        std::vector<matrix<rgb_pixel>> faces2;
        for (auto face : detector(dlibImage2))
        {
            auto shape = sp(dlibImage2, face);
            matrix<rgb_pixel> face_chip;
            dlib::extract_image_chip(dlibImage2, dlib::get_face_chip_details(shape,150,0.25), face_chip);
            faces2.push_back(move(face_chip));
            break;
        }

        // Check whether there is any face in image 2:
        if (faces2.size() == 0)
        {
            cout << "No faces found in image 2!" << endl;
            return false;
        }

        std::vector<matrix<float,0,1>> face_descriptors1 = net(faces1);
        std::vector<matrix<float,0,1>> face_descriptors2 = net(faces2);

        bool thereIsAmatch = false;
        for (size_t i = 0; i < face_descriptors1.size(); ++i)
        {
            for (size_t j = i; j < face_descriptors2.size(); ++j)
            {
                if (length(face_descriptors1[i]-face_descriptors2[j]) < 0.6)
                    thereIsAmatch = true;
            }
        }

        return thereIsAmatch;
}

//int main (int argc, char *argv[]) {}


#include <merely3d/mesh.hpp>

#include <Eigen/Dense>

#include <array>

using Eigen::Vector3f;

namespace merely3d
{
    StaticMesh::StaticMesh(std::vector<float> vertices_and_normals, std::vector<unsigned int> faces)
            : _data(new detail::StaticMeshData(std::move(vertices_and_normals),
                                               std::move(faces)))
    {
        if (_data->faces.size() % 3 != 0)
        {
            throw std::invalid_argument("Faces must have size divisible by 3");
        }
        if (_data->vertices_and_normals.size() % 6 != 0)
        {
            throw std::invalid_argument("Vertices and normals must have size divisible by 6");
        }
    }

    StaticMesh::StaticMesh(std::vector<float> vertices,
                                  std::vector<float> normals,
                                  std::vector<unsigned int> faces)
    {
        if (vertices.size() % 3 != 0)
        {
            throw std::invalid_argument("Vertices must have size divisible by 3");
        }
        if (normals.size() % 3 != 0)
        {
            throw std::invalid_argument("Normals must have size divisible by 3");
        }
        if (faces.size() % 3 != 0)
        {
            throw std::invalid_argument("Faces must have size divisible by 3");
        }
        if (vertices.size() != normals.size())
        {
            throw std::invalid_argument("Number of vertices and normals must be the same.");
        }

        const auto num_vertices = vertices.size() / 3;
        decltype(vertices) vertices_and_normals;
        auto & vn = vertices_and_normals;

        for (size_t i = 0; i < num_vertices; ++i)
        {
            const auto vbegin = vertices.begin() + 3 * i;
            const auto nbegin = normals.begin() + 3 * i;
            vn.insert(vn.end(), vbegin, vbegin + 3);
            vn.insert(vn.end(), nbegin, nbegin + 3);
        }
        _data = std::make_shared<detail::StaticMeshData>(std::move(vertices_and_normals), std::move(faces));
    }

    StaticMesh StaticMesh::with_angle_weighted_normals(std::vector<float> vertices, std::vector<unsigned int> faces)
    {
        if (vertices.size() % 3 != 0)
        {
            throw std::invalid_argument("Vertices must have size divisible by 3");
        }
        if (faces.size() % 3 != 0)
        {
            throw std::invalid_argument("Faces must have size divisible by 3");
        }

        std::vector<float> normals(vertices.size(), 0.0f);

        for (size_t i = 0; i < faces.size(); i += 3)
        {
            const auto triangle = std::array<unsigned int, 3> { faces[i], faces[i + 1], faces[i + 2] };

            const auto v0 = Vector3f(vertices[3 * triangle[0]], vertices[3 * triangle[0] + 1], vertices[3 * triangle[0] + 2]);
            const auto v1 = Vector3f(vertices[3 * triangle[1]], vertices[3 * triangle[1] + 1], vertices[3 * triangle[1] + 2]);
            const auto v2 = Vector3f(vertices[3 * triangle[2]], vertices[3 * triangle[2] + 1], vertices[3 * triangle[2] + 2]);

            const auto face_vertices = std::array<Vector3f, 3> { v0, v1, v2 };
            const Vector3f face_normal = (v1 - v0).cross(v2 - v0);

            // For each vertex in the face, compute contribution to angle-weighted vertex normals
            for (size_t j = 0; j < 3; ++j)
            {
                // Compute left and right edge vectors that meet in local vertex (j + 1)
                const Vector3f left = (face_vertices[j] - face_vertices[(j + 1) % 3]).normalized();
                const Vector3f right = (face_vertices[(j + 2) % 3] - face_vertices[(j + 1) % 3]).normalized();
                const float dot = left.dot(right);
                // Rounding errors may push the dot product beyond the domain of acos, so we clamp the result
                // to the allowed range [-1, 1]
                const float angle = std::acos(std::max(-1.0f, std::min(dot, 1.0f)));
                const auto vertex_index = triangle[(j + 1) % 3];
                assert(vertex_index < vertices.size() / 3);
                assert(std::isfinite(angle));
                normals[3 * vertex_index + 0] += angle * face_normal[0];
                normals[3 * vertex_index + 1] += angle * face_normal[1];
                normals[3 * vertex_index + 2] += angle * face_normal[2];
            }
        }

        for (size_t i = 0; i < normals.size(); i += 3)
        {
            const Vector3f normal = Vector3f(normals[i], normals[i + 1], normals[i + 2]).normalized();
            normals[i + 0] = normal[0];
            normals[i + 1] = normal[1];
            normals[i + 2] = normal[2];
        }

        return StaticMesh(std::move(vertices), std::move(normals), std::move(faces));
    }
}


/*
 * Copyright (c) 2013-2014 Nicholas Corgan (n.corgan@gmail.com)
 *
 * Distributed under the MIT License (MIT) (See accompanying file LICENSE.txt
 * or copy at http://opensource.org/licenses/MIT)
 */

#include <boost/format.hpp>
#include <boost/assign/list_of.hpp>

#include <pkmn/calculations.hpp>
#include <pkmn/enums.hpp>
#include <pkmn/paths.hpp>
#include <pkmn/database/queries.hpp>
#include <pkmn/types/dict.hpp>
#include <pkmds/pkmds_g5_sqlite.h>

#include "SQLiteCpp/SQLiteC++.h"

namespace pkmn
{
    namespace calculations
    {
        uint8_t get_ability_num(uint32_t personality) {return personality % 2;}

        uint8_t get_gen2_gender(uint16_t species_id, uint8_t ivATK)
        {
            //PKMDS has function to get gender rate
            opendb(get_database_path().c_str());
            int gender_rate = getpkmgenderrate(::Species::species(species_id));
            closedb();

            dict<uint32_t, uint32_t> thresholds = boost::assign::map_list_of
                (0,0)
                (1,2)
                (2,4)
                (4,8)
                (6,12)
            ;

            switch(gender_rate)
            {
                case -1:
                    return Genders::GENDERLESS;

                case 0:
                    return Genders::MALE;

                case 8:
                    return Genders::FEMALE;

                default:
                    return (ivATK >= thresholds.at(gender_rate, 0)) ? Genders::MALE : Genders::FEMALE;
            }
        }

        uint8_t get_modern_gender(uint16_t species_id, uint32_t personality)
        {
            personality %= 256;

            //PKMDS has function to get gender rate
            opendb(get_database_path().c_str());
            int gender_rate = getpkmgenderrate(::Species::species(species_id));
            closedb();

            dict<uint32_t, uint32_t> ratios = boost::assign::map_list_of
                (0,254)
                (1,31)
                (2,63)
                (4,127)
                (6,191)
            ;

            switch(gender_rate)
            {
                case -1:
                    return Genders::GENDERLESS;

                case 0:
                    return Genders::MALE;

                case 8:
                    return Genders::FEMALE;

                default:
                    return (personality > ratios.at(gender_rate, 0)) ? Genders::MALE : Genders::FEMALE;
            }
        }

        std::pair<uint8_t, uint8_t> get_hidden_power(uint8_t ivHP, uint8_t ivATK, uint8_t ivDEF,
                                                     uint8_t ivSATK, uint8_t ivSDEF, uint8_t ivSPD)
        {
            ivHP %= 4;
            ivATK %= 4;
            ivDEF %= 4;
            ivSPD %= 4;
            ivSATK %= 4;
            ivSDEF %= 4;

            uint8_t power, type;

            power = (32 * (ivHP >> 1))
                  + (16 * (ivATK >> 1))
                  + (8 * (ivDEF >> 1))
                  + (4 * (ivSPD >> 1))
                  + (2 * (ivSATK >> 1))
                  + (ivSDEF >> 1);

            type = (32 * (ivHP & 1))
                 + (16 * (ivATK & 1))
                 + (8 * (ivDEF & 1))
                 + (4 * (ivSPD & 1))
                 + (2 * (ivSATK & 1))
                 + (ivSDEF & 1);

            return std::make_pair(power, type);
        }

        uint8_t get_nature(uint32_t personality) {return (personality % 24);}

        bool get_gen2_shiny(uint8_t ivATK, uint8_t ivDEF,
                            uint8_t ivSPD, uint8_t ivSPCL)
        {
            return ((ivDEF == 10 and ivSPD == 10 and ivSPCL == 10) and
                    (ivATK == 2 or ivATK == 3 or ivATK == 6 or ivATK == 7
                     or ivATK == 10 or ivATK == 11 or ivATK == 14
                     or ivATK == 15));
        }

        bool get_modern_shiny(uint32_t personality, uint16_t secret_tid, uint16_t public_tid)
        {
            uint16_t p1 = personality & 0xFF00;
            uint16_t p2 = personality & 0xFF;

            return ((secret_tid xor public_tid xor p1 xor p2) < 8);
        }

        uint32_t get_gen2_unown_form(uint8_t ivATK, uint8_t ivDEF,
                                                 uint8_t ivSPD, uint8_t ivSPCL)
        {
            uint8_t form = (((ivATK & 6) << 6)
                         +  ((ivDEF & 6) << 4)
                         +  ((ivSPD & 6) << 2)
                         +  (ivSPCL & 6));
            form /= 10;

            return (form == 0) ? 201 : (Forms::Unown::B + form - 1);
        }

        uint32_t get_gen3_unown_form(uint32_t personality)
        {
            uint32_t form = ((personality & 0x3000000)
                          +  (personality & 0x30000)
                          +  (personality & 0x300)
                          +  (personality & 0x3));
            form %= 28;

            return (form = 0) ? 201 : (Forms::Unown::B + form - 1);
        }

        uint16_t get_wurmple_evolution(uint32_t personality)
        {
            return (((personality % 65536) % 10) < 5) ? Species::SILCOON : Species::CASCOON;
        }

                unsigned int get_min_possible_stat(base_pokemon::sptr b_pkmn,
                                           std::string stat_name,
                                           unsigned int level,
                                           unsigned int gen)
        {
            pkmn::dict<std::string, unsigned int> stats = b_pkmn->get_base_stats();

            //Check inputs for errors
            if(not stats.has_key(stat_name)) return 0;
            if(gen < 1 or gen > 6) return 0;

            //Gen 1-2: IV = 0, EV = 0
            //Gen 3-5: IV = 0, EV = 0, disadvantageous nature
            if(gen == 1 or gen == 2)
            {
                if(stat_name == "HP")
                    return (unsigned int)(floor(((stats["HP"]+50.0)*level)/50.0)+10.0);
                else
                    return (unsigned int)(floor((stats[stat_name]*level)/50.0)+5.0);
            }
            else
            {
                if(stat_name == "HP")
                    return (unsigned int)(floor(((((2.0*stats["HP"])+100.0)
                           *level)+10.0)/100.0));
                else
                    return (unsigned int)(floor(((((2.0*stats[stat_name])*level)/100.0)+5.0)*0.9));
            }
        }

        unsigned int get_max_possible_stat(base_pokemon::sptr b_pkmn,
                                           std::string stat_name,
                                           unsigned int level,
                                           unsigned int gen)
        {
            pkmn::dict<std::string, unsigned int> stats = b_pkmn->get_base_stats();

            //Check inputs for errors
            if(stats.has_key(stat_name)) return -1;
            if(gen < 1 or gen > 6) return -1;

            //Gen 1-2: IV = 15, EV = 65535
            //Gen 3-5: IV = 31, EV = 255, advantageous nature
            if(gen == 1 or gen == 2)
            {
                if(stat_name == "HP")
                    return (unsigned int)(floor(((((15.0+stats["HP"]+
                           (pow(65535.0,0.5)/8.0)+50.0)*level)/50.0)+10.0)));
                else
                    return (unsigned int)(floor((((15.0+stats[stat_name]+(pow(65535.0,0.5)/8.0))
                           *level)/50.0)+5.0));
            }
            else
            {
                if(stat_name == "HP")
                    return (unsigned int)(floor(((31.0+(2.0*stats["HP"]+(255.0/4.0)+100.0)
                            *level)/100.0)+10.0));
                else
                    return (unsigned int)(floor((((31.0 + (2.0*stats[stat_name]+(255.0/4.0))
                            *level) / 100.0) + 5.0) * 1.1));
            }
        }

        bool is_stat_possible(base_pokemon::sptr b_pkmn, unsigned int stat_value,
                              std::string stat_name, unsigned int level, unsigned int gen)
        {
            return (stat_value > get_min_possible_stat(b_pkmn,stat_name,level,gen)
                   and stat_value < get_max_possible_stat(b_pkmn,stat_name,level,gen));
        }

        std::pair<unsigned int, unsigned int> get_stat_range(base_pokemon::sptr b_pkmn, std::string stat_name,
                                                             unsigned int level, unsigned int gen)
        {
            std::pair<unsigned int, unsigned int> stat_pair;

            stat_pair.first = get_min_possible_stat(b_pkmn, stat_name, level, gen);
            stat_pair.second = get_max_possible_stat(b_pkmn, stat_name, level, gen);

            return stat_pair;
        }

        //TODO: Account for Gen V-VI differences
        double get_type_damage_mod(std::string type1, std::string type2, unsigned int gen)
        {
            SQLite::Database db(get_database_path().c_str());

            unsigned int type1_id = database::get_type_id(type1);
            unsigned int type2_id = database::get_type_id(type2);

            if((gen == 1) and (type1_id == Types::DARK or type1_id == Types::STEEL
                          or type2_id == Types::DARK or type2_id == Types::STEEL)) return 1;

            double damage_mod;
            string query_string;

            if(type1_id != Types::NONE and type1_id != Types::QUESTION_MARK and type1_id != Types::SHADOW
            and type2_id != Types::NONE and type2_id != Types::QUESTION_MARK and type2_id != Types::SHADOW)
            {
                //Get damage mod from database
                query_string = str(boost::format("SELECT damage_factor FROM type_efficacy WHERE damage_type_id=%d AND target_type_id=%d")
                                                 % type1 % type2);
                damage_mod = db.execAndGet(query_string.c_str());
                damage_mod /= 100; //Stored as 50, 100, or 200 in database

                return damage_mod;
            }
            else return 1.0;
        }

        unsigned int get_base_damage(unsigned int level, unsigned int attack,
                                     unsigned int defense, unsigned int base_power)
        {
            return (unsigned int)(floor((((2.0 * double(level) + 10.0) / 250.0) *
                   (double(attack) / double(defense)) * double(base_power) * 2.0)));
        }

        unsigned int get_base_damage(team_pokemon::sptr attacker, team_pokemon::sptr defender,
                                     move::sptr attack)
        {
            unsigned int level = attacker->get_level();
            unsigned int base_power = attack->get_base_power();
            unsigned int attacker_ATK, defender_DEF;

            unsigned int move_damage_class = attack->get_move_damage_class_id();
            switch(move_damage_class)
            {
                case Move_Classes::PHYSICAL:
                    attacker_ATK = attacker->get_stats()["Attack"];
                    defender_DEF = defender->get_stats()["Defense"];
                    break;

                case Move_Classes::SPECIAL:
                    if(attacker->get_generation() == 1)
                    {
                        attacker_ATK = attacker->get_stats()["Special"];
                        defender_DEF = defender->get_stats()["Special"];
                    }
                    else
                    {
                        attacker_ATK = attacker->get_stats()["Special Attack"];
                        defender_DEF = defender->get_stats()["Special Defense"];
                    }
                    break;

                default:
                    return 0;
            }
            return get_base_damage(level, attacker_ATK, defender_DEF, base_power);
        }

        //Abilities not taken unsigned into account
        std::pair<unsigned int, unsigned int> get_damage_range(base_pokemon::sptr attacker, base_pokemon::sptr defender,
                                                               move::sptr attack, unsigned int attacker_level,
                                                               unsigned int defender_level)
        {
            std::pair<unsigned int, unsigned int> damage_pair;

            unsigned int attacker_min_ATK, attacker_max_ATK;
            unsigned int defender_min_DEF, defender_max_DEF;
            unsigned int min_damage, max_damage;

            unsigned int move_damage_class = attack->get_move_damage_class_id();
            unsigned int gen = attacker->get_generation();
            switch(move_damage_class)
            {
                case Move_Classes::PHYSICAL:
                    attacker_min_ATK = get_min_possible_stat(attacker, "Attack", attacker_level, gen);
                    attacker_max_ATK = get_max_possible_stat(attacker, "Attack", attacker_level, gen);
                    defender_min_DEF = get_min_possible_stat(defender, "Defense", defender_level, gen);
                    defender_max_DEF = get_max_possible_stat(defender, "Defense", defender_level, gen);
                    break;

                case Move_Classes::SPECIAL:
                    if(gen == 1)
                    {
                        attacker_min_ATK = get_min_possible_stat(attacker, "Special", attacker_level, 1);
                        attacker_max_ATK = get_max_possible_stat(attacker, "Special", attacker_level, 1);
                        defender_min_DEF = get_min_possible_stat(defender, "Special", defender_level, 1);
                        defender_max_DEF = get_max_possible_stat(defender, "Special", defender_level, 1);
                    }
                    else
                    {
                        attacker_min_ATK = get_min_possible_stat(attacker, "Special Attack", attacker_level, gen);
                        attacker_max_ATK = get_max_possible_stat(attacker, "Special Attack", attacker_level, gen);
                        defender_min_DEF = get_min_possible_stat(defender, "Special Defense", defender_level, gen);
                        defender_max_DEF = get_max_possible_stat(defender, "Special Defense", defender_level, gen);
                    }
                    break;

                default:
                    damage_pair.first = 0;
                    damage_pair.second = 0;
                    return damage_pair;
            }
            min_damage = (unsigned int)(floor(get_base_damage(attacker_level, attacker_min_ATK, defender_max_DEF, attack->get_base_power()) * 0.85));
            max_damage = get_base_damage(attacker_level, attacker_max_ATK, defender_min_DEF, attack->get_base_power());

            double type_mod = double(get_type_damage_mod(attack->get_type(), defender->get_types().first, gen)) *
                              double(get_type_damage_mod(attack->get_type(), defender->get_types().second, gen));

            min_damage = (unsigned int)(floor(double(min_damage) * type_mod));
            max_damage = (unsigned int)(floor(double(max_damage) * type_mod));

            if(type_mod != 0)
            {
                if(defender->get_species_id() == Species::SHEDINJA)
                {
                    if(type_mod > 1.0)
                    {
                        /*
                         * Shedinja's ability, Wonder Guard, blocks any attacks except for
                         * super-effective moves. However, super-effective moves will
                         * automatically make it faint.
                         */
                        damage_pair.first = 1;
                        damage_pair.second = 1;
                        return damage_pair;
                    }
                    else
                    {
                        damage_pair.first = 0;
                        damage_pair.second = 0;
                        return damage_pair;
                    }
                }
                else if(attack->get_move_id() == Moves::SEISMIC_TOSS
                        or attack->get_move_id() == Moves::NIGHT_SHADE)
                {
                    /*
                     * Seismic Toss and Night Shade's damage automatically
                     * matches the attacker's level.
                     */
                    damage_pair.first = attacker_level;
                    damage_pair.second = attacker_level;
                    return damage_pair;
                }
                else if(attack->get_move_id() == Moves::DRAGON_RAGE)
                {
                    //Dragon Rage always deals 40 damage
                    damage_pair.first = 40;
                    damage_pair.second = 40;
                    return damage_pair;
                }
                else if(attack->get_move_id() == Moves::SONIC_BOOM)
                {
                    //Sonicboom always deals 20 damage
                    damage_pair.first = 20;
                    damage_pair.second = 20;
                    return damage_pair;
                }
                else
                {
                    /*
                     * STAB: Same-type attack bonus
                     * If an attack's type matches the attacker's type,
                     * it's damage will be increased by 150%.
                     */
                    if(attack->get_type() == attacker->get_types().first or
                       attack->get_type() == attacker->get_types().second)
                    {
                        min_damage = (unsigned int)(floor(double(min_damage) * 1.5));
                        max_damage = (unsigned int)(floor(double(max_damage) * 1.5));
                    }
                }
                /*
                 * As long as the attack does any damage, it will do a minimum
                 * of 1, no matter the strength discrepancy.
                 */
                if(min_damage == 0) min_damage = 1;
                if(max_damage == 0) max_damage = 1;

                damage_pair.first = min_damage;
                damage_pair.second = max_damage;
                return damage_pair;
            }
            else
            {
                damage_pair.first = 0;
                damage_pair.second = 0;
                return damage_pair;
            }
        }

        std::pair<unsigned int, unsigned int> get_damage_range(team_pokemon::sptr attacker, team_pokemon::sptr defender,
                                                               move::sptr attack)
        {
            return get_damage_range(attacker->get_base_pokemon(), defender->get_base_pokemon(),
                                    attack, attacker->get_level(), defender->get_level());
        }
    }
}


//  Copyright 2020 Fraunhofer-Gesellschaft zur Förderung der angewandten Forschung e.V.
//
//  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
//
//  https: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 "mars_agent_physical_common/RobotAgentProperties.h"

#include "mars_routing_common/utility/AffineProfile.h"
#include "mars_routing_common/utility/ConstantProfile.h"
#include "mars_routing_common/utility/MotionProfile.h"
#include "mars_routing_common/utility/differential.h"
#include "mars_routing_common/utility/timeprediction.h"

#include "mars_common/geometry/Segment.h"
#include "mars_routing_common/geometry/utilities.h"

#include <algorithm>
#include <stdlib.h>
#include <type_traits>

#include <boost/geometry/algorithms/intersection.hpp>
#include <eigen3/Eigen/Core>

template <class Origin, class Target, class Profile>
boost::optional<ros::Duration>
mars::routing::common::utility::timeprediction<Origin, Target, Profile>::getResourceDuration(
    Origin& pCurrentEntity, Target& pNextEntity, Target& pPreviousEntity,
    const mars::agent::physical::common::RobotAgentProperties& pRAP, double& pVelocity,
    mars::routing::common::utility::TimeInfo<Profile>& pTimeInfo)
{
  static_assert(std::is_base_of<mars::routing::common::utility::MotionProfile, Profile>::value,
                "Profile type must "
                "extend MotionProfile");

  boost::optional<Profile> lRotationProfile;
  boost::optional<Profile> lEntryProfile;
  boost::optional<Profile> lExitProfile;
  boost::optional<double> lAngularMotion;
  boost::optional<double> lEntryMotion;
  boost::optional<double> lExitMotion;
  ros::Duration lRobotFrontEntryDuration;
  ros::Duration lRobotBackExitDuration;
  ros::Duration lRessourceDuration;

  boost::optional<Eigen::Vector3d> lBoostOptionalCurrentLocation = pCurrentEntity.getLocation();
  Eigen::Vector3d lCurrentLocation = lBoostOptionalCurrentLocation.value();
  boost::optional<Eigen::Vector3d> lBoostOptionalNextLocation = pNextEntity.getLocation();
  Eigen::Vector3d lNextLocation = lBoostOptionalNextLocation.value();
  boost::optional<Eigen::Vector3d> lBoostOptionalPreviousLocation = pPreviousEntity.getLocation();
  Eigen::Vector3d lPreviousLocation = lBoostOptionalPreviousLocation.value();

  if (!lBoostOptionalCurrentLocation || !lBoostOptionalNextLocation || !lBoostOptionalPreviousLocation)
  {
    return boost::none;
  }

  double lRotationAngle = mars::routing::common::geometry::utilities::get2DRotationAngle(
      lCurrentLocation, lNextLocation,
      mars::routing::common::geometry::utilities::get2DAngleBetweenPoints(lPreviousLocation,
                                                                          lCurrentLocation));

  mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setMotion(
      lAngularMotion, lEntryMotion, lExitMotion, lRotationAngle, pRAP, pCurrentEntity, pNextEntity,
      pPreviousEntity);

  if (!lAngularMotion || !lEntryMotion || !lExitMotion)
  {
    return boost::none;
  }

  mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setProfile(
      lRotationProfile, lEntryProfile, lExitProfile, *lAngularMotion, *lEntryMotion, *lExitMotion,
      pRAP, pCurrentEntity, pNextEntity, pVelocity);

  if (!lRotationProfile || !lEntryProfile || !lExitProfile)
  {
    return boost::none;
  }

  pVelocity = lExitProfile->getExitVelocity();
  double tmpEntryVelo = lEntryProfile->getExitVelocity();

  lRobotFrontEntryDuration =
      ros::Duration(pRAP.getFrontLength() / lEntryProfile->getEntryVelocity());
  lRobotBackExitDuration = ros::Duration(pRAP.getBackLength() / lExitProfile->getExitVelocity());

  lRessourceDuration = ros::Duration(lRobotFrontEntryDuration + lEntryProfile->getMotionDuration() +
                                     lRotationProfile->getMotionDuration() +
                                     lExitProfile->getMotionDuration() + lRobotBackExitDuration);

  pTimeInfo.setEntryMotion(lEntryMotion.value());
  pTimeInfo.setEntryProfile(lEntryProfile.value());
  pTimeInfo.setAngularMotion(lAngularMotion.value());
  pTimeInfo.setRotationProfile(lRotationProfile.value());
  pTimeInfo.setExitMotion(lExitMotion.value());
  pTimeInfo.setExitProfile(lExitProfile.value());

  return lRessourceDuration;
}

template <class Origin, class Target, class Profile>
boost::optional<ros::Duration>
mars::routing::common::utility::timeprediction<Origin, Target, Profile>::getResourceDuration(
    Origin& pCurrentEntity, Target& pNextEntity, double pOrientation,
    const mars::agent::physical::common::RobotAgentProperties& pRAP, double& pVelocity,
    mars::routing::common::utility::TimeInfo<Profile>& pTimeInfo)
{
  static_assert(std::is_base_of<mars::routing::common::utility::MotionProfile, Profile>::value,
                "Profile type must "
                "extend MotionProfile");

  boost::optional<Profile> lRotationProfile;
  boost::optional<Profile> lEntryProfile;
  boost::optional<Profile> lExitProfile;
  boost::optional<double> lAngularMotion;
  boost::optional<double> lEntryMotion;
  boost::optional<double> lExitMotion;
  ros::Duration lRobotBackExitDuration;

  boost::optional<Eigen::Vector3d> lCurrentLocation = pCurrentEntity.getLocation();
  boost::optional<Eigen::Vector3d> lNextLocation = pNextEntity.getLocation();

  if (!lCurrentLocation || !lNextLocation)
  {
    return boost::none;
  }

  double lRotationAngle = mars::routing::common::geometry::utilities::get2DRotationAngle(
      *lCurrentLocation, *lNextLocation, pOrientation);

  mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setMotion(
      lAngularMotion, lEntryMotion, lExitMotion, lRotationAngle, pRAP, pCurrentEntity, pNextEntity);

  if (!lAngularMotion || !lExitMotion)
  {
    return boost::none;
  }

  mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setProfile(
      lRotationProfile, lEntryProfile, lExitProfile, *lAngularMotion, 0, *lExitMotion, pRAP,
      pCurrentEntity, pNextEntity, pVelocity);

  if (!lRotationProfile || !lEntryProfile || !lExitProfile)
  {
    return boost::none;
  }

  pVelocity = lExitProfile->getExitVelocity();

  lRobotBackExitDuration = ros::Duration(pRAP.getBackLength() / lExitProfile->getExitVelocity());

  //pTimeInfo.setEntryMotion(lEntryMotion.value());
 // pTimeInfo.setEntryProfile(lEntryProfile.value());
  pTimeInfo.setAngularMotion(lAngularMotion.value());
  pTimeInfo.setRotationProfile(lRotationProfile.value());
  pTimeInfo.setExitMotion(lExitMotion.value());
  pTimeInfo.setExitProfile(lExitProfile.value());

  return lEntryProfile->getMotionDuration() + lRotationProfile->getMotionDuration() +
         lExitProfile->getMotionDuration() + lRobotBackExitDuration;
}

template <class Origin, class Target, class Profile>
void mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setMotion(
    boost::optional<double>& pAngularMotion, boost::optional<double>& pEntryMotion,
    boost::optional<double>& pExitMotion, double pRotationAngle,
    const mars::agent::physical::common::RobotAgentProperties& pRAP, Origin& pCurrentEntity,
    Target& pNextEntity)
{
  static_assert(std::is_base_of<mars::routing::common::utility::MotionProfile, Profile>::value,
                "Profile type must "
                "extend MotionProfile");

  double lTurningRadius = pRAP.getTurningRadius();

  bool lAxialTurn = lTurningRadius == 0;
  if (lAxialTurn)
  {
    mars::routing::common::utility::differential<Origin, Target, Profile>::setMotion(
        pAngularMotion, pEntryMotion, pExitMotion, pRotationAngle, pCurrentEntity, pNextEntity);
  }
  else
  {
    pAngularMotion = 0;
    mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setAckermannMotion(
        pEntryMotion, pExitMotion, lTurningRadius, pCurrentEntity, pNextEntity);
  }
}

template <class Origin, class Target, class Profile>
void mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setMotion(
    boost::optional<double>& pAngularMotion, boost::optional<double>& pEntryMotion,
    boost::optional<double>& pExitMotion, double pRotationAngle,
    const mars::agent::physical::common::RobotAgentProperties& pRAP, Origin& pCurrentEntity,
    Target& pNextEntity, Target& pPreviousEntity)
{
  static_assert(std::is_base_of<mars::routing::common::utility::MotionProfile, Profile>::value,
                "Profile type must "
                "extend MotionProfile");

  double lTurningRadius = pRAP.getTurningRadius();

  bool lAxialTurn = lTurningRadius == 0.0;
  if (lAxialTurn)
  {
    mars::routing::common::utility::differential<Origin, Target, Profile>::setMotion(
        pAngularMotion, pEntryMotion, pExitMotion, pRotationAngle, pCurrentEntity, pNextEntity,
        pPreviousEntity);
  }
  else
  {
    pAngularMotion = 0;
    mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setAckermannMotion(
        pEntryMotion, pExitMotion, lTurningRadius, pCurrentEntity, pNextEntity, pPreviousEntity);
  }
}

template <class Origin, class Target, class Profile>
void mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setAckermannMotion(
    boost::optional<double>& pEntryMotion, boost::optional<double>& pExitMotion,
    double pTurningRadius, Origin& pCurrentEntity, Target& pNextEntity)
{
  static_assert(std::is_base_of<mars::routing::common::utility::MotionProfile, Profile>::value,
                "Profile type must "
                "extend MotionProfile");

  // TODO: Tangential circle calculation
  // For now no Ackermann calculations available.
  pEntryMotion = boost::none;
  pExitMotion = boost::none;
}

template <class Origin, class Target, class Profile>
void mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setAckermannMotion(
    boost::optional<double>& pEntryMotion, boost::optional<double>& pExitMotion,
    double pTurningRadius, Origin& pCurrentEntity, Target& pNextEntity, Target& pPreviousEntity)
{
  static_assert(std::is_base_of<mars::routing::common::utility::MotionProfile, Profile>::value,
                "Profile type must "
                "extend MotionProfile");

  // TODO: Tangential circle calculation
  // For now no Ackermann calculations available.
  pEntryMotion = boost::none;
  pExitMotion = boost::none;
}

template <class Origin, class Target, class Profile>
void mars::routing::common::utility::timeprediction<Origin, Target, Profile>::setProfile(
    boost::optional<Profile>& pRotationProfile, boost::optional<Profile>& pEntryProfile,
    boost::optional<Profile>& pExitProfile, double pAngularMotion, double pEntryMotion,
    double pExitMotion, const mars::agent::physical::common::RobotAgentProperties& pRAP,
    Origin& pCurrentEntity, Target& pNextEntity, double pVelocity)
{
  static_assert(std::is_base_of<mars::routing::common::utility::MotionProfile, Profile>::value,
                "Profile type must "
                "extend MotionProfile");

  boost::optional<mars::topology::common::TopologyEntityRestrictions&> lCurrentEntityRestrictions =
      pCurrentEntity.getRestrictions();
  boost::optional<mars::topology::common::TopologyEntityRestrictions&> lNextEntityRestrictions =
      pNextEntity.getRestrictions();

  if (!lCurrentEntityRestrictions || !lNextEntityRestrictions)
  {
    pRotationProfile = boost::none;
    pEntryProfile = boost::none;
    pExitProfile = boost::none;
    return;
  }

  double lLinearVelocity =
      std::min(pRAP.getForwardVelocity(), lCurrentEntityRestrictions->getMaxLinearVelocity());
  double lLinearAcceleration = std::min(pRAP.getLinearAcceleration(),
                                        lCurrentEntityRestrictions->getMaxLinearAcceleration());
  double lLinearDeceleration = std::min(pRAP.getLinearDeceleration(),
                                        lCurrentEntityRestrictions->getMaxLinearAcceleration());
  double lAngularVelocity =
      std::min(pRAP.getAngularVelocity(), lCurrentEntityRestrictions->getMaxAngularVelocity());
  double lAngularAcceleration = std::min(pRAP.getAngularAcceleration(),
                                         lCurrentEntityRestrictions->getMaxAngularAcceleration());
  double lAngularDeceleration = std::min(pRAP.getAngularDeceleration(),
                                         lCurrentEntityRestrictions->getMaxAngularAcceleration());

  double lExitVelocity =
      std::min(pRAP.getForwardVelocity(), lNextEntityRestrictions->getMaxLinearVelocity());

  pRotationProfile =
      Profile(pAngularMotion, 0, lAngularVelocity, 0, lAngularAcceleration, lAngularDeceleration);

  bool lNoRotation = pAngularMotion == 0;
  if (lNoRotation)
  {
    pEntryProfile = Profile(pEntryMotion, pVelocity, lLinearVelocity, lLinearVelocity,
                            lLinearAcceleration, lLinearDeceleration);
    pExitProfile = Profile(pExitMotion, pEntryProfile->getExitVelocity(), lLinearVelocity,
                           lExitVelocity, lLinearAcceleration, lLinearDeceleration);
  }
  else
  {
    pEntryProfile = Profile(pEntryMotion, pVelocity, lLinearVelocity, 0, lLinearAcceleration,
                            lLinearDeceleration);
    pExitProfile = Profile(pExitMotion, 0, lLinearVelocity, lExitVelocity, lLinearAcceleration,
                           lLinearDeceleration);
  }
}

template <class Origin, class Target, class Profile>
ros::Duration
mars::routing::common::utility::timeprediction<Origin, Target, Profile>::getMotionDuration(
    double pMotion, const mars::agent::physical::common::RobotAgentProperties& pRAP)
{
  static_assert(std::is_base_of<mars::routing::common::utility::MotionProfile, Profile>::value,
                "Profile type must "
                "extend MotionProfile");

  Profile lMotionProfile(pMotion, 0, pRAP.getForwardVelocity(), 0, pRAP.getLinearAcceleration(),
                         pRAP.getLinearDeceleration());

  return lMotionProfile.getMotionDuration();
}

template <class Origin, class Target, class Profile>
mars::routing::common::utility::timeprediction<Origin, Target, Profile>::timeprediction()
{
}

template class mars::routing::common::utility::timeprediction<
    mars::routing::common::topology::Vertex, mars::routing::common::topology::Edge,
    mars::routing::common::utility::AffineProfile>;

template class mars::routing::common::utility::timeprediction<
    mars::routing::common::topology::Edge, mars::routing::common::topology::Vertex,
    mars::routing::common::utility::AffineProfile>;