#include "Mat.h"

// #include "align.h"
#include "mapping.h"

#include <iostream>
#include <ros/ros.h>
#include <thread>
#include "senddata.h"



int main(int argc, char** argv)
{
    ros::init(argc, argv, "locate_info_pub_node"); // 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>();
    std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();

    


    mp->uwb_ = uwb;
    mp->align_ = align;
    align->uwb_ =uwb;
    align->lighthouse_ = lighthouse;
    sender->uwb_ = uwb;
    // //  control data fllow in system
    // std::thread rose_trd ([&system]() {
    //     system->Run();
    // });

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

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

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

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

    // std::thread lighthouse_trd ([&lighthouse]() {
    //     lighthouse->Run();
    // });

    ros::spin(); 
}
