#include "behaviortree_ros2/bt_action_node.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose_with_constraints.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>

using namespace BT;
using MoveitSendPoseWithConstraintAction =
    rbs_skill_interfaces::action::MoveitSendPoseWithConstraints;

class MoveToPoseOrientationConstraint
    : public RosActionNode<MoveitSendPoseWithConstraintAction> {
public:
  MoveToPoseOrientationConstraint(const std::string &name,
                                  const NodeConfig &conf,
                                  const RosNodeParams &params)
      : RosActionNode<MoveitSendPoseWithConstraintAction>(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>("x_tol"), BT::InputPort<double>("y_tol"),
         BT::InputPort<double>("z_tol"), BT::InputPort<double>("weight"),
         BT::InputPort<double>("duration")});
  }

  bool setGoal(RosActionNode::Goal &goal) override {
    RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
                this->action_name_.c_str());

    getInput("x_tol", goal.orientation_constraint.absolute_x_axis_tolerance);
    getInput("y_tol", goal.orientation_constraint.absolute_y_axis_tolerance);
    getInput("z_tol", goal.orientation_constraint.absolute_z_axis_tolerance);
    getInput("weight", goal.orientation_constraint.weight);
    getInput("robot_name", goal.robot_name);
    getInput("pose", m_target_pose);
    getInput("duration", goal.duration);
    goal.target_pose = *m_target_pose;
    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(MoveToPoseOrientationConstraint,
                    "MtpOrientationConstraint");