#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtService.hpp"


using namespace BT;
using AddPlanningSceneObjectClient = rbs_skill_interfaces::srv::AddPlanningSceneObject;

class GetPickPlacePose : public BtService<AddPlanningSceneObjectClient>
{
public:
    GetPickPlacePose(const std::string & name, const BT::NodeConfiguration & config)
    : BtService<AddPlanningSceneObjectClient>(name, config) {
        RCLCPP_INFO(_node->get_logger(), "Start the node");
    }

    AddPlanningSceneObjectClient::Request::SharedPtr populate_request() override
    {
        auto request = std::make_shared<AddPlanningSceneObjectClient::Request>();
        RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
        object_name_ = getInput<std::string>("ObjectName").value();
        auto place_pose_ = getInput<std::vector<double>>("pose").value();
        request->object_id = object_name_;
        request->object_pose = place_pose_;

        return request;
    }

    BT::NodeStatus handle_response(AddPlanningSceneObjectClient::Response::SharedPtr response) override
    {
        RCLCPP_INFO(_node->get_logger(), "Response %d", response->success);
        return BT::NodeStatus::SUCCESS;
    }

    static PortsList providedPorts() {
        return providedBasicPorts({
            InputPort<std::string>("ObjectName"),
            InputPort<std::vector<double>>("pose"),
        });
    }
private:
    std::string object_name_{};
};

BT_REGISTER_NODES(factory) {
  factory.registerNodeType<GetPickPlacePose>("AddPlanningSceneObject");
}