//
// Created by bastian on 28.03.22.
//

#ifndef BUILD_MOVECONNECTION_H
#define BUILD_MOVECONNECTION_H

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <rclcpp_action/client.hpp>

class MoveConnection {
private:
    enum {IDLE,RUNNING,SPEED_CHANGE} state = IDLE;
    moveit::planning_interface::MoveGroupInterface moveGroup;
    std::shared_ptr<geometry_msgs::msg::Pose> lastTarget = nullptr;
    std::function<void(bool)> lastCallback = [](__attribute__((unused))bool finished) {};
    double currentSpeed = 1;
    std::mutex lock;
public:
    MoveConnection(const std::shared_ptr<rclcpp::Node>& node, const std::string& planningGroup);

    void planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,const std::function<void(bool)> &callback);

    void stop();

    void setSpeedMultiplier(double speed);
};


#endif //BUILD_MOVECONNECTION_H
