#ifndef MG_WHEEL_INTERFACE_HPP_
#define MG_WHEEL_INTERFACE_HPP_
#include <vector>
#include <string>

#include "boost/asio.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "pluginlib/class_list_macros.hpp"

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

namespace toid {
    namespace asio = boost::asio;

    class StepperInterface : public hardware_interface::SystemInterface {
      public:

        CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& info) override;
        CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
        CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override;

        std::vector<hardware_interface::StateInterface>   export_state_interfaces() override;
        std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

        hardware_interface::return_type read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override;
        hardware_interface::return_type write(const rclcpp::Time& /*time*/,
                                              const rclcpp::Duration& /*period*/) override;

      private:
        std::string serial_port_name_;
        asio::io_context io_context_;
        asio::serial_port serial_port_ = asio::serial_port(io_context_);

        double left_wheel_vel_cmd_    = 0;
        double left_wheel_pos_state_  = 0;
        double right_wheel_vel_cmd_   = 0;
        double right_wheel_pos_state_ = 0;
    };
}

PLUGINLIB_EXPORT_CLASS(toid::StepperInterface, hardware_interface::SystemInterface)

#endif