// https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/examples/moveit_cpp/src/moveit_cpp_tutorial.cpp

// https://moveit.picknik.ai/foxy/doc/move_group_interface/move_group_interface_tutorial.html
// https://moveit.picknik.ai/foxy/doc/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html
// https://docs.ros.org/en/indigo/api/moveit_core/html/classmoveit_1_1core_1_1RobotState.html
// https://docs.ros.org/en/kinetic/api/moveit_core/html/classmoveit_1_1core_1_1JointModelGroup.html



#include <rclcpp/rclcpp.hpp>
#include <string>
#include <thread>

// MoveitCpp
#include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/rdf_loader/rdf_loader.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include "geometry_msgs/msg/pose_stamped.hpp"

const std::string PLANNING_GROUP = "ar3_arm";

class ScanNPlan : public rclcpp::Node {
public:
    ScanNPlan() : Node("scan_n_plan") {
        callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);

        rclcpp::SubscriptionOptions options;
        options.callback_group = callback_group_;

        // Create /move_pose subscriber
        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "move_pose",
            rclcpp::QoS(1),
            std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1),
            options
        );

        relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "move_relative_pose",
            rclcpp::QoS(1),
            std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1),
            options
        );
    }

    void init() {
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
                shared_from_this(), std::string("ar3_arm"));
    }

private:
    void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
    	RCLCPP_INFO(this->get_logger(), "GOT MSG");

        move_group_->setStartStateToCurrentState();
        move_group_->setPoseTarget(p->pose);

        moveit::planning_interface::MoveGroupInterface::Plan plan;
        bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

        RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");

        move_group_->move();
    }

    void relativeMoveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
    	RCLCPP_INFO(this->get_logger(), "GOT MSG");

        moveit::core::RobotStatePtr robot_state = move_group_->getCurrentState();
        const Eigen::Isometry3d &end_effector_state = robot_state->getGlobalLinkTransform(move_group_->getEndEffectorLink());
        RCLCPP_INFO_STREAM(this->get_logger(), "POS: \n" << end_effector_state.translation() << "\n");
        RCLCPP_INFO_STREAM(this->get_logger(), "ROT: \n" << end_effector_state.rotation() << "\n");

        geometry_msgs::msg::Pose target_pose;
        target_pose.position.x = 0.24;
        target_pose.position.y = -0.32;
        target_pose.position.z = 0.56;
        move_group_->setPoseTarget(target_pose);

        moveit::planning_interface::MoveGroupInterface::Plan plan;
        bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

        RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");

        move_group_->move();
    }
private:
    rclcpp::CallbackGroup::SharedPtr callback_group_;

    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;

    moveit::planning_interface::MoveGroupInterfacePtr move_group_;
    moveit::core::RobotModelConstPtr robot_model_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    auto node = std::make_shared<ScanNPlan>();
    RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");

    rclcpp::executors::MultiThreadedExecutor executor;
    node->init();
    executor.add_node(node);
    executor.spin();
    // rclcpp::spin(node);

    RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
    rclcpp::shutdown();
    return 0;
}

