#include "dual_arm_remote_teleop/real_robot_driver.hpp"
#include <algorithm>
#include <cstdarg>
#include <cstdio>
#include <cstring>

namespace dual_arm_remote_teleop
{

RealRobotDriver * RealRobotDriver::instance_ = nullptr;

RealRobotDriver::RealRobotDriver()
: logger_(rclcpp::get_logger("dual_arm_remote_teleop")),
  left_handle_(nullptr),
  right_handle_(nullptr),
  connected_(false)
{
}

RealRobotDriver::~RealRobotDriver()
{
  shutdown();
}

bool RealRobotDriver::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)
{
  logger_ = logger;
  left_ip_ = left_ip;
  right_ip_ = right_ip;

  rm_service_ = std::make_unique<RM_Service>();
  if (!rm_service_) {
    RCLCPP_ERROR(logger_, "Failed to allocate RM_Service");
    return false;
  }

  if (rm_service_->rm_init(RM_TRIPLE_MODE_E) != 0) {
    RCLCPP_ERROR(logger_, "rm_init failed");
    rm_service_.reset();
    return false;
  }

  instance_ = this;
  rm_service_->rm_set_log_call_back(api_log_callback, 3);
  rm_service_->rm_realtime_arm_state_call_back(realtime_state_callback);

  if (!init_handle(&left_handle_, left_ip, arm_port, left_push_ip, push_port, push_cycle)) {
    shutdown();
    return false;
  }

  if (!init_handle(&right_handle_, right_ip, arm_port, right_push_ip, push_port, push_cycle)) {
    shutdown();
    return false;
  }

  connected_.store(true);
  return true;
}

void RealRobotDriver::shutdown()
{
  connected_.store(false);
  if (rm_service_) {
    rm_service_->rm_destroy();
    rm_service_.reset();
  }
  left_handle_ = nullptr;
  right_handle_ = nullptr;
  instance_ = nullptr;
}

bool RealRobotDriver::is_connected() const
{
  return connected_.load();
}

bool RealRobotDriver::send_pose_canfd(
  uint8_t arm_id,
  const rm_pose_t & pose,
  bool follow,
  int trajectory_mode,
  int smooth_ratio,
  std::string * error_msg)
{
  std::lock_guard<std::mutex> lock(api_mutex_);
  rm_robot_handle * handle = handle_for(arm_id);
  if (!handle) {
    if (error_msg) {
      *error_msg = "Invalid arm_id or missing robot handle";
    }
    return false;
  }

  const int ratio = clamp_smooth_ratio(smooth_ratio);
  const int result = rm_service_->rm_movep_canfd(
    handle,
    pose,
    follow,
    trajectory_mode,
    ratio);
  if (result != 0) {
    if (error_msg) {
      *error_msg = "rm_movep_canfd failed with code " + std::to_string(result);
    }
    return false;
  }

  return true;
}

void RealRobotDriver::stop_arm(uint8_t arm_id)
{
  std::lock_guard<std::mutex> lock(api_mutex_);
  rm_robot_handle * handle = handle_for(arm_id);
  if (handle && rm_service_) {
    rm_service_->rm_set_arm_stop(handle);
  }
}

void RealRobotDriver::stop_all()
{
  stop_arm(0);
  stop_arm(1);
}

void RealRobotDriver::realtime_state_callback(rm_realtime_arm_joint_state_t data)
{
  (void)data;
}

void RealRobotDriver::api_log_callback(const char * message, va_list args)
{
  if (!instance_ || !message) {
    return;
  }
  char buffer[1024];
  vsnprintf(buffer, sizeof(buffer), message, args);
  RCLCPP_INFO(instance_->logger_, "%s", buffer);
}

bool RealRobotDriver::init_handle(
  rm_robot_handle ** handle,
  const std::string & ip,
  int port,
  const std::string & push_ip,
  int push_port,
  int push_cycle)
{
  if (!rm_service_) {
    return false;
  }

  *handle = rm_service_->rm_create_robot_arm(ip.c_str(), port);
  if (*handle == nullptr || (*handle)->id <= 0) {
    RCLCPP_ERROR(logger_, "Failed to create handle for arm %s", ip.c_str());
    return false;
  }

  rm_realtime_push_config_t config{};
  config.cycle = push_cycle;
  config.enable = true;
  config.port = push_port;
  config.force_coordinate = 0;
  std::snprintf(config.ip, sizeof(config.ip), "%s", push_ip.c_str());

  const int push_result = rm_service_->rm_set_realtime_push(*handle, config);
  if (push_result != 0) {
    RCLCPP_ERROR(
      logger_,
      "Failed to set realtime push for arm %s (code %d)",
      ip.c_str(),
      push_result);
    return false;
  }

  rm_robot_info_t robot_info{};
  const int info_result = rm_service_->rm_get_robot_info(*handle, &robot_info);
  if (info_result != 0 || robot_info.arm_dof <= 0) {
    RCLCPP_ERROR(
      logger_,
      "Invalid robot info for arm %s (code %d, dof %d)",
      ip.c_str(),
      info_result,
      robot_info.arm_dof);
    return false;
  }

  return true;
}

rm_robot_handle * RealRobotDriver::handle_for(uint8_t arm_id) const
{
  if (arm_id == 0) {
    return left_handle_;
  }
  if (arm_id == 1) {
    return right_handle_;
  }
  return nullptr;
}

int RealRobotDriver::clamp_smooth_ratio(int smooth_ratio)
{
  return std::clamp(smooth_ratio, 0, 1000);
}

}  // namespace dual_arm_remote_teleop

