#include "behaviortree_ros2/bt_service_node.hpp"

#include "behaviortree_ros2/plugins.hpp"
#include <behaviortree_cpp/basic_types.h>
// #include <geometry_msgs/msg/detail/point__struct.hpp>
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
#include <memory>
// #include <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
#include <rclcpp/logging.hpp>
#include <string>

using GraspingService = rbs_skill_interfaces::srv::GetPickPlacePoses;
using namespace BT;

class Grasping : public RosServiceNode<GraspingService> {
public:
  Grasping(const std::string &name, const NodeConfig &conf,
           const RosNodeParams &params)
      : RosServiceNode<GraspingService>(name, conf, params) {

    RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
  }

  static PortsList providedPorts() {
    return providedBasicPorts(
        {InputPort<std::string>("object_name"),
         InputPort<std::string>("action_name"),
         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pre_pose"),
         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("post_pose")});
  }

  bool setRequest(Request::SharedPtr &request) override {
    RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
                this->service_name_.c_str());
    if (!getInput("action_name", request->action_name)) {
      RCLCPP_ERROR(this->node_.lock()->get_logger(),
                   "Failed to get object_name from input port");
      return false;
    }
    return true;
  }

  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
    if (!response->ok) {
      RCLCPP_ERROR(this->node_.lock()->get_logger(),
                   "Response indicates failure.");
      return NodeStatus::FAILURE;
    }

    RCLCPP_INFO(this->node_.lock()->get_logger(),
                "Response received successfully.");

    auto logPose = [this](const std::string &pose_name,
                          const geometry_msgs::msg::Pose &pose) {
      RCLCPP_INFO(this->node_.lock()->get_logger(),
                  "%s:\n"
                  "  Position: x = %.4f, y = %.4f, z = %.4f\n"
                  "  Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
                  pose_name.c_str(), pose.position.x, pose.position.y,
                  pose.position.z, pose.orientation.x, pose.orientation.y,
                  pose.orientation.z, pose.orientation.w);
    };
    if (!response->grasp.empty()) {
      RCLCPP_INFO(this->logger(), "Got Pick Response");
      std::vector<std::string> poses = {"Pregrasp Pose", "Grasp Pose",
                                        "Postgrasp Pose"};

      for (size_t n = 0; n < poses.size() && n < response->grasp.size(); ++n) {
        logPose(poses[n], response->grasp.at(n));
      }

      auto grasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
      auto pregrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
      auto postgrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();

      *pregrasp_pose = response->grasp.at(0);
      *grasp_pose = response->grasp.at(1);
      *postgrasp_pose = response->grasp.at(2);

      setOutput("pre_pose", pregrasp_pose);
      setOutput("pose", grasp_pose);
      setOutput("post_pose", postgrasp_pose);
      return NodeStatus::SUCCESS;
    }
    if (!response->place.empty()) {
      RCLCPP_INFO(this->logger(), "Got Place Response");
      std::vector<std::string> poses = {"Preplace Pose", "Place Pose",
                                        "Postplace Pose"};

      for (size_t n = 0; n < poses.size() && n < response->place.size(); ++n) {
        logPose(poses[n], response->place.at(n));
      }

      auto place_pose = std::make_shared<geometry_msgs::msg::Pose>();
      auto preplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
      auto postplace_pose = std::make_shared<geometry_msgs::msg::Pose>();

      *preplace_pose = response->place.at(0);
      *place_pose = response->place.at(1);
      *postplace_pose = response->place.at(2);

      setOutput("pre_pose", preplace_pose);
      setOutput("pose", place_pose);
      setOutput("post_pose", postplace_pose);
      return NodeStatus::SUCCESS;
    }

    RCLCPP_FATAL(this->logger(), "Could not response grasp pose");
    return NodeStatus::FAILURE;
  }
  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
};

CreateRosNodePlugin(Grasping, "GetGraspPlacePose");