#ifndef ROS2_CONTROL_CAN_REAL_ROBOT_H
#define ROS2_CONTROL_CAN_REAL_ROBOT_H

#include <cstring>
#include "rclcpp/rclcpp.hpp"

#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <rclcpp_lifecycle/state.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>

#include "config.h"
#include "wheel.h"
#include "can1__main_ft24.h"

using hardware_interface::CallbackReturn;
using hardware_interface::return_type;

namespace ros2_control_can
{

class Ros2ControlCAN : public hardware_interface::SystemInterface
{


public:
  Ros2ControlCAN();

  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;

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

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

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

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

  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;

  return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:

  Config cfg_;

  Wheel f_l_wheel_;
  Wheel f_r_wheel_;
  Wheel r_l_wheel_;
  Wheel r_r_wheel_;

  can1__main_ft24_jetson_tx_t tx_frame;
  can1__main_ft24_jetson_rx_t rx_frame;
  uint8_t can_buffer[64];

  rclcpp::Logger logger_;

  std::chrono::time_point<std::chrono::system_clock> time_;

  int socket_instance;
  
};
} // namespace ros2_control_can

#endif // ROS2_CONTROL_CAN_REAL_ROBOT_H