#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 "geometry_msgs/msg/transform_stamped.h"
#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"

#include "tf2_ros/transform_listener.h"

using namespace std::chrono_literals;

class MoveitInterface : public rclcpp::Node
{
  public:
    MoveitInterface()
    : Node("moveit_interface"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_), 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);
      move_group_->setStartStateToCurrentState();
      // 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, "Franka_IML_Experiment", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);

      // TF2
      tf2_ros::Buffer tf_buffer_{this->get_clock()};
      tf2_ros::TransformListener tf_listener_{tf_buffer_};
      tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&MoveitInterface::compute_endeffector_pose, this));

    }

  private:
    void compute_endeffector_pose()
    {
      try
      {
        geometry_msgs::msg::TransformStamped transform_stamped;
        transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0));
        
        geometry_msgs::msg::PoseStamped current_pose;
        current_pose.header.frame_id = "panda_link8";
        current_pose.header.stamp = transform_stamped.header.stamp;
        //Position
        current_pose.pose.position.x = transform_stamped.transform.translation.x;
        current_pose.pose.position.y = transform_stamped.transform.translation.y;
        current_pose.pose.position.z = transform_stamped.transform.translation.z;
        // Orientation
        current_pose.pose.orientation.x = transform_stamped.transform.rotation.x;
        current_pose.pose.orientation.y = transform_stamped.transform.rotation.y;
        current_pose.pose.orientation.z = transform_stamped.transform.rotation.z;
        current_pose.pose.orientation.w = transform_stamped.transform.rotation.w;
        pose_publisher_->publish(current_pose);
      }
      catch (tf2::TransformException &ex)
      {
        RCLCPP_WARN(this->get_logger(), "%s", ex.what());
      }
    }

    void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg)
    {
      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_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    rclcpp::TimerBase::SharedPtr tf2_timer_;

    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;
}