/*
 * Copyright 2015-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.
 */

/*
 Localization program using Normal Distributions Transform

 Yuki KITSUKAWA
 */

#include <pthread.h>
#include <chrono>
#include <fstream>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>

#include <thread>

#include <boost/filesystem.hpp>

//#include <nav_msgs/Odometry.h>
//#include <ros/ros.h>
//#include <sensor_msgs/Imu.h>
//#include <sensor_msgs/PointCloud2.h>
//#include <std_msgs/Bool.h>
//#include <std_msgs/Float32.h>
//#include <std_msgs/String.h>
//#include <velodyne_pointcloud/point_types.h>
//#include <velodyne_pointcloud/rawdata.h>

//#include <geometry_msgs/PoseWithCovarianceStamped.h>
//#include <geometry_msgs/TwistStamped.h>

#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

//#include <ndt_cpu/NormalDistributionsTransform.h>
#include <pcl/registration/ndt.h>

#include <ndt_gpu/NormalDistributionsTransform.h>
#include "ndtpos.pb.h"

#ifdef USE_PCL_OPENMP
#include <pcl_omp_registration/ndt.h>
#endif

#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>

//#include <autoware_config_msgs/ConfigNDT.h>

//#include <autoware_msgs/NDTStat.h>

#include "ndt_matching.h"

#include "modulecomm.h"

//the following are UBUNTU/LINUX ONLY terminal color codes.
#define RESET   "\033[0m"
#define BLACK   "\033[30m"      /* Black */
#define RED     "\033[31m"      /* Red */
#define GREEN   "\033[32m"      /* Green */
#define YELLOW  "\033[33m"      /* Yellow */
#define BLUE    "\033[34m"      /* Blue */
#define MAGENTA "\033[35m"      /* Magenta */
#define CYAN    "\033[36m"      /* Cyan */
#define WHITE   "\033[37m"      /* White */
#define BOLDBLACK   "\033[1m\033[30m"      /* Bold Black */
#define BOLDRED     "\033[1m\033[31m"      /* Bold Red */
#define BOLDGREEN   "\033[1m\033[32m"      /* Bold Green */
#define BOLDYELLOW  "\033[1m\033[33m"      /* Bold Yellow */
#define BOLDBLUE    "\033[1m\033[34m"      /* Bold Blue */
#define BOLDMAGENTA "\033[1m\033[35m"      /* Bold Magenta */
#define BOLDCYAN    "\033[1m\033[36m"      /* Bold Cyan */
#define BOLDWHITE   "\033[1m\033[37m"      /* Bold White */


#define PREDICT_POSE_THRESHOLD 0.5

#define Wa 0.4
#define Wb 0.3
#define Wc 0.3


static int gindex = 0;
iv::lidar_pose glidar_pose;

void * gpset;
void * gppose;



enum class MethodType
{
  PCL_GENERIC = 0,
  PCL_ANH = 1,
  PCL_ANH_GPU = 2,
  PCL_OPENMP = 3,
};
static MethodType _method_type = MethodType::PCL_GENERIC;

static pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose,
    ndt_pose, current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose;

static double offset_x, offset_y, offset_z, offset_yaw;  // current_pos - previous_pose
static double offset_imu_x, offset_imu_y, offset_imu_z, offset_imu_roll, offset_imu_pitch, offset_imu_yaw;
static double offset_odom_x, offset_odom_y, offset_odom_z, offset_odom_roll, offset_odom_pitch, offset_odom_yaw;
static double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch,
    offset_imu_odom_yaw;

// Can't load if typed "pcl::PointCloud<pcl::PointXYZRGB> map, add;"
static pcl::PointCloud<pcl::PointXYZ> map, add;

// If the map is loaded, map_loaded will be 1.
static int map_loaded = 0;
static int _use_gnss = 1;
static int init_pos_set = 0;

static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
//static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> anh_ndt;
#ifdef CUDA_FOUND
static std::shared_ptr<gpu::GNormalDistributionsTransform> anh_gpu_ndt_ptr =
    std::make_shared<gpu::GNormalDistributionsTransform>();
#endif
#ifdef USE_PCL_OPENMP
static pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> omp_ndt;
#endif

// Default values
static int max_iter = 30;        // Maximum iterations
static float ndt_res = 1.0;      // Resolution
static double step_size = 0.1;   // Step size
static double trans_eps = 0.01;  // Transformation epsilon

//static ros::Publisher predict_pose_pub;
//static geometry_msgs::PoseStamped predict_pose_msg;

//static ros::Publisher predict_pose_imu_pub;
//static geometry_msgs::PoseStamped predict_pose_imu_msg;

//static ros::Publisher predict_pose_odom_pub;
//static geometry_msgs::PoseStamped predict_pose_odom_msg;

//static ros::Publisher predict_pose_imu_odom_pub;
//static geometry_msgs::PoseStamped predict_pose_imu_odom_msg;

//static ros::Publisher ndt_pose_pub;
//static geometry_msgs::PoseStamped ndt_pose_msg;

// current_pose is published by vel_pose_mux
/*
static ros::Publisher current_pose_pub;
static geometry_msgs::PoseStamped current_pose_msg;
 */

//static ros::Publisher localizer_pose_pub;
//static geometry_msgs::PoseStamped localizer_pose_msg;

//static ros::Publisher estimate_twist_pub;
//static geometry_msgs::TwistStamped estimate_twist_msg;

//static ros::Duration scan_duration;

static double exe_time = 0.0;
static bool has_converged;
static int iteration = 0;
static double fitness_score = 0.0;
static double trans_probability = 0.0;

static double diff = 0.0;
static double diff_x = 0.0, diff_y = 0.0, diff_z = 0.0, diff_yaw;

static double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0;  // [m/s]
static double current_velocity_x = 0.0, previous_velocity_x = 0.0;
static double current_velocity_y = 0.0, previous_velocity_y = 0.0;
static double current_velocity_z = 0.0, previous_velocity_z = 0.0;
// static double current_velocity_yaw = 0.0, previous_velocity_yaw = 0.0;
static double current_velocity_smooth = 0.0;

static double current_velocity_imu_x = 0.0;
static double current_velocity_imu_y = 0.0;
static double current_velocity_imu_z = 0.0;

static double current_accel = 0.0, previous_accel = 0.0;  // [m/s^2]
static double current_accel_x = 0.0;
static double current_accel_y = 0.0;
static double current_accel_z = 0.0;
// static double current_accel_yaw = 0.0;

static double angular_velocity = 0.0;

static int use_predict_pose = 0;

//static ros::Publisher estimated_vel_mps_pub, estimated_vel_kmph_pub, estimated_vel_pub;
//static std_msgs::Float32 estimated_vel_mps, estimated_vel_kmph, previous_estimated_vel_kmph;

static std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;

//static ros::Publisher time_ndt_matching_pub;
//static std_msgs::Float32 time_ndt_matching;

static int _queue_size = 1000;

//static ros::Publisher ndt_stat_pub;
//static autoware_msgs::NDTStat ndt_stat_msg;

static double predict_pose_error = 0.0;

static double _tf_x, _tf_y, _tf_z, _tf_roll, _tf_pitch, _tf_yaw;
static Eigen::Matrix4f tf_btol;

static std::string _localizer = "velodyne";
static std::string _offset = "linear";  // linear, zero, quadratic

//static ros::Publisher ndt_reliability_pub;
//static std_msgs::Float32 ndt_reliability;

static bool _get_height = false;
static bool _use_local_transform = false;
static bool _use_imu = false;
static bool _use_odom = false;
static bool _imu_upside_down = false;
static bool _output_log_data = false;

static std::string _imu_topic = "/imu_raw";

static std::ofstream ofs;
static std::string filename;

//static sensor_msgs::Imu imu;
//static nav_msgs::Odometry odom;

// static tf::TransformListener local_transform_listener;
static tf::StampedTransform local_transform;

static unsigned int points_map_num = 0;

pthread_mutex_t mutex;

pthread_mutex_t mutex_read;

pthread_mutex_t mutex_pose;



pcl::PointCloud<pcl::PointXYZ>::Ptr local_map_ptr;
pcl::PointCloud<pcl::PointXYZ>::Ptr global_map_ptr;

pose glastmappose;

#include <QDateTime>

//cv::Mat gmatimage;
void ListenPoseSet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::lidar::ndtpos pos;

    if(false == pos.ParseFromArray(strdata,nSize))
    {
        std::cout<<" ndtpos parse error."<<std::endl;
        return;
    }

    SetCurPose(pos.pose_x(),pos.pose_y(),pos.pose_yaw(),pos.pose_z(),pos.pose_pitch(),pos.pose_roll());


}

static void SetLocalMap()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr local_ptr(new pcl::PointCloud<pcl::PointXYZ>());

    int nSize = global_map_ptr->size();

    int i;
    for(i=0;i<nSize;i++)
    {
        pcl::PointXYZ xp = global_map_ptr->at(i);
        if(sqrt(pow(xp.x - current_pose.x,2)+pow(xp.y-current_pose.y,2)+pow(xp.z-current_pose.z,2))<30)
        {
            local_ptr->push_back(xp);
        }
    }

    glastmappose = current_pose;

    local_map_ptr = local_ptr;
    std::cout<<"current map size is "<<local_map_ptr->size()<<std::endl;
}

static bool gbNDTRun = true;

static bool gbLMRun= true;
static std::thread * gpmapthread;
static bool gbNeedUpdate = false;
static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> glocal_ndt;
void LocalMapUpdate(int n)
{
    std::cout<<"LocalMap n is "<<n<<std::endl;

    while(gbLMRun)
    {

        if(sqrt(pow(glastmappose.x-current_pose.x,2)+pow(glastmappose.y-current_pose.y,2)+pow(glastmappose.z-current_pose.z,2))>5)
        {
            SetLocalMap();

//            pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_ndt;
//            pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//            new_ndt.setResolution(ndt_res);
//         //   new_ndt.setInputTarget(map_ptr);
//            new_ndt.setInputTarget(local_map_ptr);
//         //   new_ndt.setInputTarget(filtered_scan);
//            new_ndt.setMaximumIterations(max_iter);
//            new_ndt.setStepSize(step_size);
//            new_ndt.setTransformationEpsilon(trans_eps);

//            new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
//            pthread_mutex_lock(&mutex);
//            glocal_ndt = new_ndt;
//            pthread_mutex_unlock(&mutex);

            gbNeedUpdate = true;

        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}


void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
{
    _tf_x = 0;
    _tf_y = 0;
    _tf_z = 0;
    _tf_roll = 0;
    _tf_pitch = 0;
    _tf_yaw = 0;

    Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
    Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
    Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();



    init_pos_set = 1;

    initial_pose.x = 0;
    initial_pose.y = 0;
    initial_pose.z = 0;
    initial_pose.roll = 0;
    initial_pose.pitch = 0;
    initial_pose.yaw = 0;

    localizer_pose.x = initial_pose.x;
    localizer_pose.y = initial_pose.y;
    localizer_pose.z = initial_pose.z;
    localizer_pose.roll = initial_pose.roll;
    localizer_pose.pitch = initial_pose.pitch;
    localizer_pose.yaw = initial_pose.yaw;

    previous_pose.x = initial_pose.x;
    previous_pose.y = initial_pose.y;
    previous_pose.z = initial_pose.z;
    previous_pose.roll = initial_pose.roll;
    previous_pose.pitch = initial_pose.pitch;
    previous_pose.yaw = initial_pose.yaw;

    current_pose.x = initial_pose.x;
    current_pose.y = initial_pose.y;
    current_pose.z = initial_pose.z;
    current_pose.roll = initial_pose.roll;
    current_pose.pitch = initial_pose.pitch;
    current_pose.yaw = initial_pose.yaw;

    current_velocity = 0;
    current_velocity_x = 0;
    current_velocity_y = 0;
    current_velocity_z = 0;
    angular_velocity = 0;


//    ndt.setResolution(ndt_res);
//    ndt.setStepSize(step_size);
//    ndt.setTransformationEpsilon(trans_eps);
//    ndt.setMaximumIterations(max_iter);

  //  anh_gpu_ndt_ptr->setResolution();

    anh_gpu_ndt_ptr->setResolution(ndt_res);
    anh_gpu_ndt_ptr->setStepSize(step_size);
    anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
    anh_gpu_ndt_ptr->setMaximumIterations(max_iter);


//    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());


    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));

//    int i;
//    for(i=0;i<map.size();i++)
//    {
//        pcl::PointXYZ px;
//        pcl::PointXYZI pxi;
//        pxi = mappcd->at(i);
//        px.x = pxi.x;
//        px.y = pxi.y;
//        px.z = pxi.z;
//        map_ptr->push_back(px);
//        ++map_ptr->width;
////        px.x = mappcd->at(i).x;
//    }
    std::cout<<"map size is"<<mappcd->size()<<std::endl;
    std::cout<<"ptr size is "<<map_ptr->size()<<std::endl;;

    global_map_ptr = map_ptr;

 //   SetLocalMap();


//    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());

//    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
//    voxel_grid_filter.setLeafSize(1.0,1.0,1.0);
//    voxel_grid_filter.setInputCloud(map_ptr);
//    voxel_grid_filter.filter(*filtered_scan);

//    std::cout<<"filter map  size is "<<filtered_scan->size()<<std::endl;;

 //   ndt.setInputTarget(map_ptr);
//    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());


    gpset = iv::modulecomm::RegisterRecv("ndtposeset",ListenPoseSet);
    gppose = iv::modulecomm::RegisterSend("ndtpose",10000,3);

    if(map_ptr->size() == 0)
    {
        gbNDTRun = false;
        return;
    }

    std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
        std::make_shared<gpu::GNormalDistributionsTransform>();
    new_anh_gpu_ndt_ptr->setResolution(ndt_res);
    new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
    new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
    new_anh_gpu_ndt_ptr->setStepSize(step_size);
    new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);

    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointXYZ dummy_point;
    dummy_scan_ptr->push_back(dummy_point);
    new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);

    new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());

    pthread_mutex_lock(&mutex);
    anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
    pthread_mutex_unlock(&mutex);



    if(map_ptr->size()>0)map_loaded = 1;

 //   gpmapthread = new std::thread(LocalMapUpdate,1);




}

void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
{
    if(mappcd->size() == 0 )return;

    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));


    std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
        std::make_shared<gpu::GNormalDistributionsTransform>();
    new_anh_gpu_ndt_ptr->setResolution(ndt_res);
    new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
    new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
    new_anh_gpu_ndt_ptr->setStepSize(step_size);
    new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);

    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointXYZ dummy_point;
    dummy_scan_ptr->push_back(dummy_point);
    new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);

    new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());

    pthread_mutex_lock(&mutex);
    anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
    pthread_mutex_unlock(&mutex);



    if(map_ptr->size()>0)map_loaded = 1;
}



static double wrapToPm(double a_num, const double a_max)
{
  if (a_num >= a_max)
  {
    a_num -= 2.0 * a_max;
  }
  return a_num;
}

static double wrapToPmPi(const double a_angle_rad)
{
  return wrapToPm(a_angle_rad, M_PI);
}

static double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
{
  double diff_rad = lhs_rad - rhs_rad;
  if (diff_rad >= M_PI)
    diff_rad = diff_rad - 2 * M_PI;
  else if (diff_rad < -M_PI)
    diff_rad = diff_rad + 2 * M_PI;
  return diff_rad;
}


static unsigned int g_seq_old = 0;

#include <QTime>

void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
{

    if(gbNDTRun == false)return;
    if(gbNeedUpdate)
    {
        pthread_mutex_lock(&mutex);
//        ndt = glocal_ndt;
        pthread_mutex_unlock(&mutex);
        gbNeedUpdate = false;
        std::cout<<BOLDRED<<"map size is "<<local_map_ptr->size()<<RESET<<std::endl;
    }

    if(map_loaded == 0)
    {
        std::cout<<BOLDRED<<" point_ndt not set map."<<RESET<<std::endl;
        return;
    }



    pthread_mutex_lock(&mutex_pose);

    QTime xTime;
    xTime.start();
//    std::cout<<"time is "<<xTime.elapsed()<<std::endl;
    double voxel_leaf_size = 2.0;
    double measurement_range = 200.0;

    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());

    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
    voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
    voxel_grid_filter.setInputCloud(raw_scan);
    voxel_grid_filter.filter(*filtered_scan);

    std::cout<<"voxed time is "<<xTime.elapsed()<<std::endl;

//    std::cout<<" seq is "<<raw_scan->header.seq<<std::endl;
  //  qDebug("seq = %d stamp = %d ",raw_scan->header.seq,raw_scan->header.stamp);
  //  std::cout<<"raw scan size is "<<raw_scan->size()<<"  filtered scan size is "<<filtered_scan->size()<<std::endl;

 //   return;
    matching_start = std::chrono::system_clock::now();

    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(*filtered_scan));

    int scan_points_num = filtered_scan_ptr->size();

    Eigen::Matrix4f t(Eigen::Matrix4f::Identity());   // base_link
    Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());  // localizer

    std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start,
        getFitnessScore_end;
    static double align_time, getFitnessScore_time = 0.0;

//    std::cout<<"run hear"<<std::endl;
    pthread_mutex_lock(&mutex);

//    if (_method_type == MethodType::PCL_GENERIC)
 //   ndt.setInputSource(filtered_scan_ptr);

    anh_gpu_ndt_ptr->setInputSource(filtered_scan_ptr);

    // Guess the initial gross estimation of the transformation
//    double diff_time = (current_scan_time - previous_scan_time).toSec();


    double diff_time = 0.0;

    if(g_seq_old != 0)
    {
        if((raw_scan->header.seq - g_seq_old)>0)
        {
            diff_time = raw_scan->header.seq - g_seq_old;
            diff_time = diff_time * 0.1;
        }
    }

    g_seq_old = raw_scan->header.seq;


//    std::cout<<" diff time is "<<diff_time<<std::endl;

//    diff_time = 0.0;

//    if (_offset == "linear")
//    {

      diff_time = 0.1;
      offset_x = current_velocity_x * diff_time;
      offset_y = current_velocity_y * diff_time;
      offset_z = current_velocity_z * diff_time;
      offset_yaw = angular_velocity * diff_time;
//    }
      predict_pose.x = previous_pose.x + offset_x;
      predict_pose.y = previous_pose.y + offset_y;
      predict_pose.z = previous_pose.z + offset_z;
      predict_pose.roll = previous_pose.roll;
      predict_pose.pitch = previous_pose.pitch;
      predict_pose.yaw = previous_pose.yaw + offset_yaw;

      pose predict_pose_for_ndt;
      predict_pose_for_ndt = predict_pose;

      Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
      Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
      Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
      Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
      Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;

      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);

      std::cout<<"before ndt time is "<<xTime.elapsed()<<std::endl;
//      std::cout<<"time is "<<xTime.elapsed()<<std::endl;
      align_start = std::chrono::system_clock::now();
      anh_gpu_ndt_ptr->align(init_guess);
      align_end = std::chrono::system_clock::now();

      std::cout<<"after ndt time is "<<xTime.elapsed()<<std::endl;

      has_converged = anh_gpu_ndt_ptr->hasConverged();

      t = anh_gpu_ndt_ptr->getFinalTransformation();
      iteration = anh_gpu_ndt_ptr->getFinalNumIteration();

      getFitnessScore_start = std::chrono::system_clock::now();
      fitness_score = anh_gpu_ndt_ptr->getFitnessScore();
      getFitnessScore_end = std::chrono::system_clock::now();

      trans_probability = anh_gpu_ndt_ptr->getTransformationProbability();

  //    std::cout<<" scoure is "<<fitness_score<<std::endl;

      std::cout<<" iter is "<<iteration<<std::endl;

      t2 = t * tf_btol.inverse();

      getFitnessScore_time =
          std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() /
          1000.0;

      pthread_mutex_unlock(&mutex);

      tf::Matrix3x3 mat_l;  // localizer
      mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
                     static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
                     static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));

      // Update localizer_pose
      localizer_pose.x = t(0, 3);
      localizer_pose.y = t(1, 3);
      localizer_pose.z = t(2, 3);
      mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);


//     std::cout<<" local pose x is "<<localizer_pose.x<<std::endl;

      tf::Matrix3x3 mat_b;  // base_link
      mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
                     static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
                     static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));

      // Update ndt_pose
      ndt_pose.x = t2(0, 3);
      ndt_pose.y = t2(1, 3);
      ndt_pose.z = t2(2, 3);

//      ndt_pose.x = localizer_pose.x;
//      ndt_pose.y = localizer_pose.y;
//      ndt_pose.z = localizer_pose.z;
      mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);


      // Calculate the difference between ndt_pose and predict_pose
      predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
                                (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
                                (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));


 //     std::cout<<" pose error is "<<predict_pose_error<<std::endl;
 //     std::cout<<"ndt pose x is "<<ndt_pose.x<<std::endl;
      if (predict_pose_error <= PREDICT_POSE_THRESHOLD)
      {
        use_predict_pose = 0;
      }
      else
      {
        use_predict_pose = 1;
      }
      use_predict_pose = 0;

      if (use_predict_pose == 0)
      {
        current_pose.x = ndt_pose.x;
        current_pose.y = ndt_pose.y;
        current_pose.z = ndt_pose.z;
        current_pose.roll = ndt_pose.roll;
        current_pose.pitch = ndt_pose.pitch;
        current_pose.yaw = ndt_pose.yaw;
      }
      else
      {
        current_pose.x = predict_pose_for_ndt.x;
        current_pose.y = predict_pose_for_ndt.y;
        current_pose.z = predict_pose_for_ndt.z;
        current_pose.roll = predict_pose_for_ndt.roll;
        current_pose.pitch = predict_pose_for_ndt.pitch;
        current_pose.yaw = predict_pose_for_ndt.yaw;
      }

 //     std::cout<<" current pose x is "<<current_pose.x<<std::endl;

      // Compute the velocity and acceleration
      diff_x = current_pose.x - previous_pose.x;
      diff_y = current_pose.y - previous_pose.y;
      diff_z = current_pose.z - previous_pose.z;
      diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
      diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);

      current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
      current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
      current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
      current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
      angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;

      current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
      if (current_velocity_smooth < 0.2)
      {
        current_velocity_smooth = 0.0;
      }

      current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
      current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
      current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
      current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;

 //   std::cout<<"fit time is "<<getFitnessScore_time<<std::endl;

    std::cout<<" x is "<<current_pose.x<<" y is "<<current_pose.y<<" yaw is"<<current_pose.yaw<<std::endl;


//    gw->Updatexyyaw(current_pose.x,current_pose.y,current_pose.yaw);

    std::cout<<"vel is "<<current_velocity<<std::endl;
//    std::cout<<" vx is "<<current_velocity_x<<" vy is "<<current_velocity_y<<std::endl;
    previous_pose.x = current_pose.x;
    previous_pose.y = current_pose.y;
    previous_pose.z = current_pose.z;
    previous_pose.roll = current_pose.roll;
    previous_pose.pitch = current_pose.pitch;
    previous_pose.yaw = current_pose.yaw;

//    previous_scan_time = current_scan_time;

    previous_previous_velocity = previous_velocity;
    previous_velocity = current_velocity;
    previous_velocity_x = current_velocity_x;
    previous_velocity_y = current_velocity_y;
    previous_velocity_z = current_velocity_z;
    previous_accel = current_accel;

    pthread_mutex_lock(&mutex_read);
    gindex++;
    glidar_pose.accel = current_accel;
    glidar_pose.accel_x = current_accel_x;
    glidar_pose.accel_y = current_accel_y;
    glidar_pose.accel_z = current_accel_z;
    glidar_pose.vel = current_velocity;
    glidar_pose.vel_x = current_velocity_x;
    glidar_pose.vel_y = current_velocity_y;
    glidar_pose.vel_z = current_velocity_z;
    glidar_pose.mpose = current_pose;
    pthread_mutex_unlock(&mutex_read);

    pthread_mutex_unlock(&mutex_pose);

    iv::lidar::ndtpos ndtpos;
    ndtpos.set_lidartime(raw_scan->header.stamp/1000);
    ndtpos.set_ndt_time(QDateTime::currentMSecsSinceEpoch());
    ndtpos.set_accel(current_accel);
    ndtpos.set_accel_x(current_accel_x);
    ndtpos.set_accel_y(current_accel_y);
    ndtpos.set_accel_z(current_accel_z);
    ndtpos.set_vel(current_velocity);
    ndtpos.set_vel_x(current_velocity_x);
    ndtpos.set_vel_y(current_velocity_y);
    ndtpos.set_vel_z(current_velocity_z);
    ndtpos.set_fitness_score(fitness_score);
    ndtpos.set_has_converged(has_converged);
    ndtpos.set_id(0);
    ndtpos.set_iteration(iteration);
    ndtpos.set_trans_probability(trans_probability);
    ndtpos.set_pose_pitch(current_pose.pitch);
    ndtpos.set_pose_roll(current_pose.roll);
    ndtpos.set_pose_yaw(current_pose.yaw);
    ndtpos.set_pose_x(current_pose.x);
    ndtpos.set_pose_y(current_pose.y);
    ndtpos.set_pose_z(current_pose.z);
    ndtpos.set_comp_time(xTime.elapsed());

    int ndatasize = ndtpos.ByteSize();
    char * strser = new char[ndatasize];
    bool bSer = ndtpos.SerializeToArray(strser,ndatasize);
    if(bSer)
    {
        iv::modulecomm::ModuleSendMsg(gppose,strser,ndatasize);
    }
    else
    {
        std::cout<<BOLDRED<<"ndtpos serialize error."<<RESET<<std::endl;
    }
    delete strser;

}


int GetPose(int & curindex,iv::lidar_pose & xlidar_pose)
{
     pthread_mutex_lock(&mutex_read);
    if(curindex == gindex)
    {
        pthread_mutex_unlock(&mutex_read);
        return 0;
    }
    curindex = gindex;
    xlidar_pose = glidar_pose;
    pthread_mutex_unlock(&mutex_read);
    return 1;
}

void SetCurPose(double x0, double y0, double yaw0, double z0, double pitch0, double roll0)
{

    std::cout<<"set pose"<<std::endl;
    pthread_mutex_lock(&mutex_pose);

    initial_pose.x = x0;
    initial_pose.y = y0;
    initial_pose.z = z0;
    initial_pose.roll = roll0;
    initial_pose.pitch = pitch0;
    initial_pose.yaw = yaw0;

    localizer_pose.x = initial_pose.x;
    localizer_pose.y = initial_pose.y;
    localizer_pose.z = initial_pose.z;
    localizer_pose.roll = initial_pose.roll;
    localizer_pose.pitch = initial_pose.pitch;
    localizer_pose.yaw = initial_pose.yaw;

    previous_pose.x = initial_pose.x;
    previous_pose.y = initial_pose.y;
    previous_pose.z = initial_pose.z;
    previous_pose.roll = initial_pose.roll;
    previous_pose.pitch = initial_pose.pitch;
    previous_pose.yaw = initial_pose.yaw;

    current_pose.x = initial_pose.x;
    current_pose.y = initial_pose.y;
    current_pose.z = initial_pose.z;
    current_pose.roll = initial_pose.roll;
    current_pose.pitch = initial_pose.pitch;
    current_pose.yaw = initial_pose.yaw;

    current_velocity = 0;
    current_velocity_x = 0;
    current_velocity_y = 0;
    current_velocity_z = 0;
    angular_velocity = 0;

    pthread_mutex_unlock(&mutex_pose);


}

void SetRunState(bool bRun)
{
    std::cout<<"set state."<<std::endl;
    gbNDTRun = bRun;
}