#include "behaviortree_ros2/bt_service_node.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include <memory>
#include <rclcpp/parameter_client.hpp>

using namespace BT;
using namespace std::chrono_literals;
using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;

class ObjectDetection : public RosServiceNode<ObjDetectionSrv> {
public:
  ObjectDetection(const std::string &name, const NodeConfig &conf,
                  const RosNodeParams &params)
      : RosServiceNode<ObjDetectionSrv>(name, conf, params) {
    m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
        node_.lock(), "/object_detection");
    while (!m_params_client->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(logger(),
                     "Interrupted while waiting for the service. Exiting.");
        break;
      }
    }
    getInput("SettingPath", m_settings_path);
  }

  static PortsList providedPorts() {
    return providedBasicPorts({
        InputPort<std::string>("ReqKind"),
        InputPort<std::string>("SettingPath"),
    });
  }

  bool setRequest(Request::SharedPtr &request) override {
    getInput("ReqKind", m_req_type);
    auto transition = transition_event(m_req_type);
    request->set__transition(transition);
    return true;
  }

  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
    if (!response->success) {
      return NodeStatus::FAILURE;
    }
    return NodeStatus::SUCCESS;
  }

private:
  uint8_t transition_id_{};
  std::string m_settings_path{};
  //   std::string _model_path{};
  std::string m_req_type{};
  std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
  std::vector<rcl_interfaces::msg::Parameter> _params;
  rcl_interfaces::msg::Parameter m_param;

  lifecycle_msgs::msg::Transition
  transition_event(const std::string &req_type) {
    lifecycle_msgs::msg::Transition translation{};
    if (req_type == "configure") {
      set_str_param();
      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
    } else if (req_type == "activate") {
      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
    } else if (req_type == "deactivate") {
      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
    } else if (req_type == "cleanup") {
      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
    }
    translation.set__id(transition_id_);
    return translation;
  }

  void set_str_param() {
    RCLCPP_INFO_STREAM(logger(),
                       "Set string parameter: <" + m_settings_path + ">");

    std::vector<rclcpp::Parameter> t_parameters{
        rclcpp::Parameter("setting_path", m_settings_path)};
    m_params_client->set_parameters(t_parameters);
  }
};

CreateRosNodePlugin(ObjectDetection, "ObjectDetection");