#pragma once

#include "rm_service.h"
#include "rm_define.h"
#include "interfaces/msg/arm_motion_params.hpp"
#include "arm_action_define.h"
#include <rclcpp/rclcpp.hpp>
#include <atomic>
#include <mutex>
#include <string>
#include <vector>

namespace dual_arm_action_server
{

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 execute_motion(
    const interfaces::msg::ArmMotionParams & params,
    uint8_t arm_id,
    int velocity_scaling,
    int blend_radius,
    std::string * error_msg);

  bool get_current_joints(
    std::vector<double> & joints_left,
    std::vector<double> & joints_right);

  int clear_joint_error(uint8_t arm_id, int joint_num);
  int get_joint_error_and_brake(
    uint8_t arm_id,
    std::vector<uint16_t> & err_flag,
    std::vector<uint16_t> & brake_state);
  int inverse_kinematics(
    uint8_t arm_id,
    const rm_pose_t & pose,
    std::vector<double> & joint_angles);

  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);

  void update_joints(const rm_realtime_arm_joint_state_t & data);
  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_speed_percent(int percent);
  static int clamp_blend_radius_percent(int percent);

  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 joints_mutex_;
  std::vector<double> joints_left_;
  std::vector<double> joints_right_;
};

}  // namespace dual_arm_action_server

