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

#include "libserial/SerialPort.h"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/system_interface.hpp"
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "pluginlib/class_list_macros.hpp"

namespace mg {
    class MgStepperInterface : public hardware_interface::SystemInterface {
      public:
        CallbackReturn on_init(const hardware_interface::HardwareInfo& 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;
        LibSerial::SerialPort odrive_serial_interface;

        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(mg::MgStepperInterface, hardware_interface::SystemInterface)

#endif