#ifndef __TF_CONVERTER_H_INCLUDED_
#define __TF_CONVERTER_H_INCLUDED_

#include <ros/ros.h>
#include <robot/robot.h>
#include <tf2_ros/transform_listener.h>
#include <tf3/buffer_core.h>
#include <tf3/exceptions.h>
#include <move_base_core/common.h>

namespace amr_control
{
  class TfConverter
  {
  public:
    TfConverter(std::shared_ptr<tf2_ros::Buffer> tf);
    ~TfConverter();

    /**
     * @brief Lấy buffer tf3 từ tf_core_ptr_
     * @param tf_buffer Shared pointer to tf3::BufferCore
     */
    void TfBuffer(std::shared_ptr<tf3::BufferCore> &tf_buffer);

    /**
     * @brief Kiểm tra và in ra thông tin về tf_core_ptr_ để verify dữ liệu cập nhật
     * @param check_frames Các frame pairs cần kiểm tra (ví dụ: {"map", "base_footprint"})
     */
    void checkTfCoreData(const std::vector<std::pair<std::string, std::string>> &check_frames, robot::TFListenerPtr &tf_core_ptr);

  private:
    /**
     * @brief Thread worker for tf conversion
     */
    void tfWorker();

    std::shared_ptr<tf2_ros::Buffer> tf_;
    std::shared_ptr<tf3::BufferCore> tf3_buffer_;
    std::thread tf_thread_;
    std::atomic<bool> stop_tf_thread_{false};
    std::mutex tf3_mutex_;
  };
}

#endif