#include "moveit/move_group_interface/move_group_interface.h"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_servers/base_skill.hpp"

namespace rbs_skill_actions {

using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;

class MoveitMtp : public SkillBase<MoveitSendPose> {
public:
  explicit MoveitMtp(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
      : SkillBase("mtp_moveit", options) {}

private:
  std::string requiredActionController() override {
    return "joint_trajectory_controller";
  }

  void executeAction() override {
    RCLCPP_INFO(this->get_logger(), "Executing action goal");

    moveit::planning_interface::MoveGroupInterface move_group_interface(
        this->shared_from_this(), m_current_goal->robot_name);
    move_group_interface.startStateMonitor();

    move_group_interface.setPoseTarget(m_current_goal->target_pose);
    move_group_interface.setMaxVelocityScalingFactor(
        m_current_goal->end_effector_velocity);
    move_group_interface.setMaxAccelerationScalingFactor(
        m_current_goal->end_effector_acceleration);
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::core::MoveItErrorCode plan_err_code =
        move_group_interface.plan(plan);
    if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) {
      move_group_interface.plan(plan);
    }
    if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
      RCLCPP_INFO(this->get_logger(), "Planning success");
      // move_group_interface.execute(plan);
      moveit::core::MoveItErrorCode move_err_code =
          move_group_interface.execute(plan);
      if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
        m_current_result->success = true;
        m_current_goal_handle->succeed(m_current_result);
        RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
      } else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) {
        RCLCPP_ERROR(this->get_logger(), "Failure in move:");
      }

    } else {
      RCLCPP_WARN_STREAM(this->get_logger(),
                         "Failed to generate plan because "
                             << moveit::core::errorCodeToString(plan_err_code));
      m_current_goal_handle->abort(m_current_result);
    }

    if (m_current_goal_handle->is_canceling()) {
      m_current_goal_handle->canceled(m_current_result);
      RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
      return;
    }
  }
}; // class MoveToPoseActionServer

} // namespace rbs_skill_actions

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveitMtp);