#pragma once
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <fstream>
#include <ostream>
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include "Filter.h"
#include "uwb.h"
#include "runL.h"


namespace uwb_slam{
    class Data{
        public:
        Data();
        ~Data(){};
        void Run();
        void odomCB(const nav_msgs::Odometry& odom);
        void calculate(std::vector<vector<float>> &data);
        
        public:
        ros::NodeHandle nh_;
        ros::Subscriber odom_sub_;

        std::ofstream fileout,fileout1,fileout2;
        std::string filename = "/home/firefly/pibot_ros/ros_ws/src/upbot_location/parameter/AnchorPos.txt";
        std::string filename1 = "/home/firefly/pibot_ros/ros_ws/src/upbot_init/data1.txt";
        std::string filename2 = "/home/firefly/pibot_ros/ros_ws/src/upbot_init/data2.txt";

        nav_msgs::Odometry odom_;
        
        float H_0 = 25.0;   // high of car tag
        float H_tag = 85.0;   // high of tag
        int tag_num = 3;    //nums of tag

        ros::Time imuDataRxTime;
        std::vector<float> data_,predata_;
        std::vector<vector<float>> data_all;
        float x=-1,y=-1;
        Filter filter;
        std::shared_ptr<uwb_slam::Uwb> uwb_;
        std::shared_ptr<uwb_slam::RunL> runL_;

    };

};