//
// Created by sebastian on 04.05.20.
//
#include <ros/ros.h>

#include <std_msgs/String.h>
#include <geometry_msgs/Pose.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include "panda_teaching/GetCollectedPoses.h"
#include "panda_teaching/ComputeError.h"

void printError(){

}

void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {

    if (msg->data.compare("replay") == 0) {
        std::string planningGroup;
        if (!node_handle.getParam("planning_group", planningGroup)) {
            ROS_ERROR("Could not get string value for panda_teaching/planning_group from param server");
            return;
        }

        moveit::planning_interface::MoveGroupInterface move_group(planningGroup);
        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
        std::vector<geometry_msgs::Pose> poses;

        ros::ServiceClient client = node_handle.serviceClient<panda_teaching::GetCollectedPoses>("GetCollectedPoses");
        panda_teaching::GetCollectedPoses srv;

        if (client.call(srv)) {
            for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
                poses.push_back(srv.response.pose_array.poses[i]);
            }
        } else {
            ROS_ERROR("Failed to fetch saved poses");
            return;
        }

        for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {

            //move_group.setStartState(*move_group.getCurrentState());
            geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i];
            move_group.setPoseTarget(another_pose);

            moveit::planning_interface::MoveGroupInterface::Plan my_plan;
            bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
            ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED");
            move_group.move();

            ros::ServiceClient client = node_handle.serviceClient<panda_teaching::ComputeError>("errorChecker");
            panda_teaching::ComputeError srv;
            srv.request.poseNumber = i;

            if (!client.call(srv)) {
                ROS_ERROR("Failed to call errorChecker.");
                return;
            }
        }
    } else {
        ROS_ERROR_STREAM("pose_replayer received unknown panda_teaching/poseReplayer message " << msg->data);
    }
}

int main(int argc, char **argv) {

    ros::init(argc, argv, "pose_replayer");
    ros::NodeHandle node_handle("panda_teaching");

    ros::AsyncSpinner spinner(2);
    spinner.start();

    ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseReplayer", 1000, boost::bind(&replayCallback, _1,
                                                                                                    boost::ref(
                                                                                                            node_handle)));
    ros::waitForShutdown();
    return 0;
}