#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "geometry_msgs/msg/pose_array.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_utils/rbs_utils.hpp"
#include <behaviortree_cpp_v3/bt_factory.h>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>

using MoveToPoseArrayAction = rbs_skill_interfaces::action::MoveitSendPose;

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

  MoveToPoseArrayAction::Goal populate_goal() override {
    auto goal = MoveToPoseArrayAction::Goal();
    getInput<std::string>("robot_name", robot_name_);
    getInput<geometry_msgs::msg::PoseArray>("pose_vec_in", target_pose_vec_);
    for (auto &point : target_pose_vec_.poses) {
      RCLCPP_INFO(_node->get_logger(), "Pose array: \n %f \n %f \n %f",
                  point.position.x, point.position.y, point.position.z);
    }
    if (!target_pose_vec_.poses.empty()) {
      goal.robot_name = robot_name_;
      goal.target_pose = target_pose_vec_.poses.at(0);

      target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
      setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out",
                                               target_pose_vec_);
    } else {
      RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
    }

    return goal;
  }

  static BT::PortsList providedPorts() {
    return providedBasicPorts(
        {BT::InputPort<std::string>("robot_name"),
         BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
         BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")});
  }

private:
  std::string robot_name_;
  geometry_msgs::msg::PoseArray target_pose_vec_;
};

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