#include "convertor.h"
#include <robot/robot.h>
#include <cstdlib>
#include <cstring>


robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan) 
{
  robot_sensor_msgs::LaserScan robot_laser_scan;
  robot_laser_scan.header.seq = laser_scan.header.seq;
  robot_laser_scan.header.stamp.sec = laser_scan.header.sec;
  robot_laser_scan.header.stamp.nsec = laser_scan.header.nsec;
  robot_laser_scan.header.frame_id = laser_scan.header.frame_id;
  robot_laser_scan.angle_min = laser_scan.angle_min;
  robot_laser_scan.angle_max = laser_scan.angle_max;
  robot_laser_scan.angle_increment = laser_scan.angle_increment;
  robot_laser_scan.time_increment = laser_scan.time_increment;
  robot_laser_scan.scan_time = laser_scan.scan_time;
  robot_laser_scan.range_min = laser_scan.range_min;
  robot_laser_scan.range_max = laser_scan.range_max;
  size_t ranges_size = laser_scan.ranges ? laser_scan.ranges_count : 0;
  size_t intensities_size = laser_scan.intensities ? laser_scan.intensities_count : 0;
  robot_laser_scan.ranges.resize(ranges_size);
  robot_laser_scan.intensities.resize(intensities_size);
  for (size_t i = 0; i < ranges_size; i++) {
    robot_laser_scan.ranges[i] = laser_scan.ranges[i];
  }
  for (size_t i = 0; i < intensities_size; i++) {
    robot_laser_scan.intensities[i] = laser_scan.intensities[i];
  }
  return robot_laser_scan;
}


robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occupancy_grid) {
  robot::log_info("OccupancyGrid sizeof=%zu data_off=%zu data_count_off=%zu",
                  sizeof(OccupancyGrid),
                  offsetof(OccupancyGrid, data),
                  offsetof(OccupancyGrid, data_count));
  robot_nav_msgs::OccupancyGrid robot_occupancy_grid;
  robot_occupancy_grid.header.seq = occupancy_grid.header.seq;
  robot_occupancy_grid.header.stamp.sec = occupancy_grid.header.sec;
  robot_occupancy_grid.header.stamp.nsec = occupancy_grid.header.nsec;
  robot_occupancy_grid.header.frame_id = occupancy_grid.header.frame_id;
  robot_occupancy_grid.info.map_load_time.sec = occupancy_grid.info.map_load_time.sec;
  robot_occupancy_grid.info.map_load_time.nsec = occupancy_grid.info.map_load_time.nsec;
  robot_occupancy_grid.info.resolution = occupancy_grid.info.resolution;
  robot_occupancy_grid.info.width = occupancy_grid.info.width;
  robot_occupancy_grid.info.height = occupancy_grid.info.height;
  robot_occupancy_grid.info.origin.position.x = occupancy_grid.info.origin.position.x;
  robot_occupancy_grid.info.origin.position.y = occupancy_grid.info.origin.position.y;
  robot_occupancy_grid.info.origin.position.z = occupancy_grid.info.origin.position.z;
  robot_occupancy_grid.info.origin.orientation.x = occupancy_grid.info.origin.orientation.x;
  robot_occupancy_grid.info.origin.orientation.y = occupancy_grid.info.origin.orientation.y;
  robot_occupancy_grid.info.origin.orientation.z = occupancy_grid.info.origin.orientation.z;
  robot_occupancy_grid.info.origin.orientation.w = occupancy_grid.info.origin.orientation.w;

  size_t data_size = occupancy_grid.data ? occupancy_grid.data_count : 0;

  robot_occupancy_grid.data.resize(data_size);
  for (size_t i = 0; i < data_size; i++) {
    robot_occupancy_grid.data[i] = occupancy_grid.data[i];
  }
  return robot_occupancy_grid;
}

robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry) 
{
  robot_nav_msgs::Odometry robot_odometry;
  robot_odometry.header.seq = odometry.header.seq;
  robot_odometry.header.stamp.sec = odometry.header.sec;
  robot_odometry.header.stamp.nsec = odometry.header.nsec;
  robot_odometry.header.frame_id = odometry.header.frame_id ? odometry.header.frame_id : "base_link";
  
  robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : "base_link";
  robot_odometry.pose.pose.position.x = odometry.pose.pose.position.x;
  robot_odometry.pose.pose.position.y = odometry.pose.pose.position.y;
  robot_odometry.pose.pose.position.z = odometry.pose.pose.position.z;
  robot_odometry.pose.pose.orientation.x = odometry.pose.pose.orientation.x;
  robot_odometry.pose.pose.orientation.y = odometry.pose.pose.orientation.y;
  robot_odometry.pose.pose.orientation.z = odometry.pose.pose.orientation.z;
  robot_odometry.pose.pose.orientation.w = odometry.pose.pose.orientation.w;

  if (odometry.pose.covariance && odometry.pose.covariance_count > 0)
  {
    const size_t n = std::min(static_cast<size_t>(36), odometry.pose.covariance_count);
    for (size_t i = 0; i < n; ++i)
      robot_odometry.pose.covariance[i] = odometry.pose.covariance[i];
  }

  robot_odometry.twist.twist.linear.x = odometry.twist.twist.linear.x;
  robot_odometry.twist.twist.linear.y = odometry.twist.twist.linear.y;
  robot_odometry.twist.twist.linear.z = odometry.twist.twist.linear.z;
  robot_odometry.twist.twist.angular.x = odometry.twist.twist.angular.x;
  robot_odometry.twist.twist.angular.y = odometry.twist.twist.angular.y;
  robot_odometry.twist.twist.angular.z = odometry.twist.twist.angular.z;

  if (odometry.twist.covariance && odometry.twist.covariance_count > 0)
  {
    const size_t n = std::min(static_cast<size_t>(36), odometry.twist.covariance_count);
    for (size_t i = 0; i < n; ++i)
      robot_odometry.twist.covariance[i] = odometry.twist.covariance[i];
  }

  return robot_odometry;
}

// Convert C PoseStamped to C++ PoseStamped
robot_geometry_msgs::PoseStamped convert2CppPoseStamped(const PoseStamped& c_pose)
{
  robot_geometry_msgs::PoseStamped cpp_pose;
  cpp_pose.header.seq = c_pose.header.seq;
  cpp_pose.header.stamp.sec = c_pose.header.sec;
  cpp_pose.header.stamp.nsec = c_pose.header.nsec;
  cpp_pose.header.frame_id = c_pose.header.frame_id;
  
  cpp_pose.pose.position.x = c_pose.pose.position.x;
  cpp_pose.pose.position.y = c_pose.pose.position.y;
  cpp_pose.pose.position.z = c_pose.pose.position.z;
  cpp_pose.pose.orientation.x = c_pose.pose.orientation.x;
  cpp_pose.pose.orientation.y = c_pose.pose.orientation.y;
  cpp_pose.pose.orientation.z = c_pose.pose.orientation.z;
  cpp_pose.pose.orientation.w = c_pose.pose.orientation.w;
  return cpp_pose;
}

// Convert C++ PoseStamped to C PoseStamped
PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pose) 
{
  PoseStamped c_pose;
  c_pose.header.seq = cpp_pose.header.seq;
  c_pose.header.sec = cpp_pose.header.stamp.sec;
  c_pose.header.nsec = cpp_pose.header.stamp.nsec;
  c_pose.header.frame_id = cpp_pose.header.frame_id.empty() ? nullptr : strdup(cpp_pose.header.frame_id.c_str());
  c_pose.pose.position.x = cpp_pose.pose.position.x;
  c_pose.pose.position.y = cpp_pose.pose.position.y;
  c_pose.pose.position.z = cpp_pose.pose.position.z;
  c_pose.pose.orientation.x = cpp_pose.pose.orientation.x;
  c_pose.pose.orientation.y = cpp_pose.pose.orientation.y;
  c_pose.pose.orientation.z = cpp_pose.pose.orientation.z;
  c_pose.pose.orientation.w = cpp_pose.pose.orientation.w;
  return c_pose;
}

// Convert C++ Pose2D to C Pose2D
Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose) 
{
  Pose2D c_pose;
  c_pose.x = cpp_pose.x;
  c_pose.y = cpp_pose.y;
  c_pose.theta = cpp_pose.theta;
  return c_pose;
}
Header convert2CHeader(const robot_std_msgs::Header& cpp_header)
{
  Header c_header;
  c_header.seq = cpp_header.seq;
  c_header.sec = cpp_header.stamp.sec;
  c_header.nsec = cpp_header.stamp.nsec;
  c_header.frame_id = cpp_header.frame_id.empty() ? nullptr : strdup(cpp_header.frame_id.c_str());
  return c_header;
}

Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist) 
{
  Twist2DStamped c_twist;
  c_twist.header = convert2CHeader(cpp_twist.header);
  c_twist.velocity.x = cpp_twist.velocity.x;
  c_twist.velocity.y = cpp_twist.velocity.y;
  c_twist.velocity.theta = cpp_twist.velocity.theta;
  return c_twist;
}

// ========== C++ to C converters (for get_* API) ==========

void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out)
{
  out.header.seq = cpp.header.seq;
  out.header.sec = cpp.header.stamp.sec;
  out.header.nsec = cpp.header.stamp.nsec;
  out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str());
  out.info.map_load_time.sec = cpp.info.map_load_time.sec;
  out.info.map_load_time.nsec = cpp.info.map_load_time.nsec;
  out.info.resolution = cpp.info.resolution;
  out.info.width = cpp.info.width;
  out.info.height = cpp.info.height;
  out.info.origin.position.x = cpp.info.origin.position.x;
  out.info.origin.position.y = cpp.info.origin.position.y;
  out.info.origin.position.z = cpp.info.origin.position.z;
  out.info.origin.orientation.x = cpp.info.origin.orientation.x;
  out.info.origin.orientation.y = cpp.info.origin.orientation.y;
  out.info.origin.orientation.z = cpp.info.origin.orientation.z;
  out.info.origin.orientation.w = cpp.info.origin.orientation.w;
  out.data_count = cpp.data.size();
  out.data = nullptr;
  if (out.data_count > 0)
  {
    out.data = static_cast<int8_t*>(malloc(out.data_count * sizeof(int8_t)));
    if (out.data)
      memcpy(out.data, cpp.data.data(), out.data_count * sizeof(int8_t));
  }
}

static void convert2CPose2DStamped(const robot_nav_2d_msgs::Pose2DStamped& cpp, Pose2DStamped& out)
{
  out.header = convert2CHeader(cpp.header);
  out.pose = convert2CPose2D(cpp.pose);
}

void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out)
{
  out.header = convert2CHeader(cpp.header);
  out.poses_count = cpp.poses.size();
  out.poses = nullptr;
  if (out.poses_count > 0)
  {
    out.poses = static_cast<Pose2DStamped*>(malloc(out.poses_count * sizeof(Pose2DStamped)));
    if (out.poses)
    {
      for (size_t i = 0; i < out.poses_count; i++)
        convert2CPose2DStamped(cpp.poses[i], out.poses[i]);
    }
  }
}

void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out)
{
  out.header = convert2CHeader(cpp.header);
  out.x = cpp.x;
  out.y = cpp.y;
  out.width = cpp.width;
  out.height = cpp.height;
  out.data_count = cpp.data.size();
  out.data = nullptr;
  if (out.data_count > 0)
  {
    out.data = static_cast<int8_t*>(malloc(out.data_count * sizeof(int8_t)));
    if (out.data)
      memcpy(out.data, cpp.data.data(), out.data_count * sizeof(int8_t));
  }
}

static void convert2CPolygon(const robot_geometry_msgs::Polygon& cpp, Polygon& out)
{
  out.points_count = cpp.points.size();
  out.points = nullptr;
  if (out.points_count > 0)
  {
    out.points = static_cast<Point32*>(malloc(out.points_count * sizeof(Point32)));
    if (out.points)
    {
      for (size_t i = 0; i < out.points_count; i++)
      {
        out.points[i].x = cpp.points[i].x;
        out.points[i].y = cpp.points[i].y;
        out.points[i].z = cpp.points[i].z;
      }
    }
  }
}

void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out)
{
  out.header = convert2CHeader(cpp.header);
  convert2CPolygon(cpp.polygon, out.polygon);
}

void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out)
{
  out.header.seq = cpp.header.seq;
  out.header.sec = cpp.header.stamp.sec;
  out.header.nsec = cpp.header.stamp.nsec;
  out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str());
  out.angle_min = cpp.angle_min;
  out.angle_max = cpp.angle_max;
  out.angle_increment = cpp.angle_increment;
  out.time_increment = cpp.time_increment;
  out.scan_time = cpp.scan_time;
  out.range_min = cpp.range_min;
  out.range_max = cpp.range_max;
  out.ranges_count = cpp.ranges.size();
  out.ranges = nullptr;
  if (out.ranges_count > 0)
  {
    out.ranges = static_cast<float*>(malloc(out.ranges_count * sizeof(float)));
    if (out.ranges)
      memcpy(out.ranges, cpp.ranges.data(), out.ranges_count * sizeof(float));
  }
  out.intensities_count = cpp.intensities.size();
  out.intensities = nullptr;
  if (out.intensities_count > 0)
  {
    out.intensities = static_cast<float*>(malloc(out.intensities_count * sizeof(float)));
    if (out.intensities)
      memcpy(out.intensities, cpp.intensities.data(), out.intensities_count * sizeof(float));
  }
}

void convert2CPointCloud(const robot_sensor_msgs::PointCloud& cpp, PointCloud& out)
{
  out.header.seq = cpp.header.seq;
  out.header.sec = cpp.header.stamp.sec;
  out.header.nsec = cpp.header.stamp.nsec;
  out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str());
  out.points_count = cpp.points.size();
  out.points = nullptr;
  if (out.points_count > 0)
  {
    out.points = static_cast<Point32*>(malloc(out.points_count * sizeof(Point32)));
    if (out.points)
      for (size_t i = 0; i < out.points_count; ++i)
      {
        out.points[i].x = cpp.points[i].x;
        out.points[i].y = cpp.points[i].y;
        out.points[i].z = cpp.points[i].z;
      }
  }
  out.channels_count = cpp.channels.size();
  out.channels = nullptr;
  if (out.channels_count > 0)
  {
    out.channels = static_cast<ChannelFloat32*>(malloc(out.channels_count * sizeof(ChannelFloat32)));
    if (out.channels)
      for (size_t i = 0; i < out.channels_count; ++i)
      {
        out.channels[i].name = cpp.channels[i].name.empty() ? nullptr : strdup(cpp.channels[i].name.c_str());
        out.channels[i].values_count = cpp.channels[i].values.size();
        out.channels[i].values = nullptr;
        if (out.channels[i].values_count > 0)
        {
          out.channels[i].values = static_cast<float*>(malloc(out.channels[i].values_count * sizeof(float)));
          if (out.channels[i].values)
            memcpy(out.channels[i].values, cpp.channels[i].values.data(), out.channels[i].values_count * sizeof(float));
        }
      }
  }
}

void convert2CPointCloud2(const robot_sensor_msgs::PointCloud2& cpp, PointCloud2& out)
{
  out.header.seq = cpp.header.seq;
  out.header.sec = cpp.header.stamp.sec;
  out.header.nsec = cpp.header.stamp.nsec;
  out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str());
  out.height = cpp.height;
  out.width = cpp.width;
  out.is_bigendian = cpp.is_bigendian;
  out.point_step = cpp.point_step;
  out.row_step = cpp.row_step;
  out.is_dense = cpp.is_dense;
  out.fields_count = cpp.fields.size();
  out.fields = nullptr;
  if (out.fields_count > 0)
  {
    out.fields = static_cast<PointField*>(malloc(out.fields_count * sizeof(PointField)));
    if (out.fields)
      for (size_t i = 0; i < out.fields_count; ++i)
      {
        out.fields[i].name = cpp.fields[i].name.empty() ? nullptr : strdup(cpp.fields[i].name.c_str());
        out.fields[i].offset = cpp.fields[i].offset;
        out.fields[i].datatype = cpp.fields[i].datatype;
        out.fields[i].count = cpp.fields[i].count;
      }
  }
  out.data_count = cpp.data.size();
  out.data = nullptr;
  if (out.data_count > 0)
  {
    out.data = static_cast<uint8_t*>(malloc(out.data_count));
    if (out.data)
      memcpy(out.data, cpp.data.data(), out.data_count);
  }
}

// ========== C to C++ for PointCloud / PointCloud2 (for add_* API) ==========

robot_sensor_msgs::PointCloud convert2CppPointCloud(const PointCloud& c)
{
  robot_sensor_msgs::PointCloud cpp;
  cpp.header.seq = c.header.seq;
  cpp.header.stamp.sec = c.header.sec;
  cpp.header.stamp.nsec = c.header.nsec;
  cpp.header.frame_id = c.header.frame_id ? c.header.frame_id : "";
  cpp.points.resize(c.points_count);
  for (size_t i = 0; i < c.points_count && c.points; ++i)
  {
    cpp.points[i].x = c.points[i].x;
    cpp.points[i].y = c.points[i].y;
    cpp.points[i].z = c.points[i].z;
  }
  cpp.channels.resize(c.channels_count);
  for (size_t i = 0; i < c.channels_count && c.channels; ++i)
  {
    cpp.channels[i].name = c.channels[i].name ? c.channels[i].name : "";
    cpp.channels[i].values.resize(c.channels[i].values_count);
    if (c.channels[i].values && c.channels[i].values_count > 0)
      memcpy(cpp.channels[i].values.data(), c.channels[i].values, c.channels[i].values_count * sizeof(float));
  }
  return cpp;
}

robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c)
{
  robot_sensor_msgs::PointCloud2 cpp;
  cpp.header.seq = c.header.seq;
  cpp.header.stamp.sec = c.header.sec;
  cpp.header.stamp.nsec = c.header.nsec;
  cpp.header.frame_id = c.header.frame_id ? c.header.frame_id : "";
  cpp.height = c.height;
  cpp.width = c.width;
  cpp.is_bigendian = c.is_bigendian;
  cpp.point_step = c.point_step;
  cpp.row_step = c.row_step;
  cpp.is_dense = c.is_dense;
  cpp.fields.resize(c.fields_count);
  for (size_t i = 0; i < c.fields_count && c.fields; ++i)
  {
    cpp.fields[i].name = c.fields[i].name ? c.fields[i].name : "";
    cpp.fields[i].offset = c.fields[i].offset;
    cpp.fields[i].datatype = c.fields[i].datatype;
    cpp.fields[i].count = c.fields[i].count;
  }
  cpp.data.resize(c.data_count);
  if (c.data && c.data_count > 0)
    memcpy(cpp.data.data(), c.data, c.data_count);
  return cpp;
}

// --- protocol_msgs helpers (C <-> C++ robot_protocol_msgs) ---

static inline char* dup_cstr(const char* s)
{
  if (!s) return nullptr;
  const size_t n = std::strlen(s) + 1;
  char* p = static_cast<char*>(std::malloc(n));
  if (p) std::memcpy(p, s, n);
  return p;
}

static robot_protocol_msgs::ActionParameter convert2CppActionParameter(const ActionParameter& c)
{
  robot_protocol_msgs::ActionParameter cpp;
  cpp.key = c.key ? c.key : "";
  cpp.value = c.value ? c.value : "";
  return cpp;
}

static ActionParameter convert2CActionParameter(const robot_protocol_msgs::ActionParameter& cpp)
{
  ActionParameter c;
  c.key = dup_cstr(cpp.key.c_str());
  c.value = dup_cstr(cpp.value.c_str());
  return c;
}

static robot_protocol_msgs::ControlPoint convert2CppControlPoint(const ControlPoint& c)
{
  robot_protocol_msgs::ControlPoint cpp;
  cpp.x = c.x;
  cpp.y = c.y;
  cpp.weight = c.weight;
  return cpp;
}

static ControlPoint convert2CControlPoint(const robot_protocol_msgs::ControlPoint& cpp)
{
  ControlPoint c;
  c.x = cpp.x;
  c.y = cpp.y;
  c.weight = cpp.weight;
  return c;
}

static robot_protocol_msgs::NodePosition convert2CppNodePosition(const NodePosition& c)
{
  robot_protocol_msgs::NodePosition cpp;
  cpp.x = c.x;
  cpp.y = c.y;
  cpp.theta = c.theta;
  cpp.allowedDeviationXY = c.allowedDeviationXY;
  cpp.allowedDeviationTheta = c.allowedDeviationTheta;
  cpp.mapId = c.mapId ? c.mapId : "";
  cpp.mapDescription = c.mapDescription ? c.mapDescription : "";
  return cpp;
}

static NodePosition convert2CNodePosition(const robot_protocol_msgs::NodePosition& cpp)
{
  NodePosition c;
  c.x = cpp.x;
  c.y = cpp.y;
  c.theta = cpp.theta;
  c.allowedDeviationXY = cpp.allowedDeviationXY;
  c.allowedDeviationTheta = cpp.allowedDeviationTheta;
  c.mapId = dup_cstr(cpp.mapId.c_str());
  c.mapDescription = dup_cstr(cpp.mapDescription.c_str());
  return c;
}

static robot_protocol_msgs::Trajectory convert2CppTrajectory(const Trajectory& c)
{
  robot_protocol_msgs::Trajectory cpp;
  cpp.degree = c.degree;
  const size_t kv = c.knotVector ? c.knotVector_count : 0;
  cpp.knotVector.resize(kv);
  for (size_t i = 0; i < kv; i++)
    cpp.knotVector[i] = c.knotVector[i];
  const size_t ncp = c.controlPoints ? c.controlPoints_count : 0;
  cpp.controlPoints.resize(ncp);
  for (size_t i = 0; i < ncp; i++)
    cpp.controlPoints[i] = convert2CppControlPoint(c.controlPoints[i]);
  return cpp;
}

static Trajectory convert2CTrajectory(const robot_protocol_msgs::Trajectory& cpp)
{
  Trajectory c;
  c.degree = cpp.degree;
  c.knotVector_count = cpp.knotVector.size();
  c.knotVector = c.knotVector_count ? static_cast<double*>(std::malloc(c.knotVector_count * sizeof(double))) : nullptr;
  if (c.knotVector)
    for (size_t i = 0; i < c.knotVector_count; i++)
      c.knotVector[i] = cpp.knotVector[i];
  c.controlPoints_count = cpp.controlPoints.size();
  c.controlPoints = c.controlPoints_count ? static_cast<ControlPoint*>(std::malloc(c.controlPoints_count * sizeof(ControlPoint))) : nullptr;
  if (c.controlPoints)
    for (size_t i = 0; i < c.controlPoints_count; i++)
      c.controlPoints[i] = convert2CControlPoint(cpp.controlPoints[i]);
  return c;
}

static robot_protocol_msgs::Action convert2CppAction(const Action& c)
{
  robot_protocol_msgs::Action cpp;
  cpp.actionType = c.actionType ? c.actionType : "";
  cpp.actionId = c.actionId ? c.actionId : "";
  cpp.actionDescription = c.actionDescription ? c.actionDescription : "";
  cpp.blockingType = c.blockingType ? c.blockingType : "";
  const size_t n = c.actionParameters ? c.actionParameters_count : 0;
  cpp.actionParameters.resize(n);
  for (size_t i = 0; i < n; i++)
    cpp.actionParameters[i] = convert2CppActionParameter(c.actionParameters[i]);
  return cpp;
}

static Action convert2CAction(const robot_protocol_msgs::Action& cpp)
{
  Action c;
  c.actionType = dup_cstr(cpp.actionType.c_str());
  c.actionId = dup_cstr(cpp.actionId.c_str());
  c.actionDescription = dup_cstr(cpp.actionDescription.c_str());
  c.blockingType = dup_cstr(cpp.blockingType.c_str());
  c.actionParameters_count = cpp.actionParameters.size();
  c.actionParameters = c.actionParameters_count ? static_cast<ActionParameter*>(std::malloc(c.actionParameters_count * sizeof(ActionParameter))) : nullptr;
  if (c.actionParameters)
    for (size_t i = 0; i < c.actionParameters_count; i++)
      c.actionParameters[i] = convert2CActionParameter(cpp.actionParameters[i]);
  return c;
}

static robot_protocol_msgs::Node convert2CppNode(const Node& c)
{
  robot_protocol_msgs::Node cpp;
  cpp.nodeId = c.nodeId ? c.nodeId : "";
  cpp.sequenceId = c.sequenceId;
  cpp.nodeDescription = c.nodeDescription ? c.nodeDescription : "";
  cpp.released = c.released;
  cpp.nodePosition = convert2CppNodePosition(c.nodePosition);
  const size_t n = c.actions ? c.actions_count : 0;
  cpp.actions.resize(n);
  for (size_t i = 0; i < n; i++)
    cpp.actions[i] = convert2CppAction(c.actions[i]);
  return cpp;
}

static Node convert2CNode(const robot_protocol_msgs::Node& cpp)
{
  Node c;
  c.nodeId = dup_cstr(cpp.nodeId.c_str());
  c.sequenceId = cpp.sequenceId;
  c.nodeDescription = dup_cstr(cpp.nodeDescription.c_str());
  c.released = cpp.released;
  c.nodePosition = convert2CNodePosition(cpp.nodePosition);
  c.actions_count = cpp.actions.size();
  c.actions = c.actions_count ? static_cast<Action*>(std::malloc(c.actions_count * sizeof(Action))) : nullptr;
  if (c.actions)
    for (size_t i = 0; i < c.actions_count; i++)
      c.actions[i] = convert2CAction(cpp.actions[i]);
  return c;
}

static robot_protocol_msgs::Edge convert2CppEdge(const Edge& c)
{
  robot_protocol_msgs::Edge cpp;
  cpp.edgeId = c.edgeId ? c.edgeId : "";
  cpp.sequenceId = c.sequenceId;
  cpp.edgeDescription = c.edgeDescription ? c.edgeDescription : "";
  cpp.released = c.released;
  cpp.startNodeId = c.startNodeId ? c.startNodeId : "";
  cpp.endNodeId = c.endNodeId ? c.endNodeId : "";
  cpp.maxSpeed = c.maxSpeed;
  cpp.maxHeight = c.maxHeight;
  cpp.minHeight = c.minHeight;
  cpp.orientation = c.orientation;
  cpp.orientationType = c.orientationType ? c.orientationType : "";
  cpp.direction = c.direction ? c.direction : "";
  cpp.rotationAllowed = c.rotationAllowed;
  cpp.maxRotationSpeed = c.maxRotationSpeed;
  cpp.trajectory = convert2CppTrajectory(c.trajectory);
  cpp.length = c.length;
  const size_t n = c.actions ? c.actions_count : 0;
  cpp.actions.resize(n);
  for (size_t i = 0; i < n; i++)
    cpp.actions[i] = convert2CppAction(c.actions[i]);
  return cpp;
}

static Edge convert2CEdge(const robot_protocol_msgs::Edge& cpp)
{
  Edge c;
  c.edgeId = dup_cstr(cpp.edgeId.c_str());
  c.sequenceId = cpp.sequenceId;
  c.edgeDescription = dup_cstr(cpp.edgeDescription.c_str());
  c.released = cpp.released;
  c.startNodeId = dup_cstr(cpp.startNodeId.c_str());
  c.endNodeId = dup_cstr(cpp.endNodeId.c_str());
  c.maxSpeed = cpp.maxSpeed;
  c.maxHeight = cpp.maxHeight;
  c.minHeight = cpp.minHeight;
  c.orientation = cpp.orientation;
  c.orientationType = dup_cstr(cpp.orientationType.c_str());
  c.direction = dup_cstr(cpp.direction.c_str());
  c.rotationAllowed = cpp.rotationAllowed;
  c.maxRotationSpeed = cpp.maxRotationSpeed;
  c.trajectory = convert2CTrajectory(cpp.trajectory);
  c.length = cpp.length;
  c.actions_count = cpp.actions.size();
  c.actions = c.actions_count ? static_cast<Action*>(std::malloc(c.actions_count * sizeof(Action))) : nullptr;
  if (c.actions)
    for (size_t i = 0; i < c.actions_count; i++)
      c.actions[i] = convert2CAction(cpp.actions[i]);
  return c;
}

static void free_action_parameter(ActionParameter* p)
{
  if (!p) return;
  std::free(p->key);
  std::free(p->value);
  p->key = nullptr;
  p->value = nullptr;
}

static void free_action(Action* p)
{
  if (!p) return;
  std::free(p->actionType);
  std::free(p->actionId);
  std::free(p->actionDescription);
  std::free(p->blockingType);
  p->actionType = p->actionId = p->actionDescription = p->blockingType = nullptr;
  if (p->actionParameters) {
    for (size_t i = 0; i < p->actionParameters_count; i++)
      free_action_parameter(&p->actionParameters[i]);
    std::free(p->actionParameters);
    p->actionParameters = nullptr;
  }
  p->actionParameters_count = 0;
}

static void free_node_position(NodePosition* p)
{
  if (!p) return;
  std::free(p->mapId);
  std::free(p->mapDescription);
  p->mapId = p->mapDescription = nullptr;
}

static void free_trajectory(Trajectory* p)
{
  if (!p) return;
  std::free(p->knotVector);
  p->knotVector = nullptr;
  p->knotVector_count = 0;
  std::free(p->controlPoints);
  p->controlPoints = nullptr;
  p->controlPoints_count = 0;
}

static void free_node(Node* p)
{
  if (!p) return;
  std::free(p->nodeId);
  std::free(p->nodeDescription);
  p->nodeId = p->nodeDescription = nullptr;
  free_node_position(&p->nodePosition);
  if (p->actions) {
    for (size_t i = 0; i < p->actions_count; i++)
      free_action(&p->actions[i]);
    std::free(p->actions);
    p->actions = nullptr;
  }
  p->actions_count = 0;
}

static void free_edge(Edge* p)
{
  if (!p) return;
  std::free(p->edgeId);
  std::free(p->edgeDescription);
  std::free(p->startNodeId);
  std::free(p->endNodeId);
  std::free(p->orientationType);
  std::free(p->direction);
  p->edgeId = p->edgeDescription = p->startNodeId = p->endNodeId = nullptr;
  p->orientationType = p->direction = nullptr;
  free_trajectory(&p->trajectory);
  if (p->actions) {
    for (size_t i = 0; i < p->actions_count; i++)
      free_action(&p->actions[i]);
    std::free(p->actions);
    p->actions = nullptr;
  }
  p->actions_count = 0;
}

robot_protocol_msgs::Order convert2CppOrder(const Order& order)
{
  robot_protocol_msgs::Order cpp_order;
  cpp_order.headerId = order.headerId;
  cpp_order.timestamp = order.timestamp ? order.timestamp : "";
  cpp_order.version = order.version ? order.version : "";
  cpp_order.manufacturer = order.manufacturer ? order.manufacturer : "";
  cpp_order.serialNumber = order.serialNumber ? order.serialNumber : "";
  cpp_order.orderId = order.orderId ? order.orderId : "";
  cpp_order.orderUpdateId = order.orderUpdateId;
  const size_t nn = order.nodes ? order.nodes_count : 0;
  cpp_order.nodes.resize(nn);
  for (size_t i = 0; i < nn; i++)
    cpp_order.nodes[i] = convert2CppNode(order.nodes[i]);
  const size_t ne = order.edges ? order.edges_count : 0;
  cpp_order.edges.resize(ne);
  for (size_t i = 0; i < ne; i++)
    cpp_order.edges[i] = convert2CppEdge(order.edges[i]);
  cpp_order.zoneSetId = order.zoneSetId ? order.zoneSetId : "";
  return cpp_order;
}

Order convert2COrder(const robot_protocol_msgs::Order& cpp_order)
{
  Order order;
  order.headerId = cpp_order.headerId;
  order.timestamp = dup_cstr(cpp_order.timestamp.c_str());
  order.version = dup_cstr(cpp_order.version.c_str());
  order.manufacturer = dup_cstr(cpp_order.manufacturer.c_str());
  order.serialNumber = dup_cstr(cpp_order.serialNumber.c_str());
  order.orderId = dup_cstr(cpp_order.orderId.c_str());
  order.orderUpdateId = cpp_order.orderUpdateId;
  order.nodes_count = cpp_order.nodes.size();
  order.nodes = order.nodes_count ? static_cast<Node*>(std::malloc(order.nodes_count * sizeof(Node))) : nullptr;
  if (order.nodes)
    for (size_t i = 0; i < order.nodes_count; i++)
      order.nodes[i] = convert2CNode(cpp_order.nodes[i]);
  order.edges_count = cpp_order.edges.size();
  order.edges = order.edges_count ? static_cast<Edge*>(std::malloc(order.edges_count * sizeof(Edge))) : nullptr;
  if (order.edges)
    for (size_t i = 0; i < order.edges_count; i++)
      order.edges[i] = convert2CEdge(cpp_order.edges[i]);
  order.zoneSetId = dup_cstr(cpp_order.zoneSetId.c_str());
  return order;
}

extern "C" void order_free(Order* order)
{
  if (!order) return;
  std::free(order->timestamp);
  std::free(order->version);
  std::free(order->manufacturer);
  std::free(order->serialNumber);
  std::free(order->orderId);
  std::free(order->zoneSetId);
  order->timestamp = order->version = order->manufacturer = nullptr;
  order->serialNumber = order->orderId = order->zoneSetId = nullptr;
  if (order->nodes) {
    for (size_t i = 0; i < order->nodes_count; i++)
      free_node(&order->nodes[i]);
    std::free(order->nodes);
    order->nodes = nullptr;
  }
  order->nodes_count = 0;
  if (order->edges) {
    for (size_t i = 0; i < order->edges_count; i++)
      free_edge(&order->edges[i]);
    std::free(order->edges);
    order->edges = nullptr;
  }
  order->edges_count = 0;
}

bool isQuaternionValid(const Quaternion& q)
{
  // first we need to check if the quaternion has nan's or infs
  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
  {
    robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
    return false;
  }

  tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);

  // next, we need to check if the length of the quaternion is close to zero
  if (tf_q.length2() < 1e-6)
  {
    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
    return false;
  }

  // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
  tf_q.normalize();

  tf3::Vector3 up(0, 0, 1);

  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

  if (fabs(dot - 1) > 1e-3)
  {
    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
    return false;
  }

  return true;
}