// Copyright [2023] bill-finger
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"

#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
// #include "Eigen/Geometry"

using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;

class MoveToPose : public BtAction<MoveToPoseAction> {
 public:
  MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
      : BtAction<MoveToPoseAction>(name, config) {}

  MoveToPoseAction::Goal populate_goal() override {
    getInput<std::string>("robot_name", robot_name_);
    getInput<geometry_msgs::msg::Pose>("pose", pose_des);
    RCLCPP_INFO(_node->get_logger(),
                "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
                "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
                "orientation.z: %f \n\t orientation.w: %f",
                pose_des.position.x, pose_des.position.y, pose_des.position.z,
                pose_des.orientation.x, pose_des.orientation.y,
                pose_des.orientation.z, pose_des.orientation.w);

    auto goal = MoveToPoseAction::Goal();
    RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]",
                robot_name_.c_str());

    goal.robot_name = robot_name_;
    goal.target_pose = pose_des;
    goal.end_effector_acceleration = 0.5;
    goal.end_effector_velocity = 0.5;

    RCLCPP_INFO(_node->get_logger(), "Goal populated");

    return goal;
  }

  static BT::PortsList providedPorts() {
    return providedBasicPorts(
        {BT::InputPort<std::string>("robot_name"),
         BT::InputPort<geometry_msgs::msg::Pose>("pose")});
  }

private:
  std::string robot_name_;
  geometry_msgs::msg::Pose pose_des;
};  // class MoveToPose

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