#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include <Eigen/Geometry>

#include <rclcpp/rclcpp.hpp>
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

#include "std_msgs/msg/empty.hpp"

#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit_visual_tools/moveit_visual_tools.h"
#include "moveit/robot_model_loader/robot_model_loader.h"

using namespace std::chrono_literals;

class MoveitInterface : public rclcpp::Node
{
  public:
    MoveitInterface()
    : Node("moveit_interface"), trajectory_set(false)
    {
      publisher_ = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("moveit_interface/joint_space_trajectory", 10);
      pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("moveit_interface/current_pose", 10);
      subscriber_ = this->create_subscription<geometry_msgs::msg::PoseArray>("moveit_interface/task_space_trajectory", 10, std::bind(&MoveitInterface::task_space_callback, this, std::placeholders::_1));
      execute_sub_ = this->create_subscription<std_msgs::msg::Empty>("moveit_interface/execution", 10, std::bind(&MoveitInterface::execution_callback, this, std::placeholders::_1));
      
      // Create a shared instance of the current node
      auto shared_this = std::shared_ptr<rclcpp::Node>(this);
      
      // Connecting to moveit
      static const std::string PLANNING_GROUP = "panda_arm";
      move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(shared_this), PLANNING_GROUP);

      // Connection to Rviz
      visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(std::shared_ptr<rclcpp::Node>(shared_this), "panda_link0", "move_group", move_group_->getRobotModel());
      Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
      text_pose.translation().z()=1.0;
      visual_tools_->publishText(text_pose, "MoveGroupInterface_Demo", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
    }

  private:
    void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg)
    {
      geometry_msgs::msg::PoseStamped current_pose = move_group_->getCurrentPose();
      pose_publisher_->publish(current_pose);

      std::vector<geometry_msgs::msg::Pose> waypoints = msg->poses;

      
      const double jump_threshold = 0.0;
      const double eef_step = 0.001;
      double fraction = move_group_->computeCartesianPath(waypoints, eef_step,jump_threshold, trajectory);
      RCLCPP_INFO(logger, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);

      visual_tools_->deleteAllMarkers();
      visual_tools_->publishText(text_pose, "Cartesian_Path", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
      visual_tools_->publishPath(waypoints, rviz_visual_tools::LIME_GREEN, rviz_visual_tools::SMALL);

      trajectory_set = true;


      publisher_->publish(trajectory);
    }
    void execution_callback(const std_msgs::msg::Empty::SharedPtr /*msg*/)
    {
      if(trajectory_set)
      {
        move_group_->execute(trajectory);
        trajectory_set=false;
      }
      else
      {
        RCLCPP_WARN(logger, "Trajectroy is not set. Unable to execute.");
      }
    }
    rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr publisher_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
    rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr subscriber_;
    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr execute_sub_;
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
    std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;

    Eigen::Isometry3d text_pose;
    moveit_msgs::msg::RobotTrajectory trajectory;
    rclcpp::Logger logger = this->get_logger();
    bool trajectory_set;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MoveitInterface>());
  rclcpp::shutdown();
  return 0;
}