#ifndef DESKEW_POINT_CLOUD_H
#define DESKEW_POINT_CLOUD_H

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>

#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>

#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <opencv2/opencv.hpp>
#include <mutex>
#include <map>
#include<algorithm>
#include<random>
struct PointXYZIRT {
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    double time;
    uint16_t ring;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT,
                                   (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (double , time, time)
                                           (uint16_t, ring, ring)
)

class DeskewPointCloud
{
public:
    explicit DeskewPointCloud(ros::NodeHandle &iNh,ros::NodeHandle &iNhPrivate);
    ~DeskewPointCloud();

private:
    float m_fVoxelLeafSize_;
    const int m_iQueueLength = 100;

    std::mutex imuLock;

    bool m_bFirstPointFlag_;
    bool m_bImuAvailable_;
    bool m_bOdomAvailable_;
    bool m_bOdomDeskewFlag_;

    double m_dTimeScanCur_, m_dTimeScanEnd_;
    double m_dDistanceMax_,m_dDistanceMin_;
    int m_nImuPointerFront_;
    int m_nImuPointerLast_;
    int m_nImuPointerCur_;
    int m_nOdomPointerCur_;

    std::deque<sensor_msgs::Imu> m_qImu;
    std::deque<nav_msgs::Odometry> m_qOdom_;

    double *m_pImuTime_;
    double *m_pOdomTime_;

    double *m_pImuRotX_;
    double *m_pImuRotY_;
    double *m_pImuRotZ_;

    double *m_pOdomTansX_;
    double *odomTansY;
    int m_dScanRows_,m_dScanCols_;

    pcl::PointCloud<PointXYZIRT>::Ptr m_pPointCloudIn_;
    pcl::PointCloud<pcl::PointXYZI>::Ptr m_pPointCloudDeskew_;
    pcl::PointCloud<pcl::PointXYZI>::Ptr m_pPointCloudFilter_;
    cv::Mat m_iMatCloud_;
    cv::RNG m_iRng;
    std::mt19937 m_iGen;
    std::uniform_int_distribution<int> dis;
    Eigen::Affine3f transStartInverse;

    ros::NodeHandle nh;
    ros::Subscriber m_iSubImu_;
    ros::Subscriber odom_sub;
    ros::Subscriber m_iSubPointCloud_;
    ros::Publisher m_iPubDeskewCloud_;
    ros::Publisher m_iPubFilterCloud_;
    ros::Publisher m_iPubFilterCloud2_;
    ros::Publisher m_iPubFilterCloud3_;
    ros::Publisher m_iPubCloudGrand_;
    ros::Publisher m_iPubCloudNoGrand_;

    double PointDistance(pcl::PointXYZI p);
    sensor_msgs::Imu __ImuConverter(const sensor_msgs::Imu& imu_in);
    void __ImuDataHandle();
    void __OdomDataHandle();
    bool __DataHandle();
    void __FindRotation(double dPointTime, float *pRotXCur, float *pRotYCur, float *pRotZCur);
    void __FindPosition(double dPointTime, float *pPosXCur, float *pPosYCur, float *pPosZCur);
    pcl::PointXYZI __DeskewPoint(pcl::PointXYZI *pPoint, double dRelTime);
    void __ResetParameters();
    void __ProjectPointCloud(const sensor_msgs::PointCloud2::ConstPtr &currentCloudMsg);
    static void PublishCloud(ros::Publisher *pPubObject, pcl::PointCloud<pcl::PointXYZI>::Ptr pCloud,
                             ros::Time iStamp, std::string cFrameName);
    void __OnCallbackImu(const sensor_msgs::Imu::ConstPtr &pMsgImu);
    void __OnCallbackOdom(const nav_msgs::Odometry::ConstPtr &pMsgOdom);
    void __OnCallbackLidarCloud(const sensor_msgs::PointCloud2::ConstPtr &pMsgLidarPintCloud);
    void VoxelGridFilter(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pCloud, double time, std::string cFrameName);
    void __FilterPointCloud( const cv::Mat& mat, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud);
    double __PointDistance(pcl::PointXYZI p1, pcl::PointXYZI p2);
    void __CloudClustering(pcl::PointCloud<pcl::PointXYZI>::Ptr pCloudIn, pcl::PointCloud<pcl::PointXYZRGBL>::Ptr pCloudOut);
    void __RanSac(pcl::PointCloud<pcl::PointXYZI>::Ptr pCloudIn, pcl::PointCloud<pcl::PointXYZI>::Ptr pCloudOutPlane, pcl::PointCloud<pcl::PointXYZI>::Ptr pCloudOutOther);
};


#endif