#include <queue>
#include <opencv2/opencv.hpp>
#include <mutex>
#include <nav_msgs/Odometry.h>
#include "uwb.h"
#include "align.h"
#include "UWBFilter.h"
// #include "senddata.h"

#ifndef MAPPING_H
#define MAPPING_H


namespace uwb_slam{
    class Mapping
    {
        public:
        const double PIXEL_SCALE = 5.0;     //xiangsudian cm
        const int    AREA_SIZE = 2000;      //map size cm
        Mapping() {};
        void Run();
        bool checkQueue();
        void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData);
        // void feedPos(const cv::Point2d & uwb);
        void process();
        std::mutex mMutexMap;
        std::shared_ptr<uwb_slam::Uwb> uwb_;
        std::shared_ptr<uwb_slam::Align> align_;

        private:
        int realWidth, realHeight;
        std::queue<std::vector<cv::Point2d>> posDataQueue;
        // std::queue<cv::Point2d> posDataQueue;
        std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1});
        
        nav_msgs::Odometry mapping;
        ros::NodeHandle nh_;
        ros::Subscriber odom_imu_sub_;
        void odom_imuCB(const nav_msgs::Odometry& odom);

        

        bool readPos = false;
        cv::Mat img;
        UWBFilter filter;
    };

}

#endif
