//
// Created by bastian on 26.03.22.
//

#ifndef BUILD_ACTORMOVEMENT_H
#define BUILD_ACTORMOVEMENT_H

#include <behaviortree_cpp_v3/action_node.h>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <ros_actor_action_server_msgs/action/movement.hpp>
#include <memory>
#include <cstdlib>

using Movement = ros_actor_action_server_msgs::action::Movement;
using namespace rclcpp_action;

class ActorMovement : public BT::StatefulActionNode {
    public:
        ActorMovement(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName, std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback);

        static BT::PortsList providedPorts();

        BT::NodeStatus onStart() override;

        BT::NodeStatus onRunning() override;

        void onHalted() override;

    private:
        std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback;
        BT::NodeStatus result;
        std::mutex mutex;
        std::shared_ptr<Client<Movement>> client;
        //rclcpp::Client<ros_actor_action_server_msgs::action::Movement>::FutureAndRequestId currentRequest;
};


#endif //BUILD_ACTORMOVEMENT_H
