#include "convertor_2.h"

#include <algorithm>
#include <cstddef>
#include <string>

namespace
{

/// Empty string for a NULL C string — every char* coming from the host is optional.
inline std::string safe_string(const char* s)
{
  return s != nullptr ? std::string(s) : std::string();
}

}  // namespace

robot_std_msgs::Header convert2CppHeader(const Header& header)
{
  robot_std_msgs::Header cpp;
  cpp.seq = header.seq;
  cpp.stamp.sec = header.sec;
  cpp.stamp.nsec = header.nsec;
  cpp.frame_id = safe_string(header.frame_id);
  return cpp;
}

robot_sensor_msgs::Image convert2CppImage(const Image& image)
{
  robot_sensor_msgs::Image cpp;
  cpp.header = convert2CppHeader(image.header);
  cpp.height = image.height;
  cpp.width = image.width;
  cpp.encoding = safe_string(image.encoding);
  cpp.is_bigendian = image.is_bigendian;
  cpp.step = image.step;

  // Copy, never alias: the host buffer may be a pinned .NET array released right after this call,
  // while the voxel layer reads the sample later on the map-update thread.
  const size_t count = image.data != nullptr ? image.data_count : 0;
  cpp.data.assign(image.data, image.data + count);
  return cpp;
}

robot_sensor_msgs::CameraInfo convert2CppCameraInfo(const CameraInfo& info)
{
  robot_sensor_msgs::CameraInfo cpp;
  cpp.header = convert2CppHeader(info.header);
  cpp.height = info.height;
  cpp.width = info.width;
  cpp.distortion_model = safe_string(info.distortion_model);

  const size_t d_count = info.D != nullptr ? info.D_count : 0;
  cpp.D.assign(info.D, info.D + d_count);

  std::copy(info.K, info.K + 9, cpp.K.begin());
  std::copy(info.R, info.R + 9, cpp.R.begin());
  std::copy(info.P, info.P + 12, cpp.P.begin());

  cpp.binning_x = info.binning_x;
  cpp.binning_y = info.binning_y;

  cpp.roi.x_offset = info.roi.x_offset;
  cpp.roi.y_offset = info.roi.y_offset;
  cpp.roi.height = info.roi.height;
  cpp.roi.width = info.roi.width;
  cpp.roi.do_rectify = info.roi.do_rectify;
  return cpp;
}

robot_sensor_msgs::DepthCameraData convert2CppDepthCameraData(const DepthCameraData& data)
{
  robot_sensor_msgs::DepthCameraData cpp;
  cpp.header = convert2CppHeader(data.header);
  cpp.depth = convert2CppImage(data.depth);
  cpp.camera_info = convert2CppCameraInfo(data.camera_info);
  return cpp;
}
