#include "behaviortree_ros2/bt_action_node.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include <behaviortree_cpp/basic_types.h>
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include <rclcpp/logging.hpp>

using namespace BT;
using MoveitSendPoseAction = rbs_skill_interfaces::action::MoveitSendPose;

class MoveToPose : public RosActionNode<MoveitSendPoseAction> {
public:
  MoveToPose(const std::string &name, const NodeConfig &conf,
             const RosNodeParams &params)
      : RosActionNode<MoveitSendPoseAction>(name, conf, params) {

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

  static BT::PortsList providedPorts() {
    return providedBasicPorts({
        BT::InputPort<std::string>("robot_name"),
        BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
        BT::InputPort<double>("duration"),
        BT::InputPort<bool>("try_plan_untill_success", false, "Replan if failure to plan"),
        BT::InputPort<double>("allowed_planning_time", 5.0, "Maximal time to plan"),
    });
  }

  bool setGoal(RosActionNode::Goal &goal) override {
    RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
                this->action_name_.c_str());
    if (!getInput("robot_name", goal.robot_name)) {
      RCLCPP_FATAL(this->logger(),
                   "[%s] Could not get robot_name from input port",
                   name().c_str());
      return false;
    }
    if (!getInput("pose", m_target_pose)) {
      RCLCPP_FATAL(this->logger(),
                   "[%s] Could not get target_pose from input port",
                   name().c_str());
      return false;
    }
    if (!getInput("duration", goal.duration)) {
      RCLCPP_FATAL(this->logger(),
                   "[%s] Could not get duration from input port",
                   name().c_str());
      return false;
    }

    RCLCPP_INFO_STREAM(
        this->logger(),
        "[" << name().c_str() << "]" << "Got target_pose: \nPosition [ "
            << m_target_pose->position.x << ", " << m_target_pose->position.y
            << ", " << m_target_pose->position.z << " ];\n"
            << "Orientation: [ " << m_target_pose->orientation.x << ", "
            << m_target_pose->orientation.y << ", "
            << m_target_pose->orientation.z << ", "
            << m_target_pose->orientation.w << "] ");

    goal.target_pose = *m_target_pose;
    getInput("try_plan_untill_success", goal.try_plan_untill_success);
    getInput("allowed_planning_time", goal.allowed_planning_time);
    return true;
  }

  NodeStatus onResultReceived(const WrappedResult &wr) override {

    RCLCPP_INFO(this->logger(), "[%s] Starting get response %s with status %b",
                name().c_str(), this->action_name_.c_str(), wr.result->success);
    if (!wr.result->success) {
      return NodeStatus::FAILURE;
    }
    return NodeStatus::SUCCESS;
  }

private:
  std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
};

CreateRosNodePlugin(MoveToPose, "MoveToPose");