#ifndef ROS2_CONTROL_CAN_FAKE_ROBOT_H
#define ROS2_CONTROL_CAN_FAKE_ROBOT_H


#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"

#if !OLD_ROS_CONTROL_SIGNATURE
using hardware_interface::CallbackReturn;
#endif
using hardware_interface::return_type;

class FakeRobot : public hardware_interface::SystemInterface
{


public:
  FakeRobot();

  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;

  #if OLD_ROS_CONTROL_SIGNATURE
  CallbackReturn on_activate(const rclcpp_lifecycle::State & /* previous_state */, const rclcpp_lifecycle::State & /* current_state */)
  #else
  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
  #endif

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

  #if OLD_ROS_CONTROL_SIGNATURE
  return_type read() override;
  #else
  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
  #endif

  #if OLD_ROS_CONTROL_SIGNATURE
  return_type write() override;
  #else
  return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
  #endif


private:

  Config cfg_;

  Wheel l_wheel_;
  Wheel r_wheel_;

  rclcpp::Logger logger_;

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


#endif // ROS2_CONTROL_CAN_FAKE_ROBOT_H