#include <behavior_tree/BtService.hpp>

#include <behaviortree_cpp_v3/basic_types.h>
#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>
#include "rbs_utils/rbs_utils.hpp"
#include "env_manager_interfaces/srv/start_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"

using EnvStarterService = env_manager_interfaces::srv::StartEnv;

class EnvManagerStarter : public BtService<EnvStarterService> {
public:
  EnvManagerStarter(const std::string &name,
                    const BT::NodeConfiguration &config)
      : BtService<EnvStarterService>(name, config) {
    RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
  }

  EnvStarterService::Request::SharedPtr populate_request() override {
    auto request = std::make_shared<EnvStarterService::Request>();
    std::string env_name = getInput<std::string>("env_name").value();
    std::string env_class = getInput<std::string>("env_class").value();
    request->name = env_name;
    request->type = env_class;

    return request;
  }

  BT::NodeStatus handle_response(
      const EnvStarterService::Response::SharedPtr response) override {
    if (response->ok) {
      // TODO We need better perfomance for call it in another place for all BT nodes
      // - Make it via shared_ptr forward throught blackboard
      auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
      auto p = t->getWorkspaceInspectorTrajectory();
      setOutput("workspace", p);
      return BT::NodeStatus::SUCCESS;
    }
    return BT::NodeStatus::FAILURE;
  }

  static BT::PortsList providedPorts() {
    return providedBasicPorts({
        BT::InputPort<std::string>("env_name"),
        BT::InputPort<std::string>("env_class"),
        BT::OutputPort<geometry_msgs::msg::PoseArray>("workspace"),
    });
  }
};

#include "behaviortree_cpp_v3/bt_factory.h"

BT_REGISTER_NODES(factory) {
  factory.registerNodeType<EnvManagerStarter>("EnvStarter");
}