#pragma once

#include "rm_define.h"
#include "rm_service.h"
#include <rclcpp/rclcpp.hpp>
#include <atomic>
#include <mutex>
#include <string>

namespace dual_arm_remote_teleop
{

class RealRobotDriver
{
public:
  RealRobotDriver();
  ~RealRobotDriver();

  bool initialize(
    const rclcpp::Logger & logger,
    const std::string & left_ip,
    const std::string & right_ip,
    int arm_port,
    const std::string & left_push_ip,
    const std::string & right_push_ip,
    int push_port,
    int push_cycle);

  void shutdown();
  bool is_connected() const;

  bool send_pose_canfd(
    uint8_t arm_id,
    const rm_pose_t & pose,
    bool follow,
    int trajectory_mode,
    int smooth_ratio,
    std::string * error_msg);

  void stop_arm(uint8_t arm_id);
  void stop_all();

private:
  static void realtime_state_callback(rm_realtime_arm_joint_state_t data);
  static void api_log_callback(const char * message, va_list args);
  bool init_handle(
    rm_robot_handle ** handle,
    const std::string & ip,
    int port,
    const std::string & push_ip,
    int push_port,
    int push_cycle);
  rm_robot_handle * handle_for(uint8_t arm_id) const;
  static int clamp_smooth_ratio(int smooth_ratio);

  static RealRobotDriver * instance_;
  rclcpp::Logger logger_;
  std::unique_ptr<RM_Service> rm_service_;
  rm_robot_handle * left_handle_;
  rm_robot_handle * right_handle_;
  std::atomic<bool> connected_;
  std::string left_ip_;
  std::string right_ip_;
  std::mutex api_mutex_;
};

}  // namespace dual_arm_remote_teleop

