

#ifndef ROBOTIQ_FORWARD_CONTROLLER_HPP
#define ROBOTIQ_FORWARD_CONTROLLER_HPP

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

#include <rclcpp/rclcpp.hpp>
#include "rclcpp/qos.hpp"
#include "rclcpp_lifecycle/lifecycle_node.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/msg/forward_command.hpp"

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

    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::string joint_name_;
    std::vector<std::string> command_interface_types_;
    std::vector<std::string> state_interface_types_;

    // communication interface
    bool new_msg_ = false;
    rclcpp::Subscription<robotiq_2f_msgs::msg::ForwardCommand>::SharedPtr command_subscriber_;
    std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> command_msg_;
    realtime_tools::RealtimeBuffer<std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand>>
    msg_external_ptr_;

    // 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_}};

    double desired_position_;
    double desired_max_velocity_;
    double desired_max_effort_;

};
} // end namespace robotiq_2f_controllers
#endif
