#include "Mat.h"
#include "mapping.h"
#include <iostream>
#include <ros/ros.h>
#include <thread>



int main(int argc, char** argv)
{
    ros::init(argc, argv, "upbot_location"); // Initialize the ROS node
    std::shared_ptr<uwb_slam::Mapping> mp = std::make_shared<uwb_slam::Mapping>();
    std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>();
    std::shared_ptr<uwb_slam::Senddata> sender = std::make_shared<uwb_slam::Senddata>();
    std::shared_ptr<uwb_slam::Align> align = std::make_shared<uwb_slam::Align>();
    mp->uwb_ = uwb;
    mp->align_ = align;
    align->uwb_ = uwb;
    sender->align_ = align;

    // uwb serried read
    std::thread uwb_trd([&uwb]() {
        uwb->Run();
    });

    // build map
    // std::thread map_trd ([&mp]() {
    //     mp->Run();
    // });

    std::thread sender_trd ([&sender]() {
        sender->Run();
    });

    std::thread align_trd ([&align]() {
        align->Run();
    });

    ros::spin(); 
}
