#include "std_msgs/String.h"
#include <moveit/move_group_interface/move_group_interface.h>


void move_toCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("INTRO PACKAGE: Recevied message: '%s'", msg->data.c_str());

    static const std::string PLANNING_GROUP = "panda_arm";
    moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);

    ros::Duration(3.0).sleep();

    group.setStartStateToCurrentState();
    group.setPlanningTime(5.0);

    // pose A
    geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
    target_pose_1.position.z = 0.4;
    target_pose_1.position.y = 0.4;
    target_pose_1.position.x = 0.8;
    target_pose_1.orientation.x = 0;
    target_pose_1.orientation.y = 0;
    target_pose_1.orientation.z = 0;
    target_pose_1.orientation.w = 1;

    // pose B
    geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
    target_pose_2.position.z = 0.4;
    target_pose_2.position.y = 0.4;
    target_pose_2.position.x = 0.8;
    target_pose_2.orientation.x = 0;
    target_pose_2.orientation.y = 0;
    target_pose_2.orientation.z = 0;
    target_pose_2.orientation.w = 0;

    std::vector<geometry_msgs::Pose> waypoints;
    std::string strA ("A");
    std::string strB ("B");
    if (strA == msg->data) {
        waypoints.push_back(target_pose_1);
    } else if (strB == msg->data) {
        waypoints.push_back(target_pose_2);
    } else {
        ROS_WARN("INTRO PACKAGE: Wrong message.");
        return;
    }

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit_msgs::RobotTrajectory trajectory_msg;
    group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);
    plan.trajectory_ = trajectory_msg;
    group.execute(plan);
    ROS_INFO("INTRO PACKAGE: Finished movement.");

}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "intro_package");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);

    spinner.start();

    ros::Subscriber sub = node_handle.subscribe("move_to", 1000, move_toCallback);
    ROS_INFO("INTRO PACKAGE: Listening to 'move_to'.");

    ros::waitForShutdown();

    return 0;
}