#ifndef NDT_MATCHING_H
#define NDT_MATCHING_H
#include<QDateTime>
#include <QMutex>

struct pose
{
  double x;
  double y;
  double z;
  double roll;
  double pitch;
  double yaw;
  double vx;
  double vy;
};


namespace  iv {
struct lidar_pose
{
    lidar_pose() {}

    double vel_x;
    double vel_y;
    double vel_z;
    double vel;
    double accel_x;
    double accel_y;
    double accel_z;
    double accel;
    struct pose mpose;
};

}

namespace iv {
struct imudata
{
  qint64 imutime;
  double imu_linear_acceleration_x;
  double imu_linear_acceleration_y;
  double imu_linear_acceleration_z;
  double imu_angular_velocity_x;
  double imu_angular_velocity_y;
  double imu_angular_velocity_z;
};
}


void ndt_match_Init_nomap();

void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);

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

void point_ndt_test(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);

int GetPose(int & curindex,iv::lidar_pose & xlidar_pose);

void SetCurPose(double x0,double y0,double yaw0,double z0 = 0,double pitch0 = 0,double roll0 = 0);

void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);

void SetRunState(bool bRun);

namespace iv {
struct ndttracepoint
{
    double x;
    double y;
    double z;
    double pitch;
    double roll;
    double yaw;
};
}

namespace iv {
struct gpspos
{
    double lat;
    double lon;
    double height;
    double heading;
    double pitch;
    double roll;
    double ve;
    double vn;
};
}

iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose);
pose CalcPose(iv::gpspos xori,iv::gpspos xcur);

namespace iv {
struct ndtmaptrace
{
    ndtmaptrace() {}
    std::vector<ndttracepoint> mvector_trace;
    std::string mstrpcdpath;
    iv::gpspos mndtorigin;
};

}

void setuseimu(bool bUse);
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);

#endif // NDT_MATCHING_H