#include "cyberdog_marker/marker_service.hpp"

namespace cyberdog
{
namespace marker
{

MarkerService::MarkerService(const rclcpp::NodeOptions & options)
: Node("cyberdog_marker_service", options),
  marker_count_(0)
{
  // 创建服务提供位置标记功能
  place_marker_service_ = create_service<cyberdog_marker::srv::PlaceMarker>(
    "place_marker",
    std::bind(
      &MarkerService::handle_place_marker, this,
      std::placeholders::_1, std::placeholders::_2));
      
  // 创建客户端连接到Gazebo的spawn_entity服务
  spawn_entity_client_ = create_client<gazebo_msgs::srv::SpawnEntity>("spawn_entity");
  
  RCLCPP_INFO(get_logger(), "Marker service started");
}

void MarkerService::handle_place_marker(
  const std::shared_ptr<cyberdog_marker::srv::PlaceMarker::Request> request,
  std::shared_ptr<cyberdog_marker::srv::PlaceMarker::Response> response)
{
  // 等待Gazebo服务可用
  if (!spawn_entity_client_->wait_for_service(std::chrono::seconds(1))) { // 缩短超时，快速失败
    response->success = false;
    response->message = "Gazebo spawn_entity service not available";
    RCLCPP_ERROR(get_logger(), "%s", response->message.c_str());
    return;
  }

  // 生成唯一的标记名称
  marker_count_++;
  std::string marker_name = "marker_" + std::to_string(marker_count_);

  // 生成SDF标记内容
  std::string marker_sdf = generate_marker_sdf(request->color);

  // 创建spawn_entity请求
  auto spawn_request = std::make_shared<gazebo_msgs::srv::SpawnEntity::Request>();
  spawn_request->name = marker_name;
  spawn_request->xml = marker_sdf;
  spawn_request->robot_namespace = ""; // 通常为空，除非你的Gazebo模型在特定命名空间下
  
  // 设置标记位置
  spawn_request->initial_pose.position.x = request->x;
  spawn_request->initial_pose.position.y = request->y;
  spawn_request->initial_pose.position.z = request->z;
  spawn_request->initial_pose.orientation.w = 1.0; // 默认无旋转

  // 发送请求
  auto future_response = spawn_entity_client_->async_send_request(spawn_request);
  
  // 让我们先尝试一个简单的阻塞获取，看看gazebo服务是否足够快
  std::chrono::seconds timeout(5); // 设置一个超时
  auto future_status = future_response.wait_for(timeout);

  if (future_status == std::future_status::ready) {
    auto spawn_response_ptr = future_response.get();
    response->success = spawn_response_ptr->success;
    if (response->success) {
      response->message = "Successfully placed marker '" + marker_name + "' at (" 
        + std::to_string(request->x) + ", " 
        + std::to_string(request->y) + ", " 
        + std::to_string(request->z) + ")";
    } else {
      response->message = "Failed to place marker: " + spawn_response_ptr->status_message;
    }
  } else if (future_status == std::future_status::timeout) {
    response->success = false;
    response->message = "Gazebo spawn_entity service call timed out";
  } else { // deferred
    response->success = false;
    response->message = "Gazebo spawn_entity service call deferred or error";
  }
  
  if (response->success) {
      RCLCPP_INFO(get_logger(), "%s", response->message.c_str());
  } else {
      RCLCPP_ERROR(get_logger(), "%s", response->message.c_str());
  }
}

std::string MarkerService::generate_marker_sdf(const std::string & color, double radius)
{
  // 解析颜色
  double r = 1.0, g = 0.0, b = 0.0;  // 默认红色
  if (color == "red") {
    r = 1.0; g = 0.0; b = 0.0;
  } else if (color == "green") {
    r = 0.0; g = 1.0; b = 0.0;
  } else if (color == "blue") {
    r = 0.0; g = 0.0; b = 1.0;
  } else if (color == "yellow") {
    r = 1.0; g = 1.0; b = 0.0;
  }

  // 创建SDF模型
  std::stringstream sdf;
  sdf << "<?xml version='1.0'?>\n"
      << "<sdf version='1.6'>\n"
      << "  <model name='marker'>\n"
      << "    <static>true</static>\n"
      << "    <link name='link'>\n"
      << "      <visual name='visual'>\n"
      << "        <cast_shadows>false</cast_shadows>\n"
      << "        <geometry>\n"
      << "          <sphere>\n"
      << "            <radius>" << radius << "</radius>\n"
      << "          </sphere>\n"
      << "        </geometry>\n"
      << "        <material>\n"
      << "          <ambient>" << r << " " << g << " " << b << " 1</ambient>\n"
      << "          <diffuse>" << r << " " << g << " " << b << " 1</diffuse>\n"
      << "          <specular>0.1 0.1 0.1 1</specular>\n"
      << "        </material>\n"
      << "      </visual>\n"
      << "    </link>\n"
      << "  </model>\n"
      << "</sdf>";
  
  return sdf.str();
}

}  // namespace marker
}  // namespace cyberdog

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(cyberdog::marker::MarkerService)