#ifndef ARM_KEYBOARD_NODE_HPP
#define ARM_KEYBOARD_NODE_HPP

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/arm.hpp"
#include "arm_action_define.h"
#include "rm_define.h"

using ArmAction = interfaces::action::Arm;
using GoalClientArm = rclcpp_action::Client<ArmAction>;
using GoalHandleArm = rclcpp_action::ClientGoalHandle<ArmAction>;

class ArmKeyboardNode: public rclcpp::Node
{
public:
    ArmKeyboardNode();
    ~ArmKeyboardNode();
    static rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr leftArmAimPub_;
    static rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr rightArmAimPub_;
    std::atomic<bool> exit_flag;
    GoalClientArm::SharedPtr client_ptr_;
    void SendAngleSteppingGoal(const char *command);
    void SendAngleMovingGoal(const float *values, int armId);
    void SendDirectSteppingGoal(const char *command);
    void SendDirectMovingGoal(const float *values, int armId);
    void SendDirectSteppingGoal2(const float *values, int armId);
    void StopGoal();
private:
    std::shared_mutex rw_mutex_;
    void goal_response_callback(GoalHandleArm::SharedPtr goal_handle);
    void feedback_callback(
      GoalHandleArm::SharedPtr goal_handle,
      const std::shared_ptr<const ArmAction::Feedback> feedback);
    void result_callback(const GoalHandleArm::WrappedResult & result);

    rclcpp_action::Client<ArmAction>::SendGoalOptions goal_options_;

    double leftArmAngleAim_[USED_ARM_DOF];
    double rightArmAngleAim_[USED_ARM_DOF];

    int64_t leftCommandId_;
    int64_t rightCommandId_;

    int leftDirectCommand_;
    int rightDirectCommand_;

    GoalHandleArm::SharedPtr leftArmGoalHandle_;
    GoalHandleArm::SharedPtr rightArmGoalHandle_;
};

#endif // ARM_KEYBOARD_NODE_HPP