#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"
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
#include <rclcpp/logging.hpp>

namespace rbs_skill_actions {

using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;

class MoveitMtpCart : public SkillBase<MoveitSendPose> {
public:
  explicit MoveitMtpCart(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
      : SkillBase("mtp_moveit_cart", 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();

    moveit::core::RobotState start_state(
        *move_group_interface.getCurrentState());

    std::vector<geometry_msgs::msg::Pose> waypoints;
    auto current_pose = move_group_interface.getCurrentPose();
    geometry_msgs::msg::Pose target_pose = m_current_goal->target_pose;
    waypoints.push_back(target_pose);

    RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
                target_pose.position.x, target_pose.position.y,
                target_pose.position.z);

    moveit_msgs::msg::RobotTrajectory trajectory;
    const double jump_threshold = 0.0;
    const double eef_step = 0.001;
    double fraction = move_group_interface.computeCartesianPath(
        waypoints, eef_step, jump_threshold, trajectory);

    robot_trajectory::RobotTrajectory rt(
        move_group_interface.getCurrentState()->getRobotModel(),
        m_current_goal->robot_name);

    rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);

    trajectory_processing::TimeOptimalTrajectoryGeneration tp;

    bool su = tp.computeTimeStamps(rt);
    if (!su) {
      RCLCPP_ERROR(this->get_logger(), "Failed to compute timestamp of trajectory");
      m_current_goal_handle->abort(m_current_result);
    }

    rt.getRobotTrajectoryMsg(trajectory);

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    plan.trajectory = trajectory;

    if (fraction > 0) {
      RCLCPP_INFO(this->get_logger(), "Planning success");
      moveit::core::MoveItErrorCode execute_err_code =
          move_group_interface.execute(plan);
      if (execute_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 (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {
        RCLCPP_ERROR(this->get_logger(), "Failure in move:");
      }

      // move_group_interface.move();
    } else {
      RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
      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::MoveitMtpCart);