#include <robot/robot.h>
#include "nav_c_api.h"
#include "convertor.h"
#include <angles/angles.h>
#include <data_convert/data_convert.h>
#include <boost/dll/import.hpp>
#include <robot/plugin_loader_helper.h>
#include <move_base_core/navigation.h>
#include <move_base_core/common.h>
#include <string>
#include <vector>
#include <cstring>
#include <cstdlib>
#include <stdexcept>
#include <new>
#include <unordered_map>
#include <mutex>

// Keep navigation instances alive even when returning raw pointers as C handles.
// Key is the raw BaseNavigation* (used as NavigationHandle), value owns the instance.
static std::mutex g_nav_instances_mutex;
static std::unordered_map<robot::move_base_core::BaseNavigation *,
                          std::shared_ptr<robot::move_base_core::BaseNavigation>>g_nav_instances;

static NavFeedback convert2CNavFeedback(const robot::move_base_core::NavFeedback &cpp)
{
  NavFeedback out;
  out.navigation_state = static_cast<NavigationState>(static_cast<int>(cpp.navigation_state));
  out.feed_back_str = cpp.feed_back_str.empty() ? nullptr : strdup(cpp.feed_back_str.c_str());
  out.current_pose.x = cpp.current_pose.x;
  out.current_pose.y = cpp.current_pose.y;
  out.current_pose.theta = cpp.current_pose.theta;
  out.goal_checked = cpp.goal_checked;
  out.is_ready = cpp.is_ready;
  return out;
}

extern "C" char *navigation_state_to_string(NavigationState state)
{
  robot::move_base_core::State cpp_state = static_cast<robot::move_base_core::State>(static_cast<int>(state));
  std::string str = robot::move_base_core::toString(cpp_state);
  return strdup(str.c_str());
}

extern "C" void nav_c_api_free_string(char *str)
{
  if (str)
    free(str);
}

extern "C" bool navigation_offset_goal_2d(const Pose2D &in_pose,
                                          const char *frame_id, double offset_distance,
                                          PoseStamped &out_goal)
{
  if (!frame_id)
  {
    return false;
  }

  robot_geometry_msgs::Pose2D pose = convert2CppPose2D(in_pose);
  robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
  out_goal = convert2CPoseStamped(result);
  return true;
}

extern "C" bool navigation_offset_goal_stamped(const PoseStamped &in_pose, double offset_distance,
                                               PoseStamped &out_goal)
{
  robot_geometry_msgs::PoseStamped cpp_pose = convert2CppPoseStamped(in_pose);
  robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
  out_goal = convert2CPoseStamped(result);
  return true;
}

// ============================================================================
// Navigation Handle Management
// ============================================================================
extern "C" NavigationHandle navigation_create(void)
{
  try
  {
    // Create a concrete MoveBase instance
    // Using default constructor - initialization will be done via navigation_initialize()
    robot::PluginLoaderHelper loader;
    std::string path_file_so = loader.findLibraryPath("MoveBase");
    robot::log_warning("%s", path_file_so.c_str());
    auto move_base_loader = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
        path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
    auto move_base_ptr = move_base_loader();
    if (!move_base_ptr)
      return nullptr;

    auto *raw = move_base_ptr.get();
    {
      std::lock_guard<std::mutex> lock(g_nav_instances_mutex);
      g_nav_instances[raw] = std::move(move_base_ptr);
    }

    // Return raw pointer as opaque C handle.
    return static_cast<NavigationHandle>(raw);
  }
  catch (const std::exception &e)
  {
    // Log error if possible (in production, use proper logging)
    return nullptr;
  }
  catch (...)
  {
    return nullptr;
  }
}

extern "C" void navigation_destroy(NavigationHandle handle)
{
  if (!handle)
    return;

  auto *raw = static_cast<robot::move_base_core::BaseNavigation *>(handle);
  std::lock_guard<std::mutex> lock(g_nav_instances_mutex);
  g_nav_instances.erase(raw);
}

// ============================================================================
// Navigation Interface Methods
// ============================================================================

extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle)
{
  if (!handle || !tf_handle)
  {
    printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__);
    return false;
  }
  try
  {
    printf("[%s:%d]: Initialize navigation\n", __FILE__, __LINE__);
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});

    if (!nav_ptr)
      return false;

    std::shared_ptr<tf3::BufferCore> tf_ptr =
        std::shared_ptr<tf3::BufferCore>(reinterpret_cast<tf3::BufferCore *>(tf_handle), [](::tf3::BufferCore *) {});
    if (!tf_ptr)
    {
      printf("[%s:%d]\n Error: Invalid TF listener", __FILE__, __LINE__);
      return false;
    }
    nav_ptr->initialize(tf_ptr);

    return true;
  }
  catch (const std::exception &e)
  {
    printf("[%s:%d]\n Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what());
    return false;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point *points, size_t point_count)
{
  if (!handle || !points || point_count == 0)
  {
    return false;
  }

  try
  {

    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    std::vector<robot_geometry_msgs::Point> footprint;
    footprint.reserve(point_count);
    for (size_t i = 0; i < point_count; ++i)
    {
      
      robot_geometry_msgs::Point pt;
      pt.x = points[i].x;
      pt.y = points[i].y;
      pt.z = points[i].z;
      printf("footprint x: %f, y: %f\n", pt.x, pt.y);
      footprint.push_back(pt);
    }
    nav_ptr->setRobotFootprint(footprint);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count)
{
  if (!handle)
  {
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    std::vector<robot_geometry_msgs::Point> footprint = nav_ptr->getRobotFootprint();
    if (footprint.empty())
    {
      return false;
    }

    out_count = footprint.size();
    for (size_t i = 0; i < out_count; ++i)
    {
      out_points[i].x = footprint[i].x;
      out_points[i].y = footprint[i].y;
      out_points[i].z = footprint[i].z;
    }
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped goal)
{
  if (!handle)
  {
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("Goal quaternion is invalid");
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
    return nav_ptr->moveTo(cpp_goal);
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal)
{
  if (!handle)
  {
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("Goal quaternion is invalid");
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    robot::log_error("navigation_move_to_order goal %f %f", goal.pose.position.x, goal.pose.position.y);
    robot_protocol_msgs::Order cpp_order = convert2CppOrder(order);
    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
    return nav_ptr->moveTo(cpp_order, cpp_goal);
  }
  catch (const std::exception &e)
  {
    return false;
  }
  catch (...)
  {
    printf("[%s:%d]\n Error: Failed to move to order\n", __FILE__, __LINE__);
    return false;
  }
}

extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal)
{
  if (!handle)
  {
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("Goal quaternion is invalid");
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    robot::log_error("navigation_move_to_nodes_edges goal %f %f", goal.pose.position.x, goal.pose.position.y);
    // Zero-init: the fields left unset below are all char*, and convert2CppOrder treats any
    // non-null pointer as a valid string — an uninitialized struct hands it stack garbage.
    Order order{};
    order.nodes = const_cast<Node *>(nodes);
    order.nodes_count = node_count;
    order.edges = const_cast<Edge *>(edges);
    order.edges_count = edge_count;
    robot_protocol_msgs::Order cpp_order = convert2CppOrder(order);
    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
    return nav_ptr->moveTo(cpp_order, cpp_goal);
  }
  catch (const std::exception &e)
  {
    return false;
  }
  catch (...)
  {
    printf("[%s:%d]\n Error: Failed to move to order\n", __FILE__, __LINE__);
    return false;
  }
}
extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal)
{
  if (!handle || !marker)
  {
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("Goal quaternion is invalid");
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
  return nav_ptr->dockTo(std::string(marker), cpp_goal);
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal) 
{
  if (!handle || !marker)
  {
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("Goal quaternion is invalid");
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    robot_protocol_msgs::Order cpp_order = convert2CppOrder(order);
    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
    return nav_ptr->dockTo(cpp_order, std::string(marker), cpp_goal);
  }
  catch (const std::exception &e)
  {
    return false;
  }
  catch (...)
  {
    printf("[%s:%d]\n Error: Failed to dock to order\n", __FILE__, __LINE__);
    return false;
  }
}

extern "C" bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal)
{
  if (!handle || !marker)
  {
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("Goal quaternion is invalid");
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    // Zero-init: see navigation_move_to_nodes_edges.
    Order order{};
    order.nodes = const_cast<Node *>(nodes);
    order.nodes_count = node_count;
    order.edges = const_cast<Edge *>(edges);
    order.edges_count = edge_count;
    robot_protocol_msgs::Order cpp_order = convert2CppOrder(order);
    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
    return nav_ptr->dockTo(cpp_order, std::string(marker), cpp_goal);
  }
  catch (const std::exception &e)
  {
    return false;
  }
  catch (...)
  {
    printf("[%s:%d]\n Error: Failed to dock to nodes edges\n", __FILE__, __LINE__);
    return false;
  }
}

extern "C" bool navigation_move_straight_to(NavigationHandle handle, const double distance)
{
  if (!handle)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_geometry_msgs::PoseStamped pose;
    if (!nav_ptr->getRobotPose(pose))
      return false;
    robot_geometry_msgs::Pose2D pose2d;
    pose2d.x = pose.pose.position.x;
    pose2d.y = pose.pose.position.y;
    pose2d.theta = data_convert::getYaw(pose.pose.orientation);
    robot::log_error("pose2d: %f %f %f", pose2d.x, pose2d.y, pose2d.theta);
    robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance);
    return nav_ptr->moveStraightTo(goal);
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal)
{
  if (!handle)
  {
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("Goal quaternion is invalid");
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
    return nav_ptr->rotateTo(cpp_goal);
  }
  catch (...)
  {
    return false;
  }
}

extern "C" void navigation_pause(NavigationHandle handle)
{
  if (handle)
  {
    try
    {
      std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
          std::shared_ptr<robot::move_base_core::BaseNavigation>(
              reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
      if (!nav_ptr)
        return;
      nav_ptr->pause();
    }
    catch (...)
    {
      // Ignore exceptions
    }
  }
}

extern "C" void navigation_resume(NavigationHandle handle)
{
  if (handle)
  {
    try
    {
      std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
          std::shared_ptr<robot::move_base_core::BaseNavigation>(
              reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
      if (nav_ptr)
        nav_ptr->resume();
    }
    catch (...)
    {
      // Ignore exceptions
    }
  }
}

extern "C" void navigation_cancel(NavigationHandle handle)
{
  if (handle)
  {
    try
    {
      std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
          std::shared_ptr<robot::move_base_core::BaseNavigation>(
              reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
      if (nav_ptr)
        nav_ptr->cancel();
    }
    catch (...)
    {
      // Ignore exceptions
    }
  }
}

extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
                                            double linear_x, double linear_y, double linear_z)
{
  if (!handle)
  {
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_geometry_msgs::Vector3 linear;
    linear.x = 0.1;
    linear.y = linear_y;
    linear.z = linear_z;
    bool result = nav_ptr->setTwistLinear(linear);
    robot::log_info("setTwistLinear Forward %f", linear.x);
    
    linear.x = -0.1;
    result &= result && nav_ptr->setTwistLinear(linear);
    robot::log_info("setTwistLinear Backward %f", linear.x);
    return result;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
                                             double angular_x, double angular_y, double angular_z)
{
  if (!handle)
  {
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_geometry_msgs::Vector3 angular;
    angular.x = angular_x;
    angular.y = angular_y;
    angular.z = angular_z;
    return nav_ptr->setTwistAngular(angular);
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose)
{
  if (!handle)
  {
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_geometry_msgs::PoseStamped cpp_pose;
    if (nav_ptr->getRobotPose(cpp_pose))
    {
      out_pose = convert2CPoseStamped(cpp_pose);
      return true;
    }
    return false;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose)
{
  if (!handle)
  {
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_geometry_msgs::Pose2D cpp_pose;
    if (nav_ptr->getRobotPose(cpp_pose))
    {
      out_pose = convert2CPose2D(cpp_pose);
      return true;
    }
    return false;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist)
{
  if (!handle)
  {
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav_ptr->getTwist();
    out_twist = convert2CTwist2DStamped(cpp_twist);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback)
{
  if (!handle)
  {
    return false;
  }

  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot::move_base_core::NavFeedback *fb = nav_ptr->getFeedback();
    if (fb == nullptr)
      return false;
    out_feedback = convert2CNavFeedback(*fb);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

// ============================================================================
// Navigation Data Management
// ============================================================================

extern "C" bool navigation_add_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid occupancy_grid)
{
  if (!handle || !map_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot::log_info("occupancy_grid.data_count: %d", occupancy_grid.data_count);
    robot_nav_msgs::OccupancyGrid robot_occupancy_grid = convert2CppOccupancyGrid(occupancy_grid);

    nav_ptr->addStaticMap(std::string(map_name), robot_occupancy_grid);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan laser_scan)
{
  if (!handle || !laser_scan_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_sensor_msgs::LaserScan robot_laser_scan = convert2CppLaserScan(laser_scan);
    nav_ptr->addLaserScan(std::string(laser_scan_name), robot_laser_scan);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud)
{
  if (!handle || !point_cloud_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_sensor_msgs::PointCloud cpp_cloud = convert2CppPointCloud(point_cloud);
    nav_ptr->addPointCloud(std::string(point_cloud_name), cpp_cloud);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2)
{
  if (!handle || !point_cloud2_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_sensor_msgs::PointCloud2 cpp_cloud = convert2CppPointCloud2(point_cloud2);
    nav_ptr->addPointCloud2(std::string(point_cloud2_name), cpp_cloud);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry)
{
  if (!handle || !odometry_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
    {
      return false;
    }

    robot_nav_msgs::Odometry robot_odometry = convert2CppOdometry(odometry);
    nav_ptr->addOdometry(std::string(odometry_name), robot_odometry);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map)
{
  if (!handle || !map_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_nav_msgs::OccupancyGrid robot_map = nav_ptr->getStaticMap(std::string(map_name));
    convert2COccupancyGrid(robot_map, out_map);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan)
{
  if (!handle || !laser_scan_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_sensor_msgs::LaserScan robot_scan = nav_ptr->getLaserScan(std::string(laser_scan_name));
    convert2CLaserScan(robot_scan, out_scan);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud)
{
  if (!handle || !point_cloud_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_sensor_msgs::PointCloud robot_cloud = nav_ptr->getPointCloud(std::string(point_cloud_name));
    convert2CPointCloud(robot_cloud, out_cloud);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud)
{
  if (!handle || !point_cloud2_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot_sensor_msgs::PointCloud2 robot_cloud = nav_ptr->getPointCloud2(std::string(point_cloud2_name));
    convert2CPointCloud2(robot_cloud, out_cloud);
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count)
{
  if (!handle || !out_maps)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    auto maps = nav_ptr->getAllStaticMaps();
    out_count = maps.size();
    size_t i = 0;
    for (const auto &p : maps)
    {
      out_maps[i].name = strdup(p.first.c_str());
      convert2COccupancyGrid(p.second, out_maps[i].grid);
      ++i;
    }
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count)
{
  if (!handle || !out_scans)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    auto scans = nav_ptr->getAllLaserScans();
    if (scans.empty())
    {
      out_count = 0;
      return true;
    }
    out_count = scans.size();
    size_t i = 0;
    for (const auto &p : scans)
    {
      out_scans[i].name = strdup(p.first.c_str());
      convert2CLaserScan(p.second, out_scans[i].scan);
      ++i;
    }
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count)
{
  if (!handle || !out_clouds)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    auto clouds = nav_ptr->getAllPointClouds();
    if (clouds.empty())
    {
      out_count = 0;
      return true;
    }
    out_count = clouds.size();
    size_t i = 0;
    for (const auto &p : clouds)
    {
      out_clouds[i].name = strdup(p.first.c_str());
      convert2CPointCloud(p.second, out_clouds[i].cloud);
      ++i;
    }
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count)
{
  if (!handle || !out_clouds)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    auto clouds = nav_ptr->getAllPointCloud2s();
    if (clouds.empty())
    {
      out_count = 0;
      return true;
    }
    out_count = clouds.size();
    size_t i = 0;
    for (const auto &p : clouds)
    {
      out_clouds[i].name = strdup(p.first.c_str());
      convert2CPointCloud2(p.second, out_clouds[i].cloud);
      ++i;
    }
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char *map_name)
{
  if (!handle || !map_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removeStaticMap(std::string(map_name));
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char *laser_scan_name)
{
  if (!handle || !laser_scan_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removeLaserScan(std::string(laser_scan_name));
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const char *point_cloud_name)
{
  if (!handle || !point_cloud_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removePointCloud(std::string(point_cloud_name));
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const char *point_cloud2_name)
{
  if (!handle || !point_cloud2_name)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removePointCloud2(std::string(point_cloud2_name));
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle)
{
  if (!handle)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removeAllStaticMaps();
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle)
{
  if (!handle)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removeAllLaserScans();
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle)
{
  if (!handle)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removeAllPointClouds();
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle)
{
  if (!handle)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removeAllPointCloud2s();
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_remove_all_data(NavigationHandle handle)
{
  if (!handle)
  {
    return false;
  }
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
    return nav_ptr->removeAllData();
  }
  catch (...)
  {
    return false;
  }
}

// ============================================================================
// Planner Data
// ============================================================================
static void clear_planner_data(PlannerDataOutput *out)
{
  if (!out)
    return;
  // Free Path2D plan
  if (out->plan.poses)
  {
    for (size_t i = 0; i < out->plan.poses_count; i++)
    {
      if (out->plan.poses[i].header.frame_id)
        free(out->plan.poses[i].header.frame_id);
    }
    free(out->plan.poses);
    out->plan.poses = nullptr;
    out->plan.poses_count = 0;
  }
  if (out->plan.header.frame_id)
  {
    free(out->plan.header.frame_id);
    out->plan.header.frame_id = nullptr;
  }
  // Free OccupancyGrid costmap
  if (out->costmap.header.frame_id)
  {
    free(out->costmap.header.frame_id);
    out->costmap.header.frame_id = nullptr;
  }
  if (out->costmap.data)
  {
    free(out->costmap.data);
    out->costmap.data = nullptr;
  }
  // Free OccupancyGridUpdate
  if (out->costmap_update.header.frame_id)
  {
    free(out->costmap_update.header.frame_id);
    out->costmap_update.header.frame_id = nullptr;
  }
  if (out->costmap_update.data)
  {
    free(out->costmap_update.data);
    out->costmap_update.data = nullptr;
  }
  // Free PolygonStamped footprint
  if (out->footprint.header.frame_id)
  {
    free(out->footprint.header.frame_id);
    out->footprint.header.frame_id = nullptr;
  }
  if (out->footprint.polygon.points)
  {
    free(out->footprint.polygon.points);
    out->footprint.polygon.points = nullptr;
    out->footprint.polygon.points_count = 0;
  }
}

extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput *out_data)
{
  if (!handle || !out_data)
    return false;
  clear_planner_data(out_data);
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle),
            [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot::move_base_core::PlannerDataOutput data = nav_ptr->getGlobalData();
    convert2CPath2D(data.plan, out_data->plan);
    convert2COccupancyGrid(data.costmap, out_data->costmap);
    convert2COccupancyGridUpdate(data.costmap_update, out_data->costmap_update);
    convert2CPolygonStamped(data.footprint, out_data->footprint);
    out_data->is_costmap_updated = data.is_costmap_updated;
    return true;
  }
  catch (...)
  {
    clear_planner_data(out_data);
    return false;
  }
}

extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput *out_data)
{
  if (!handle || !out_data)
    return false;
  clear_planner_data(out_data);
  try
  {
    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
        std::shared_ptr<robot::move_base_core::BaseNavigation>(
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle),
            [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;

    robot::move_base_core::PlannerDataOutput data = nav_ptr->getLocalData();
    convert2CPath2D(data.plan, out_data->plan);
    convert2COccupancyGrid(data.costmap, out_data->costmap);
    convert2COccupancyGridUpdate(data.costmap_update, out_data->costmap_update);
    convert2CPolygonStamped(data.footprint, out_data->footprint);
    out_data->is_costmap_updated = data.is_costmap_updated;
    return true;
  }
  catch (...)
  {
    clear_planner_data(out_data);
    return false;
  }
}

