#include "amr_control/tf_converter.h"

amr_control::TfConverter::TfConverter(std::shared_ptr<tf2_ros::Buffer> tf)
  : tf_(tf)
{
  tf3_buffer_ = std::make_shared<tf3::BufferCore>();
  tf3_buffer_->setUsingDedicatedThread(true);
  tf_thread_ = std::thread(&amr_control::TfConverter::tfWorker, this);
}

amr_control::TfConverter::~TfConverter()
{
  stop_tf_thread_ = true;
  tf_thread_.join();
}
void amr_control::TfConverter::tfWorker()
{
  struct TFEdge {
    std::string parent;
    std::string child;
  }; 
  std::vector<TFEdge> edges;
  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  std::string last_tree;
  std::string tree;
  std::string line;

  ros::Rate rate(20);
  while (ros::ok() && !stop_tf_thread_)
  {
      tree = tfBuffer.allFramesAsString();

      if (!tree.empty() && tree == last_tree)
      {
          // ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
          break;
      }

      last_tree = tree;
      ros::spinOnce();
      rate.sleep();
  }

  std::istringstream iss(tree);

  while (ros::ok() && std::getline(iss, line) && !stop_tf_thread_)
  {
      // Frame X exists with parent Y
      std::size_t f1 = line.find("Frame ");
      std::size_t f2 = line.find(" exists with parent ");

      if (f1 != std::string::npos && f2 != std::string::npos)
      {
          std::string child  = line.substr(f1 + 6, f2 - (f1 + 6));
          std::string parent = line.substr(f2 + 20);

          if (!parent.empty() && parent.back() == '.')
          parent.pop_back();

          edges.push_back({parent, child});
          ROS_WARN("parent: %s | child: %s", parent.c_str(), child.c_str());
      }
  }

  while (ros::ok() && !stop_tf_thread_)
  {
    if (!tf_ || !tf3_buffer_)
    {
      rate.sleep();
      continue;
    }

    try
    {
      for (const auto& e : edges)
      {
        try
        {
          if (tf_->canTransform(e.parent, e.child, ros::Time(0)))
          { 
            auto transform =
            tf_->lookupTransform(e.parent, e.child, ros::Time(0));
            
            tf3::TransformStampedMsg t;
            t.header.stamp = tf3::Time::now();
            t.header.frame_id = transform.header.frame_id;
            t.child_frame_id = transform.child_frame_id;
            t.transform.translation.x = transform.transform.translation.x;
            t.transform.translation.y = transform.transform.translation.y;
            t.transform.translation.z = transform.transform.translation.z;
            t.transform.rotation.x = transform.transform.rotation.x;
            t.transform.rotation.y = transform.transform.rotation.y;
            t.transform.rotation.z = transform.transform.rotation.z;
            t.transform.rotation.w = transform.transform.rotation.w;

            {
              std::lock_guard<std::mutex> lock(tf3_mutex_);
              tf3_buffer_->setTransform(t, "dynamic_tf");
            }
          }
        }
        catch (tf2::TransformException &ex)
        {
            ROS_WARN("TF %s -> %s failed: %s",
                    e.parent.c_str(),
                    e.child.c_str(),
                    ex.what());
        }
      }
      // ROS_INFO("TF worker thread running");
    }
    catch (const std::exception& e)
    {
      ROS_WARN("TF worker exception: %s", e.what());
    }

    rate.sleep();
  }

  ROS_INFO("TF worker thread cleanly exited");
}

void amr_control::TfConverter::TfBuffer(std::shared_ptr<tf3::BufferCore> &tf_buffer)
{
  tf_buffer = tf3_buffer_;
}

void amr_control::TfConverter::checkTfCoreData(const std::vector<std::pair<std::string, std::string>>& check_frames, robot::TFListenerPtr &tf_core_ptr)
{
  if (!tf_core_ptr)
  {
    robot::log_warning("[%s:%d] tf_core_ptr_ is nullptr", __FILE__, __LINE__);
    return;
  }

  robot::log_info("[%s:%d] ========== Checking tf_core_ptr_ Data ==========", __FILE__, __LINE__);
  
  // In ra tất cả frames có trong buffer
  try
  {
    std::string all_frames = tf_core_ptr->allFramesAsString();
    if (!all_frames.empty())
    {
      robot::log_info("[%s:%d] All frames in tf_core_ptr_:\n%s", __FILE__, __LINE__, all_frames.c_str());
    }
    else
    {
      robot::log_warning("[%s:%d] No frames found in tf_core_ptr_", __FILE__, __LINE__);
    }
  }
  catch (const std::exception& ex)
  {
    robot::log_error("[%s:%d] Exception getting all frames: %s", __FILE__, __LINE__, ex.what());
  }

  // Kiểm tra các frame pairs được chỉ định
  if (!check_frames.empty())
  {
    robot::log_info("[%s:%d] Checking specific frame pairs:", __FILE__, __LINE__);
    for (const auto& frame_pair : check_frames)
    {
      const std::string& target_frame = frame_pair.first;
      const std::string& source_frame = frame_pair.second;
      
      try
      {
        std::string error_msg;
        bool can_transform = tf_core_ptr->canTransform(target_frame, source_frame, tf3::Time(), &error_msg);
        
        if (can_transform)
        {
          try
          {
            tf3::TransformStampedMsg transform = tf_core_ptr->lookupTransform(target_frame, source_frame, tf3::Time());
            robot::log_info("[%s:%d] Transform [%s] -> [%s]:", __FILE__, __LINE__, 
                           source_frame.c_str(), target_frame.c_str());
            robot::log_info("  Translation: (%.3f, %.3f, %.3f)", 
                           transform.transform.translation.x,
                           transform.transform.translation.y,
                           transform.transform.translation.z);
            robot::log_info("  Rotation: (%.3f, %.3f, %.3f, %.3f)", 
                           transform.transform.rotation.x,
                           transform.transform.rotation.y,
                           transform.transform.rotation.z,
                           transform.transform.rotation.w);
            robot::log_info("  Stamp: %.3f", transform.header.stamp.toSec());
          }
          catch (const tf3::LookupException& ex)
          {
            robot::log_error("[%s:%d] LookupException for [%s] -> [%s]: %s", 
                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
          }
          catch (const tf3::ConnectivityException& ex)
          {
            robot::log_error("[%s:%d] ConnectivityException for [%s] -> [%s]: %s", 
                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
          }
          catch (const tf3::ExtrapolationException& ex)
          {
            robot::log_error("[%s:%d] ExtrapolationException for [%s] -> [%s]: %s", 
                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
          }
        }
        else
        {
          robot::log_warning("[%s:%d] Cannot transform [%s] -> [%s]: %s", 
                           __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), error_msg.c_str());
        }
      }
      catch (const std::exception& ex)
      {
        robot::log_error("[%s:%d] Exception checking transform [%s] -> [%s]: %s", 
                       __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
      }
    }
  }
  else
  {
    // Nếu không có frame pairs được chỉ định, kiểm tra một số frame phổ biến
    std::vector<std::pair<std::string, std::string>> common_frames = {
      {"map", "base_footprint"},
      {"odom", "base_footprint"},
      {"base_footprint", "base_link"}
    };
    
    robot::log_info("[%s:%d] Checking common frame pairs:", __FILE__, __LINE__);
    for (const auto& frame_pair : common_frames)
    {
      const std::string& target_frame = frame_pair.first;
      const std::string& source_frame = frame_pair.second;
      
      try
      {
        std::string error_msg;
        bool can_transform = tf_core_ptr->canTransform(target_frame, source_frame, tf3::Time(), &error_msg);
        
        if (can_transform)
        {
          robot::log_info("[%s:%d] ✓ Transform available: [%s] -> [%s]", 
                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str());
        }
        else
        {
          robot::log_info("[%s:%d] ✗ Transform NOT available: [%s] -> [%s] (%s)", 
                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), error_msg.c_str());
        }
      }
      catch (const std::exception& ex)
      {
        robot::log_warning("[%s:%d] Exception checking [%s] -> [%s]: %s", 
                         __FILE__, __LINE__, source_frame.c_str(), target_frame.c_str(), ex.what());
      }
    }
  }
  
  robot::log_info("[%s:%d] ========== End Checking tf_core_ptr ==========", __FILE__, __LINE__);
}