

#ifndef ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP
#define ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP

#include <stddef.h>
#include <memory>
#include <string>
#include <vector>
#include <queue>
#include <mutex>

#include <stdexcept> // For std::runtime_error

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/qos.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp/callback_group.hpp>

#include <controller_interface/controller_interface.hpp>
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>

#include "realtime_tools/realtime_buffer.h"

#include "robotiq_2f_msgs/action/gripper_command.hpp"

using GripperGoalHandle = rclcpp_action::ServerGoalHandle<robotiq_2f_msgs::action::GripperCommand>;

namespace robotiq_2f_controllers
{
class RobotiqGripperCommandController : public controller_interface::ControllerInterface
{
public:
    CONTROLLER_INTERFACE_PUBLIC
    RobotiqGripperCommandController();

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::InterfaceConfiguration command_interface_configuration() const override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::InterfaceConfiguration state_interface_configuration() const override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::return_type update(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::CallbackReturn on_init() override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::CallbackReturn on_cleanup(
    const rclcpp_lifecycle::State & previous_state) override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::CallbackReturn on_error(
    const rclcpp_lifecycle::State & previous_state) override;

    CONTROLLER_INTERFACE_PUBLIC
    controller_interface::CallbackReturn on_shutdown(
    const rclcpp_lifecycle::State & previous_state) override;

protected:
    std::mutex mutex_;
    std::string joint_name_;
    std::vector<std::string> command_interface_types_;
    std::vector<std::string> state_interface_types_;

    // Checker Methods
    bool command_interface_checker_();
    bool is_valid_goal(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
    bool is_stalled(double current_position);
    bool is_stopped();

    // Action Interface
    std::queue<std::shared_ptr<GripperGoalHandle>> goal_queue_;
    std::shared_ptr<GripperGoalHandle> current_goal_handle_;
    rclcpp_action::Server<robotiq_2f_msgs::action::GripperCommand>::SharedPtr action_server_;

    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
         std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GripperGoalHandle> goal_handle);
    void handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle);
    void process_next_goal();
    void execute_goal(const std::shared_ptr<GripperGoalHandle> goal_handle);

    void publish_feedback();

    // Error Related
    bool stop_motion(double current_position);

    // Command Interfaces
    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
    joint_position_command_interface_;
    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
    max_velocity_command_interface_;
    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
    max_effort_command_interface_;

    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
    command_interface_map_ = {
      {"position", &joint_position_command_interface_},
      {"max_velocity", &max_velocity_command_interface_},
      {"max_effort", &max_effort_command_interface_}};

    // State Interfaces
    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
    joint_position_state_interface_;

    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> *>
    state_interface_map_ = {
      {"position", &joint_position_state_interface_}};

    // Addtional State variables
    double max_position_ = 0.0;
    double target_position_ = -1.0;
    double current_position_ = -1.0;
    double last_position_ = -1.0;
    int stall_counter_ = 0;
    bool cancel_requested_ = false;

    // Const
    const int MAX_STALL_COUNT = 200;
    const double POSITION_TOLERANCE = 0.0015;
    const double MIN_SPEED = 0.0;
    const double MIN_EFFORT = 0.0;

};
} // end namespace robotiq_2f_controllers
#endif
