/*
 * 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 <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 <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/voxel_grid.h>
//#include <pcl/visualization/pcl_visualizer.h>

#include <QFile>

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

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

#include <ndt_cpu/NormalDistributionsTransform.h>

#include "ndt_matching.h"

#include "modulecomm.h"



#include "gnss_coordinate_convert.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

#include "ivfault.h"
#include "ivlog.h"

extern iv::Ivfault *gfault ;
extern iv::Ivlog *givlog;

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

void * gpset;
void * gppose;

static bool g_hasmap = false;

extern bool gbNeedReloc;

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;

bool gbuseanh = true;

static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> anh_ndt;
static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> glocalanh_ndt;

static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr glocalndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());

// 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 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 std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;


static int _queue_size = 1000;


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

bool gbNeedGPSUpdateNDT = false;

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

pose glastmappose;

static double imu_angular_velocity_x=0;
static double imu_angular_velocity_y=0;
static double imu_angular_velocity_z=0;
static double imu_linear_acceleration_x=0;
static double imu_linear_acceleration_y=0;
static double imu_linear_acceleration_z =0;

extern QFile * gpFileLastPos;//Save Last Positin
static bool gbFileNDTUpdate;

extern double garm_x ;
extern double garm_y ;

static int gndtcalcfailcount = 0;
static pose gPoseReloc;
static bool gbPoseReloc = false;

#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 gbGFRun = true;
static bool gbLMRun= true;
static std::thread * gpmapthread, * gpgpsfixthread,*gpsavelastthread;
static bool gbNeedUpdate = false;

extern iv::gpspos glastndtgpspos;
extern iv::gpspos gcurndtgpspos;
extern iv::gpspos gcurgpsgpspos;
extern bool gbGPSFix;
extern int64_t gnLastGPSTime;

static bool gbgpsupdatendt = false;

static int gusemapindex = -1;
static int gcurmapindex = -1;

extern std::vector<iv::ndtmaptrace> gvector_trace;

extern void * gpa,* gpb ,* gpc, * gpd;

iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose)
{
    double x_o,y_o;
    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
    double lon,lat;
    double hdg_o = (90 - xori.heading)*M_PI/180.0;
    double rel_x = xpose.x * cos(hdg_o) - xpose.y * sin(hdg_o);
    double rel_y = xpose.x * sin(hdg_o) + xpose.y * cos(hdg_o);
    GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
    double hdg_c = hdg_o + xpose.yaw;

    hdg_c = M_PI/2.0 - hdg_c;
    double heading = hdg_c * 180.0/M_PI;
    while(heading < 0)heading = heading + 360;
    while(heading >360)heading = heading - 360;
    iv::gpspos xgpspos;
    xgpspos.lon = lon;
    xgpspos.lat = lat;
    xgpspos.height = xpose.z + xori.height;
    xgpspos.heading = heading;
    xgpspos.pitch = xpose.pitch*180.0/M_PI  + xori.pitch;
    xgpspos.roll  = xpose.roll*180.0/M_PI + xori.roll;
    xgpspos.ve = xpose.vx * cos(hdg_o) - xpose.vy * sin(hdg_o);
    xgpspos.vn = xpose.vx * sin(hdg_o) + xpose.vy * cos(hdg_o);


//    std::cout<< " ori heading :  "<<xori.heading<<std::endl;

//    std::cout<<" pose yaw: "<<xpose.yaw<<std::endl;

    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
    {
        GaussProjCal(xgpspos.lon,xgpspos.lat,&x_o,&y_o);
        hdg_o = (90 - xgpspos.heading)*M_PI/180.0;
        rel_x = garm_x *(-1) * cos(hdg_o) - garm_y *(-1) * sin(hdg_o);
        rel_y = garm_x *(-1) * sin(hdg_o) + garm_y *(-1) * cos(hdg_o);
        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
        xgpspos.lon = lon;
        xgpspos.lat = lat;
    }
    return xgpspos;

}

pose CalcPose(iv::gpspos xori, iv::gpspos xcur)
{
    double lon,lat;
    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
    {
        double x_o,y_o;
        GaussProjCal(xcur.lon,xcur.lat,&x_o,&y_o);
        double hdg_o = (90 - xcur.heading)*M_PI/180.0;
        double rel_x = garm_x *(1) * cos(hdg_o) - garm_y *(1) * sin(hdg_o);
        double rel_y = garm_x *(1) * sin(hdg_o) + garm_y *(1) * cos(hdg_o);
        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
        xcur.lon = lon;
        xcur.lat = lat;
    }
    double x_o,y_o,hdg_o;
    double x_c,y_c,hdg_c;
    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
    GaussProjCal(xcur.lon,xcur.lat,&x_c,&y_c);
    double dis = sqrt(pow(x_c-x_o,2)+ pow(y_c-y_o,2));
    double rel_x0,rel_y0;
    rel_x0 = x_c -x_o;
    rel_y0 = y_c -y_o;
    double rel_x,rel_y;

    hdg_o = (90 - xori.heading)*M_PI/180.0;
    hdg_c = (90 - xcur.heading)*M_PI/180.0;
    rel_x = rel_x0 * cos(-hdg_o) - rel_y0 * sin(-hdg_o);
    rel_y = rel_x0 * sin(-hdg_o) + rel_y0 * cos(-hdg_o);


    double rel_hdg;
    rel_hdg = hdg_c - hdg_o;
    pose posex;
    posex.x = rel_x;
    posex.y = rel_y;
    posex.z = xcur.height - xori.height;
    posex.pitch = (xcur.pitch - xori.pitch)*M_PI/180.0;
    posex.roll = (xcur.roll - xori.roll)*M_PI/180.0;
    posex.yaw = rel_hdg;

    posex.vx = xcur.ve * cos(-hdg_o) - xcur.vn * sin(-hdg_o);
    posex.vy = xcur.ve * sin(-hdg_o) + xcur.vn * cos(-hdg_o);

    return posex;

}

static double gettracedis(int index,pose posex)
{
    double fdismin = 1000000.;
    double zdiff = 0;
    int neari;
    if(index < 0)return 1000000.0;
    if(index>= gvector_trace.size())
    {
        return 1000000.0;
    }
    int i;
    std::vector<iv::ndttracepoint> vt = gvector_trace.at(index).mvector_trace;
    int nsize = vt.size();
//    std::cout<<"size is "<<nsize<<std::endl;
    for(i=0;i<nsize;i++)
    {
        double fdis = sqrt(pow(posex.x - vt.at(i).x,2) + pow(posex.y - vt.at(i).y,2));
//        std::cout<<"fdis is "<<fdis<<std::endl;
        if((fdis < fdismin) &&(fabs(posex.z - vt.at(i).z)<5))
        {
            fdismin = fdis;
            neari = i;
            zdiff = fabs(posex.z - vt.at(i).z);
        }
    }

    return fdismin;
}

static void getmindistrace(int & index, double & ftracedis)
{
    double fdismin = 1000000.0;
    int xindexmin = -1;
    int i;
    int nsize = gvector_trace.size();


    for(i=0;i<nsize;i++)
    {
        pose posex = CalcPose(gvector_trace[i].mndtorigin,gcurndtgpspos);
        double fdis = gettracedis(i,posex);
        if(fdis<fdismin)
        {
            fdismin = fdis;
            xindexmin = i;
        }
    }

    if(xindexmin != -1)
    {
        ftracedis = fdismin;
        index = xindexmin;
    }
    else
    {
        index = -1;
        ftracedis = 100000.0;
    }
}

#include <QTime>

extern void pausecomm();
extern void continuecomm();
static void UseMap(int index)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr xmap;

    xmap = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);


    QTime xTime;
    xTime.start();
    if(0 == pcl::io::loadPCDFile(gvector_trace.at(index).mstrpcdpath.data(),*xmap))
    {

    }
    else
    {
        std::cout<<"map size is 0"<<std::endl;
        return;
    }

    qDebug("load map time is %d",xTime.elapsed());

    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*xmap));
//    gvectoranh.push_back(new_anh_gpu_ndt_ptr);

    qDebug("ndt load finish time is %d",xTime.elapsed());

    gcurmapindex = index;


    if(gbuseanh)
    {
        cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> localanhraw;

        localanhraw.setResolution(ndt_res);
        localanhraw.setInputTarget(map_ptr);
        localanhraw.setMaximumIterations(max_iter);
        localanhraw.setStepSize(step_size);
        localanhraw.setTransformationEpsilon(trans_eps);

        pthread_mutex_lock(&mutex);
        //        ndt = glocal_ndt;
        glocalanh_ndt = localanhraw;
        pthread_mutex_unlock(&mutex);

        gbNeedUpdate = true;
    }
    else
    {

        pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());

        localndtraw->setResolution(ndt_res);
        localndtraw->setInputTarget(map_ptr);
        localndtraw->setMaximumIterations(max_iter);
        localndtraw->setStepSize(step_size);
        localndtraw->setTransformationEpsilon(trans_eps);

        pthread_mutex_lock(&mutex);
        //        ndt = glocal_ndt;
        glocalndtraw = localndtraw;
        pthread_mutex_unlock(&mutex);

        gbNeedUpdate = true;
    }


    std::cout<<"change map, index is "<<index<<std::endl;
}

void LocalMapUpdate(int n)
{
    std::cout<<"LocalMap n is "<<n<<std::endl;

    int index;
    double ftracedis;
    int ncurindex = -1;

    int i;
    for(i=0;i<gvector_trace.size();i++)
    {
//        UseMap(i);
//        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    while(gbLMRun)
    {

        getmindistrace(index,ftracedis);

        if(g_hasmap == false)
        {

            if(index >= 0)
            {
                if(ftracedis < 30)
                {
                    UseMap(index);
                    ncurindex = index;
                    g_hasmap = true;
                }
            }
        }
        else
        {
           if(index != ncurindex)
           {
                pose posex = CalcPose(gvector_trace[ncurindex].mndtorigin,gcurndtgpspos);
               double fnowdis = gettracedis(ncurindex,posex);
               if((fnowdis - ftracedis)>5)
               {
                   UseMap(index);
                   ncurindex = index;
                   g_hasmap = true;
               }
           }
        }

        if(ftracedis > 50)
        {
            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" trace dis is "<<ftracedis<<std::endl;
            std::cout<<" Out Range."<<std::endl;
            g_hasmap = false;
        }


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


void SaveMonitor(bool * pbRun)
{
    iv::gpspos xoldgpspos;
    xoldgpspos.lat = 39;
    xoldgpspos.lon = 117;
    while(*pbRun)
    {
       if(gpFileLastPos != 0)
       {
           if(gbFileNDTUpdate)
           {
               if((fabs(xoldgpspos.lat - gcurndtgpspos.lat)>0.0000005)||((fabs(xoldgpspos.lon - gcurndtgpspos.lon)>0.0000005)))
               {
                   xoldgpspos = gcurndtgpspos;
                   char str[1000];
                   snprintf(str,1000,"%11.7f\t%11.7f\t%9.3f\t%6.2f\t%6.2f\t%6.2f\n                ",
                            xoldgpspos.lon,xoldgpspos.lat,xoldgpspos.height,
                            xoldgpspos.heading,xoldgpspos.pitch,xoldgpspos.roll);
                   gpFileLastPos->seek(0);
                   gpFileLastPos->write(str,strnlen(str,1000));
                   gpFileLastPos->flush();
               }
               gbFileNDTUpdate = false;
           }
       }
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

void GPSPosMonitor(bool * pbRun)
{
    if(gbGPSFix == false)
    {
        gcurndtgpspos =  glastndtgpspos;
    }
    while(*pbRun)
    {
        int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
        if(abs(nmsnow - gnLastGPSTime)>1000)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            continue;
        }
        if(gbGPSFix == true)
        {
            double x0,y0;
            double x,y;
            GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
            GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
            double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
            double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
            double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
            if((dis> 10)||((headdiff>10)&&(headdiff<350))||(zdiff>5)||(g_hasmap == false)||(gbNeedGPSUpdateNDT))
            {
                givlog->info("NDTFIX","gps fix ndt pos. dis:%f headdiff:%f zdiff:%f map %d ndt %d",
                             dis,headdiff,zdiff,g_hasmap,gbNeedGPSUpdateNDT);
                std::cout<<"gps fix ndt pos "<<" dis: "<<dis<<" headdiff: "<<headdiff<<" zdiff: "<<zdiff<<std::endl;
//                gcurndtgpspos = gcurgpsgpspos;
                  gbgpsupdatendt = true;
//                current_velocity_x = 0;
//                current_velocity_y = 0;
//                current_velocity_z = 0;
//                angular_velocity = 0;
//                gbNeedGPSUpdateNDT = false;
                if(g_hasmap == true)
                {
                    int index = gcurmapindex;
                    if((index >=0)&&(index < gvector_trace.size()))
                    {

                    }
                }
            }
//            else
//            {
//                if(gbgpsupdatendt)
//                {
//                    gcurndtgpspos = gcurgpsgpspos;
//                    gbgpsupdatendt = true;
//                    current_velocity_x = 0;
//                    current_velocity_y = 0;
//                    current_velocity_z = 0;
//                    angular_velocity = 0;
//                }
//            }

        }

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


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.align(*output_cloud, Eigen::Matrix4f::Identity());


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


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



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





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

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




}

void ndt_match_Init_nomap()
{
    _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;


    gpmapthread = new std::thread(LocalMapUpdate,1);
    gbGFRun = true;
    gpgpsfixthread = new std::thread(GPSPosMonitor,&gbGFRun);
    gpsavelastthread = new std::thread(SaveMonitor,&gbGFRun);




}


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




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







    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>



bool isfirst = true;

#include <QMutex>
std::vector<iv::imudata> gvectorimu;
QMutex gMuteximu;

static void lidar_imu_calc(qint64 current_lidar_time)
{
    int nsize;

    int i;
    gMuteximu.lock();
    nsize = gvectorimu.size();
    for(i=0;i<nsize;i++)
    {
        iv::imudata ximudata = gvectorimu[i];
        double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
        double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
        if(current_lidar_time >  ximudata.imutime)
        {
            ximu_angular_velocity_x = ximudata.imu_angular_velocity_x;
            ximu_angular_velocity_y = ximudata.imu_angular_velocity_y;
            ximu_angular_velocity_z = ximudata.imu_angular_velocity_z;
            ximu_linear_acceleration_x = ximudata.imu_linear_acceleration_x;
            ximu_linear_acceleration_y = ximudata.imu_linear_acceleration_y;
            ximu_linear_acceleration_z = ximudata.imu_linear_acceleration_z;
            qint64 current_time_imu = ximudata.imutime;
//            givlog->verbose("NDT","imu time is %ld",current_time_imu);
            static qint64 previous_time_imu = current_time_imu;
            double diff_time = (current_time_imu - previous_time_imu);
            diff_time = diff_time/1000.0;

            if(diff_time < 0)diff_time = 0.000001;
            if(diff_time > 0.1)diff_time = 0.1;

            if(current_time_imu < previous_time_imu)
            {

                std::cout<<"current time is old "<<current_time_imu<<std::endl;
                previous_time_imu = current_time_imu;
                continue;
            }

            double diff_imu_roll = ximu_angular_velocity_x * diff_time;
            double diff_imu_pitch = ximu_angular_velocity_y * diff_time;
            double diff_imu_yaw = ximu_angular_velocity_z * diff_time;

            current_pose_imu.roll += diff_imu_roll;
            current_pose_imu.pitch += diff_imu_pitch;
            current_pose_imu.yaw += diff_imu_yaw;

            double accX1 = ximu_linear_acceleration_x;
            double accY1 = std::cos(current_pose_imu.roll) * ximu_linear_acceleration_y -
                           std::sin(current_pose_imu.roll) * ximu_linear_acceleration_z;
            double accZ1 = std::sin(current_pose_imu.roll) * ximu_linear_acceleration_y +
                           std::cos(current_pose_imu.roll) * ximu_linear_acceleration_z;

            double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
            double accY2 = accY1;
            double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;

            double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
            double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
            double accZ = accZ2;

            offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
            offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
            offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;

            current_velocity_imu_x += accX * diff_time;
            current_velocity_imu_y += accY * diff_time;
            current_velocity_imu_z += accZ * diff_time;

            offset_imu_roll += diff_imu_roll;
            offset_imu_pitch += diff_imu_pitch;
            offset_imu_yaw += diff_imu_yaw;

          //  guess_pose_imu.x = previous_pose.x + offset_imu_x;
          //  guess_pose_imu.y = previous_pose.y + offset_imu_y;
          //  guess_pose_imu.z = previous_pose.z + offset_imu_z;
          //  guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
          //  guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
          //  guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;

            predict_pose_imu.x = previous_pose.x + offset_imu_x;
            predict_pose_imu.y = previous_pose.y + offset_imu_y;
            predict_pose_imu.z = previous_pose.z + offset_imu_z;
            predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
            predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
            predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;

            givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
                            offset_imu_y,offset_imu_z,offset_imu_yaw);

            previous_time_imu = current_time_imu;


        }
        else
        {
            break;;
        }

    }

    if(i>0)
    {
        gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin()+i);
    }

    gMuteximu.unlock();

//    for(i=0;i<gvectorimu.size();i++)
//    {
//        iv::imudata ximudata = gvectorimu[i];
//        double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
//        double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
//        if(current_lidar_time >  ximudata.imutime)
//        {

//            gvectorimu.erase(gvectorimu.begin())
//        }
//    }
}


static void imu_calc(qint64 current_time_imu)
{

  static qint64 previous_time_imu = current_time_imu;
  double diff_time = (current_time_imu - previous_time_imu);
  diff_time = diff_time/1000.0;

  if(current_time_imu < previous_time_imu)
  {
      std::cout<<"current time is old "<<current_time_imu<<std::endl;
      return;
  }

  double diff_imu_roll = imu_angular_velocity_x * diff_time;
  double diff_imu_pitch = imu_angular_velocity_y * diff_time;
  double diff_imu_yaw = imu_angular_velocity_z * diff_time;

  current_pose_imu.roll += diff_imu_roll;
  current_pose_imu.pitch += diff_imu_pitch;
  current_pose_imu.yaw += diff_imu_yaw;

  double accX1 = imu_linear_acceleration_x;
  double accY1 = std::cos(current_pose_imu.roll) * imu_linear_acceleration_y -
                 std::sin(current_pose_imu.roll) * imu_linear_acceleration_z;
  double accZ1 = std::sin(current_pose_imu.roll) * imu_linear_acceleration_y +
                 std::cos(current_pose_imu.roll) * imu_linear_acceleration_z;

  double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
  double accY2 = accY1;
  double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;

  double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
  double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
  double accZ = accZ2;

  offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
  offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
  offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;

  current_velocity_imu_x += accX * diff_time;
  current_velocity_imu_y += accY * diff_time;
  current_velocity_imu_z += accZ * diff_time;

  offset_imu_roll += diff_imu_roll;
  offset_imu_pitch += diff_imu_pitch;
  offset_imu_yaw += diff_imu_yaw;

//  guess_pose_imu.x = previous_pose.x + offset_imu_x;
//  guess_pose_imu.y = previous_pose.y + offset_imu_y;
//  guess_pose_imu.z = previous_pose.z + offset_imu_z;
//  guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
//  guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
//  guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;

  predict_pose_imu.x = previous_pose.x + offset_imu_x;
  predict_pose_imu.y = previous_pose.y + offset_imu_y;
  predict_pose_imu.z = previous_pose.z + offset_imu_z;
  predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
  predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
  predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;

  givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
                  offset_imu_y,offset_imu_z,offset_imu_yaw);

  previous_time_imu = current_time_imu;
}


void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double imu_yaw,
                       double acceleration_x,double acceleration_y,
                       double acceleration_z)
{
  // std::cout << __func__ << std::endl;

// double imu_angular_velocity_x,imu_angular_velocity_y,imu_angular_velocity_z;

  static double previous_time_imu = current_time_imu;
  double diff_time = (current_time_imu - previous_time_imu);
  diff_time = diff_time/1000.0;


  imu_roll = imu_roll * M_PI/180.0;
  imu_pitch = imu_pitch * M_PI/180.0;
  imu_yaw = (-1.0)*imu_yaw * M_PI/180.0;

  imu_yaw = imu_yaw + 2.0*M_PI;

//  imu_pitch = 0;
//  imu_roll = 0;

  imu_roll = wrapToPmPi(imu_roll);
  imu_pitch = wrapToPmPi(imu_pitch);
  imu_yaw = wrapToPmPi(imu_yaw);

  static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;
  const double diff_imu_roll = imu_roll - previous_imu_roll;

  const double diff_imu_pitch = imu_pitch - previous_imu_pitch;

  double diff_imu_yaw;
  if (fabs(imu_yaw - previous_imu_yaw) > M_PI)
  {
    if (imu_yaw > 0)
      diff_imu_yaw = (imu_yaw - previous_imu_yaw) - M_PI * 2;
    else
      diff_imu_yaw = -M_PI * 2 - (imu_yaw - previous_imu_yaw);
  }
  else
    diff_imu_yaw = imu_yaw - previous_imu_yaw;


  imu_linear_acceleration_x = acceleration_x;
//  imu_linear_acceleration_y = acceleration_y;
//  imu_linear_acceleration_z = acceleration_z;
  imu_linear_acceleration_y = 0;
  imu_linear_acceleration_z = 0;

  if (diff_time != 0)
  {
    imu_angular_velocity_x = diff_imu_roll / diff_time;
    imu_angular_velocity_y = diff_imu_pitch / diff_time;
    imu_angular_velocity_z = diff_imu_yaw / diff_time;
  }
  else
  {
    imu_angular_velocity_x = 0;
    imu_angular_velocity_y = 0;
    imu_angular_velocity_z = 0;
  }

  iv::imudata ximudata;
  ximudata.imutime = current_time_imu;
  ximudata.imu_linear_acceleration_x = imu_linear_acceleration_x;
  ximudata.imu_linear_acceleration_y = imu_linear_acceleration_y;
  ximudata.imu_linear_acceleration_z = imu_linear_acceleration_z;
  ximudata.imu_angular_velocity_x = imu_angular_velocity_x;
  ximudata.imu_angular_velocity_y = imu_angular_velocity_y;
  ximudata.imu_angular_velocity_z = imu_angular_velocity_z;

  gMuteximu.lock();
  gvectorimu.push_back(ximudata);
  gMuteximu.unlock();
//  imu_calc(current_time_imu);

  previous_time_imu = current_time_imu;
  previous_imu_roll = imu_roll;
  previous_imu_pitch = imu_pitch;
  previous_imu_yaw = imu_yaw;
}


void restartndtfailcount()
{
    gndtcalcfailcount = 0;
}

void setrelocpose(pose xPose)
{
    gPoseReloc = xPose;
    gbPoseReloc = true;
}


#ifdef TEST_CALCTIME
int ncalc = 0;
int gncalctotal = 0;
#endif
void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
{

    static bool bNeedUpdateVel = false; //if gps update ndt, need update velocity
    qint64 current_scan_time = raw_scan->header.stamp;
    static qint64 old_scan_time = current_scan_time;
    if(gbNDTRun == false)return;

    bool bNotChangev = false;



    if(gbgpsupdatendt == true)
    {

        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" update ndt pos use gps pos. "<<std::endl;
        gcurndtgpspos = gcurgpsgpspos;
//        gcurndtgpspos.pitch = 0; //not use gps pitch and roll
//        gcurndtgpspos.roll = 0;
        gbgpsupdatendt = false;
        gbNeedGPSUpdateNDT = false;
        current_velocity_x = 0;
        current_velocity_y = 0;
        current_velocity_z = 0;
        angular_velocity = 0;
        bNeedUpdateVel = true;

//
//
//        gcurndtgpspos = gcurgpsgpspos;
//        current_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
//       predict_pose_for_ndt = current_pose;
        current_velocity_imu_x = 0;
        current_velocity_imu_y = 0;
        current_velocity_imu_z = 0;
        gMuteximu.lock();
        gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin() + gvectorimu.size());
        gMuteximu.unlock();
        bNotChangev = true;
        return;
    }
//    gbgpsupdatendt = false;

    if(g_hasmap == false)
    {
        return;
    }
    if(gbNeedUpdate)
    {
        if(gbuseanh)
        {
            pthread_mutex_lock(&mutex);
            anh_ndt = glocalanh_ndt;
            pthread_mutex_unlock(&mutex);
        }
        else
        {
            pthread_mutex_lock(&mutex);
            ndtraw = glocalndtraw;
            pthread_mutex_unlock(&mutex);
        }

        gusemapindex = gcurmapindex;
        gbNeedUpdate = false;

    }





    previous_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);

    if(gbPoseReloc)
    {
        previous_pose = gPoseReloc;
        gbPoseReloc = false;
    }

 //   std::cout<<" previos head: "<<previous_pose.yaw<<std::endl;
    if(bNeedUpdateVel == true)
    {
        bNeedUpdateVel = false;
        current_velocity_x = previous_pose.vx;
        current_velocity_y = previous_pose.vy;
        current_velocity_imu_x = current_velocity_x;
        current_velocity_imu_y = current_velocity_y;
    }
    givlog->verbose("previos pose is %f %f",previous_pose.x,previous_pose.y);

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


    if(gbuseanh)
    {
#ifdef  TEST_CALCTIME
        std::cout<<"use anh."<<std::endl;
#endif
        anh_ndt.setInputSource(filtered_scan_ptr);
    }
    else
    {
#ifdef  TEST_CALCTIME
        std::cout<<"use ndt."<<std::endl;
#endif
        ndtraw->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;

    diff_time = current_scan_time -old_scan_time;
    diff_time = diff_time/1000.0;
    old_scan_time = current_scan_time;

    if(diff_time<=0)diff_time = 0.1;
    if(diff_time>1.0)diff_time = 0.1;

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

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

//    diff_time = 0.0;

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

      if(diff_time<0.1)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;


      givlog->verbose("NDT","previous x:%f y:%f z:%f yaw:%f",previous_pose.x,
                      previous_pose.y,previous_pose.z,previous_pose.yaw);
//      if (_use_imu == true && _use_odom == true)
//         imu_odom_calc(current_scan_time);
       if (_use_imu == true && _use_odom == false)
       {
         lidar_imu_calc((current_scan_time-0));
       }
//       if (_use_imu == false && _use_odom == true)
//         odom_calc(current_scan_time);

//       if (_use_imu == true && _use_odom == true)
//         predict_pose_for_ndt = predict_pose_imu_odom;
//       else
//           if (_use_imu == true && _use_odom == false)
//         predict_pose_for_ndt = predict_pose_imu;
//       else if (_use_imu == false && _use_odom == true)
//         predict_pose_for_ndt = predict_pose_odom;
//       else
//         predict_pose_for_ndt = predict_pose;

      if (_use_imu == true && _use_odom == false)
      {
         predict_pose_for_ndt = predict_pose_imu;
 //         predict_pose_for_ndt = predict_pose;
 //         predict_pose_for_ndt.yaw = predict_pose_imu.yaw;
      }
      else
      {
          predict_pose_for_ndt = predict_pose;
      }

      predict_pose_for_ndt = predict_pose; //not use predict_pose_imu in shelan.

      if(fabs(predict_pose_for_ndt.x -previous_pose.x)>10)
      {
          predict_pose_for_ndt = previous_pose;
      }

      if(fabs(predict_pose_for_ndt.y -previous_pose.y)>10)
      {
          predict_pose_for_ndt = previous_pose;
      }

 //     predict_pose_for_ndt = predict_pose;



//      predict_pose_for_ndt.pitch = 0;
//      predict_pose_for_ndt.roll = 0;
      givlog->verbose("NDT","pre x:%f y:%f z:%f yaw:%f",predict_pose_for_ndt.x,
                      predict_pose_for_ndt.y,predict_pose_for_ndt.z,predict_pose_for_ndt.yaw);
//      qDebug("pre x:%f y:%f z:%f yaw:%f pitch: %f roll: %f",predict_pose_for_ndt.x,
//             predict_pose_for_ndt.y,predict_pose_for_ndt.z,predict_pose_for_ndt.yaw,predict_pose_for_ndt.pitch,predict_pose_for_ndt.roll);
//      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;


  //    std::cout<<" gues pose yaw: "<<predict_pose_for_ndt.yaw<<std::endl;
      pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
      align_start = std::chrono::system_clock::now();
      if(gbuseanh)
      {
        anh_ndt.align(*aligned,init_guess);
      }
      else
        ndtraw->align(*aligned,init_guess);
      align_end = std::chrono::system_clock::now();

      if(xTime.elapsed()<90)
      std::cout<<GREEN<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
      else
          std::cout<<RED<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;


#ifdef TEST_CALCTIME

    gncalctotal = gncalctotal + xTime.elapsed();
    ncalc++;
    if(ncalc>0)
    {
        std::cout<<" average calc time: "<<gncalctotal/ncalc<<std::endl;

    }
#endif

      if(gbuseanh)
      {
          has_converged = anh_ndt.hasConverged();
          t = anh_ndt.getFinalTransformation();
          iteration = anh_ndt.getFinalNumIteration();
          getFitnessScore_start = std::chrono::system_clock::now();
          fitness_score = anh_ndt.getFitnessScore();
          getFitnessScore_end = std::chrono::system_clock::now();
          trans_probability = anh_ndt.getTransformationProbability();
      }
      else
      {
          has_converged = ndtraw->hasConverged();
          t = ndtraw->getFinalTransformation();
          iteration = ndtraw->getFinalNumIteration();
          getFitnessScore_start = std::chrono::system_clock::now();
          fitness_score = ndtraw->getFitnessScore();
          getFitnessScore_end = std::chrono::system_clock::now();
          trans_probability = ndtraw->getTransformationProbability();
      }


      if(trans_probability >= 1.0)
      {
          gndtcalcfailcount = 0;
      }
      else
      {
          gndtcalcfailcount++;
      }

      if(gndtcalcfailcount >= 30)
      {
          gbNeedReloc = true;
      }


      std::cout<<"score: "<<fitness_score<<" trans_probability:"<<trans_probability<<std::endl;
  //    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);


      givlog->verbose("NDT","calc x:%f y:%f z:%f yaw:%f",ndt_pose.x,
                      ndt_pose.y,ndt_pose.z,ndt_pose.yaw);


      // 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*10*diff_time))
      {
        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
      {
          givlog->verbose("NDT","USE PREDICT POSE");
        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_pose.vx = current_velocity_x;
      current_pose.vy = current_velocity_y;

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

 //     bNotChangev = true;
      if((diff_time >  0)&&(bNotChangev == false))
      {
          double aa = (current_velocity -previous_velocity)/diff_time;
          if(fabs(aa)>5.0)
          {
              givlog->verbose("NDT","aa is %f",aa);
              aa = fabs(5.0/aa);
              current_velocity = previous_velocity + aa*(current_velocity -previous_velocity);
              current_velocity_x = previous_velocity_x + aa *((current_velocity_x -previous_velocity_x));
              current_velocity_y = previous_velocity_y + aa *((current_velocity_y -previous_velocity_y));
              current_velocity_z = previous_velocity_z + aa *((current_velocity_z -previous_velocity_z));

          }
      }

      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;

      gcurndtgpspos = PoseToGPS(gvector_trace[gcurmapindex].mndtorigin,current_pose);
//      std::cout<<" pre x:"<<previous_pose.x<<" pre y:"<<previous_pose.y<<std::endl;
    std::cout<<" x: "<<current_pose.x<<" y: "<<current_pose.y
            <<" z: "<<current_pose.z<<" yaw:"<<current_pose.yaw
           <<" vel: "<<current_velocity<<std::endl;


    std::cout<<"NDT ve:"<<gcurndtgpspos.ve<<" vn:"<<gcurndtgpspos.vn
            <<" GPS ve:"<<gcurgpsgpspos.ve<<" vn:"<<gcurgpsgpspos.vn<<std::endl;

    gbFileNDTUpdate = true;

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

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

    current_velocity_imu_x = current_velocity_x;
    current_velocity_imu_y = current_velocity_y;
    current_velocity_imu_z = current_velocity_z;

    current_velocity_imu_z = 0;

    offset_imu_x = 0.0;
    offset_imu_y = 0.0;
    offset_imu_z = 0.0;
    offset_imu_roll = 0.0;
    offset_imu_pitch = 0.0;
    offset_imu_yaw = 0.0;

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




    double hdg_o = (90 - gvector_trace[gcurmapindex].mndtorigin.heading)*M_PI/180.0;
    double ndt_vn = current_velocity_x * cos(hdg_o) - current_velocity_y * sin(hdg_o);
    double ndt_ve = current_velocity_x * sin(hdg_o) + current_velocity_y * cos(hdg_o);
    double ndt_vd = current_accel_z;



    std::cout<<std::setprecision(9)<<"gps lat:"<<gcurndtgpspos.lat<<" lon:"
            <<gcurndtgpspos.lon<<" z:"<<gcurndtgpspos.height<<" heading:"
           <<gcurndtgpspos.heading<<std::endl;
    std::cout<<std::setprecision(6)<<std::endl;




    double x0,y0;
    double x,y;
    GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
    GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
    double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
    double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
    double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
    std::cout<<" dis:"<<dis<<" headdiff:"<<headdiff<<" zdiff:"<<zdiff<<std::endl;

    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(gpc,strser,ndatasize);
    }
    else
    {
        std::cout<<BOLDRED<<"ndtpos serialize error."<<RESET<<std::endl;
    }
    delete strser;

    iv::lidar::ndtgpspos xndtgpspos;
    xndtgpspos.set_lat(gcurndtgpspos.lat);
    xndtgpspos.set_lon(gcurndtgpspos.lon);
    xndtgpspos.set_heading(gcurndtgpspos.heading);
    xndtgpspos.set_height(gcurndtgpspos.height);
    xndtgpspos.set_pitch(gcurndtgpspos.pitch);
    xndtgpspos.set_roll(gcurndtgpspos.roll);
    xndtgpspos.set_tras_prob(trans_probability);
    xndtgpspos.set_score(fitness_score);
    xndtgpspos.set_vn(ndt_vn);
    xndtgpspos.set_ve(ndt_ve);
    xndtgpspos.set_vd(ndt_vd);

    givlog->debug("ndtgpspos","lat:%11.7f lon:%11.7f height:%11.3f heading:%6.3f pitch:%6.3f roll:%6.3f vn:%6.3f ve:%6.3f vd:%6.3f trans_prob:%6.3f score:%6.3f",
                  xndtgpspos.lat(),xndtgpspos.lon(),xndtgpspos.height(),
                  xndtgpspos.heading(),xndtgpspos.pitch(),xndtgpspos.roll(),
                  xndtgpspos.vn(),xndtgpspos.ve(),xndtgpspos.vd(),
                  xndtgpspos.tras_prob(),xndtgpspos.score());

    ndatasize = xndtgpspos.ByteSize();
    strser = new char[ndatasize];
    bSer = xndtgpspos.SerializeToArray(strser,ndatasize);
    if(bSer)
    {
        std::cout<<"share msg."<<std::endl;
        iv::modulecomm::ModuleSendMsg(gpd,strser,ndatasize);
    }
    else
    {
        std::cout<<BOLDRED<<"ndtgpspos serialize error."<<RESET<<std::endl;
    }
    delete strser;

    if(trans_probability < 0.5)gbNeedGPSUpdateNDT = true;
    else gbNeedGPSUpdateNDT = false;


}


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

void setuseimu(bool bUse)
{
    _use_imu = bUse;
}