// vision_object_recg_node.cpp
// 提供 VisionObjectRecognition.srv 服务，实现简单的目标识别逻辑

// ROS2 core
#include <rclcpp/rclcpp.hpp>

// Service and custom msg
#include "interfaces/srv/vision_object_recognition.hpp"
#include "interfaces/msg/pose_class_and_id.hpp"

// Std / geometry msgs
#include <std_msgs/msg/header.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <sstream>

using interfaces::srv::VisionObjectRecognition;

class VisionObjectRecgNode : public rclcpp::Node {
public:
  VisionObjectRecgNode(): Node("vision_object_recg") {
    // 服务名称与 SkillManager 中 to_snake_case("VisionObjectRecognition") 保持一致: VisionObjectRecognition
    service_ = create_service<VisionObjectRecognition>(
      "vision_object_recognition",
      std::bind(&VisionObjectRecgNode::handle_service, this, std::placeholders::_1, std::placeholders::_2));
    RCLCPP_INFO(get_logger(), "VisionObjectRecognition service server ready on /vision_object_recognition");
  }

private:
  void handle_service(const std::shared_ptr<VisionObjectRecognition::Request> req,
                      std::shared_ptr<VisionObjectRecognition::Response> res) {
    RCLCPP_INFO(get_logger(), "Received recognition request for camera_position='%s'", req->camera_position.c_str());

    // 填充 header
    res->header.stamp = now();
    // 若调用方传 camera_position 作为一个逻辑位姿来源，可作为 frame_id；否则给一个默认
    // res->header.frame_id = req->camera_position.empty() ? "camera_link" : ("camera_" + req->camera_position);

    res->header.frame_id = "base_link_camera";

    // 简单逻辑: 只要 camera_position 非空且不是 unknown 就认为识别到两个目标
    if (!req->camera_position.empty() && req->camera_position != "unknown") {
      res->success = true;

      // 伪造两类目标 (示例: cup / bottle)，每类给出 1~2 个 Pose
      interfaces::msg::PoseClassAndID obj1;
      obj1.class_name = "bottlex";
      obj1.class_id = 1;

      //{-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
      geometry_msgs::msg::Pose pose1; // 基本位姿
      // pose1.position.x = 0.9758;
      // pose1.position.y = 0;
      // pose1.position.z = 0.42;
      // pose1.orientation.x = -0.5;
      // pose1.orientation.y = 0.5;
      // pose1.orientation.z = 0.5;
      // pose1.orientation.w = -0.5;

      //geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.2851924786746129, y=-0.056353812689333635, z=1.073772448259523), 
      //orientation=geometry_msgs.msg.Quaternion(x=-0.16713377464857188, y=-0.42460237568763715, z=-0.26706232441180955, w=0.8487972895879773))
      // pose1.position.x = -0.2851924786746129;
      // pose1.position.y = -0.056353812689333635;
      // pose1.position.z = 1.073772448259523;
      // pose1.orientation.x = -0.16713377464857188;
      // pose1.orientation.y = -0.42460237568763715;
      // pose1.orientation.z = -0.26706232441180955;
      // pose1.orientation.w = 0.8487972895879773;

      //222.853, 514.124, 261.742
      //-0.65115316, 0.05180144, -0.19539139, 0.73153153
      //-0.63142093, 0.18186004, -0.12407289, 0.74353241
      pose1.position.x = 222.853;
      pose1.position.y = 514.124;
      pose1.position.z = 261.742;
      pose1.orientation.x = -0.65115316;
      pose1.orientation.y = 0.05180144;
      pose1.orientation.z = -0.19539139;
      pose1.orientation.w = 0.73153153;

      obj1.pose_list.push_back(pose1);

      // 第二个同类实例(示例)
      geometry_msgs::msg::Pose pose1b = pose1;
      pose1b.position.y += 0.05;
      obj1.pose_list.push_back(pose1b);

      interfaces::msg::PoseClassAndID obj2;
      obj2.class_name = "cup";
      obj2.class_id = 2;
      geometry_msgs::msg::Pose pose2;
      pose2.position.x = 0.5;
      pose2.position.y = -0.2;
      pose2.position.z = 0.78;
      pose2.orientation.w = 1.0;
      obj2.pose_list.push_back(pose2);

      res->objects.clear();
      res->objects.emplace_back(obj1);
      res->objects.emplace_back(obj2);

      std::ostringstream oss;
      oss << "Recognized " << res->objects.size() << " object classes (total instances: "
          << (obj1.pose_list.size() + obj2.pose_list.size()) << ")";
      res->info = oss.str();
    } else {
      res->success = false;
      res->objects.clear();
      res->info = "No objects recognized: invalid or unknown camera_position.";
    }

    RCLCPP_INFO(get_logger(), "Response: success=%d, info=%s, classes=%zu", res->success, res->info.c_str(), res->objects.size());
  }

  rclcpp::Service<VisionObjectRecognition>::SharedPtr service_;
};

//ros2 service call /vision_object_recognition interfaces/srv/VisionObjectRecognition "{camera_position: 'left'}"

int main(int argc, char ** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<VisionObjectRecgNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
