#ifndef RM_ARM_NODE_HPP
#define RM_ARM_NODE_HPP

#include <semaphore.h>
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/arm.hpp"
#include <std_msgs/msg/string.hpp>
#include "arm_driver_define.hpp"
#include "rm_arm_handler.hpp"

using namespace std::chrono_literals;
using std::placeholders::_1;

using ArmAction = interfaces::action::Arm;
using GoalHandleArm = rclcpp_action::ServerGoalHandle<ArmAction>;

class RmArmNode: public rclcpp::Node
{
public:
    RmArmNode(ArmDriver *armDriver, Kinematics *kinematics);
    ~RmArmNode();
    static RmArmHandler *leftArmHandler_;
    static RmArmHandler *rightArmHandler_;

    static void CallbackRealtimeArmJointState(ArmStatusCallbackData *data);

private:
    sem_t sem_;
    ArmDriver *armDriver_;
    rclcpp_action::Server<ArmAction>::SharedPtr action_server_;
    static sensor_msgs::msg::JointState armStates;

    rclcpp_action::GoalResponse ArmActionGoal(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const ArmAction::Goal> goal);
    rclcpp_action::CancelResponse ArmActionCancel(
        const std::shared_ptr<GoalHandleArm> goal_handle);
    void ArmActionAccepted(const std::shared_ptr<GoalHandleArm> goal_handle);
    void ArmActionFeedback(int collision);
    void ArmActionExecute();
    void ArmActionFinish(int64_t index, int result);
    void ArmActionCheck();
    void StartNewGoal(RmArmHandler *handle);

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robotDescriptionSub_;
    static rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr armStatesPub_;
    std::unordered_map<int64_t, std::shared_ptr<GoalHandleArm>> goal_handles_;

    void CallbackGetRobotDescription(const std_msgs::msg::String::SharedPtr msg);
};

#endif // RM_ARM_NODE_HPP
