#ifndef MAPPING_ALIGN_H
#define MAPPING_AlIGN_H
#include <cmath>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>
#include <utility>
#include <queue>
#include <fstream>
#include "pibot_msgs/RawImu.h"
#include "type.h"
#include "uwb.h"
#include "Mat.h"
#include "UWBFilter.h"



namespace uwb_slam{
    class Align
    {
    public:
        Align(){
            imu_odomPos.Init(2, 1, 0);
            uwbPos.Init(2, 1, 0);
            syncPos.Init(2, 1, 0);
            imu_odom_StartPos.Init(2, 1, 0);
            Rotate.Init(2,2,0);
            uwbStartPos.Init(2,1,0);
        };
        void Run();
        void imuCB(const pibot_msgs::RawImu& imu);
        void odom_imuCB(const nav_msgs::Odometry& odom);

    public:
        ros::NodeHandle nh_;
        ros::Subscriber imu_sub_;
        ros::Subscriber odom_imu_sub_;
        Imu_odom_pose_data imu_odom_;
        Uwb_data uwb_data_;
        ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
        Mat imu_odom_StartPos;
        Mat imu_odomPos;
        Mat uwbPos;
        Mat syncPos;
        Mat Rotate;
        Mat uwbStartPos;
        double qx, qy, qz, qw;
        ros::Time odom_tmp_ ;
        bool write_data_ = false;
        cv::Mat img1;
        std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
        std::shared_ptr<uwb_slam::Uwb> uwb_;
        UWBFilter filter;

    };
};
#endif
