/**
 * @file nav_c_api_2.cpp
 * @brief Implementation of the move_base2 additions to the C navigation API.
 *
 * Deliberately separate from nav_c_api.cpp: the surface the existing binding depends on stays byte
 * for byte where it is, and everything move_base2 needs on top of it lands here, where it can be
 * reviewed — or dropped — on its own.
 *
 * The handle is the same opaque `robot::move_base_core::BaseNavigation*` navigation_create()
 * returns; ownership still lives in nav_c_api.cpp, so nothing here creates or frees an instance.
 */

#include "nav_c_api_2.h"

#include <cstdlib>
#include <cstring>
#include <string>

#include <boost/make_shared.hpp>

#include <robot/robot.h>
#include <move_base_core/common.h>
#include <move_base_core/navigation.h>
#include <robot_sensor_msgs/DepthCameraData.h>

#include "convertor.h"
#include "convertor_2.h"

namespace
{

/// Non-owning view of the handle. The instance is owned by the table in nav_c_api.cpp.
inline robot::move_base_core::BaseNavigation *as_navigation(NavigationHandle handle)
{
  return static_cast<robot::move_base_core::BaseNavigation *>(handle);
}

/**
 * @brief Build a C Order from an id plus the node/edge arrays the host owns.
 *
 * Zero-initialized on purpose. Every optional field of Order is a char*, and the converter treats a
 * non-null pointer as a valid string — an uninitialized struct therefore hands it stack garbage to
 * build std::string from. The pointers below stay non-owning: the converter copies before this
 * returns, and the arrays are never freed here.
 */
Order make_order(const char *order_id, uint32_t order_update_id,
                 const Node *nodes, size_t node_count,
                 const Edge *edges, size_t edge_count)
{
  Order order{};
  order.orderId = const_cast<char *>(order_id);
  order.orderUpdateId = order_update_id;
  order.nodes = const_cast<Node *>(nodes);
  order.nodes_count = nodes != nullptr ? node_count : 0;
  order.edges = const_cast<Edge *>(edges);
  order.edges_count = edges != nullptr ? edge_count : 0;
  return order;
}

}  // namespace

// ================================================================================================
// Lifecycle
// ================================================================================================

extern "C" bool navigation_initialize_checked(NavigationHandle handle, TFListenerHandle tf3_buffer)
{
  if (!handle || !tf3_buffer)
  {
    robot::log_error("navigation_initialize_checked: null handle or tf3 buffer");
    return false;
  }

  try
  {
    auto *nav = as_navigation(handle);

    // The host owns the buffer and must outlive the core; hand over a non-owning alias.
    robot::TFListenerPtr tf(static_cast<tf3::BufferCore *>(tf3_buffer),
                            [](tf3::BufferCore *) {});

    nav->initialize(tf);

    // initialize() returns void, so the runtime reports failure the only way it can: it leaves
    // is_ready false and writes the reason into the feedback. Not reading it back is how a host
    // ends up pushing goals into a core that has no control thread.
    robot::move_base_core::NavFeedback *feedback = nav->getFeedback();
    if (feedback == nullptr)
    {
      robot::log_error("navigation_initialize_checked: core exposes no feedback");
      return false;
    }
    if (!feedback->is_ready)
    {
      robot::log_error("navigation_initialize_checked: core not ready: %s",
                       feedback->feed_back_str.c_str());
      return false;
    }
    return true;
  }
  catch (const std::exception &e)
  {
    robot::log_error("navigation_initialize_checked failed: %s", e.what());
    return false;
  }
  catch (...)
  {
    robot::log_error("navigation_initialize_checked failed: unknown exception");
    return false;
  }
}

extern "C" bool navigation_is_ready(NavigationHandle handle)
{
  if (!handle)
  {
    return false;
  }

  try
  {
    robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
    return feedback != nullptr && feedback->is_ready;
  }
  catch (...)
  {
    return false;
  }
}

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

  try
  {
    as_navigation(handle)->shutdown();
  }
  catch (const std::exception &e)
  {
    // Swallowed on purpose: this runs inside the host's shutdown sequence, where throwing across
    // the C boundary would abort a process that is already on its way down.
    robot::log_error("navigation_shutdown failed: %s", e.what());
  }
  catch (...)
  {
    robot::log_error("navigation_shutdown failed: unknown exception");
  }
}

// ================================================================================================
// Status
// ================================================================================================

extern "C" bool navigation_get_state(NavigationHandle handle, NavigationState *out_state)
{
  if (!handle || !out_state)
  {
    return false;
  }

  try
  {
    robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
    if (feedback == nullptr)
    {
      return false;
    }
    *out_state = static_cast<NavigationState>(static_cast<int>(feedback->navigation_state));
    return true;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" char *navigation_get_status_text(NavigationHandle handle)
{
  if (!handle)
  {
    return nullptr;
  }

  try
  {
    robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
    if (feedback == nullptr || feedback->feed_back_str.empty())
    {
      return nullptr;
    }
    return strdup(feedback->feed_back_str.c_str());
  }
  catch (...)
  {
    return nullptr;
  }
}

// ================================================================================================
// Sensor input
// ================================================================================================

extern "C" bool navigation_add_depth_camera_data(NavigationHandle handle, const char *topic,
                                                 const DepthCameraData data)
{
  if (!handle || !topic)
  {
    return false;
  }

  try
  {
    // ConstPtr is a boost::shared_ptr and the core keeps the sample for as long as it needs it, so
    // hand it a copy rather than a view of host memory.
    auto sample = boost::make_shared<robot_sensor_msgs::DepthCameraData>(
        convert2CppDepthCameraData(data));

    as_navigation(handle)->addDepthCameraData(std::string(topic), sample);
    return true;
  }
  catch (const std::exception &e)
  {
    robot::log_error("navigation_add_depth_camera_data failed: %s", e.what());
    return false;
  }
  catch (...)
  {
    return false;
  }
}

// ================================================================================================
// Orders
// ================================================================================================

extern "C" bool navigation_move_to_order_v2(NavigationHandle handle,
                                            const char *order_id, uint32_t order_update_id,
                                            const Node *nodes, size_t node_count,
                                            const Edge *edges, size_t edge_count,
                                            const PoseStamped goal)
{
  if (!handle || order_id == nullptr || order_id[0] == '\0')
  {
    robot::log_error("navigation_move_to_order_v2: null handle or empty orderId");
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("navigation_move_to_order_v2: goal quaternion is invalid");
    return false;
  }

  try
  {
    const Order order = make_order(order_id, order_update_id, nodes, node_count, edges, edge_count);
    return as_navigation(handle)->moveTo(convert2CppOrder(order), convert2CppPoseStamped(goal));
  }
  catch (const std::exception &e)
  {
    robot::log_error("navigation_move_to_order_v2 failed: %s", e.what());
    return false;
  }
  catch (...)
  {
    return false;
  }
}

extern "C" bool navigation_dock_to_order_v2(NavigationHandle handle, const char *marker,
                                            const char *order_id, uint32_t order_update_id,
                                            const Node *nodes, size_t node_count,
                                            const Edge *edges, size_t edge_count,
                                            const PoseStamped goal)
{
  if (!handle || !marker || order_id == nullptr || order_id[0] == '\0')
  {
    robot::log_error("navigation_dock_to_order_v2: null handle, marker or empty orderId");
    return false;
  }
  if (!isQuaternionValid(goal.pose.orientation))
  {
    robot::log_error("navigation_dock_to_order_v2: goal quaternion is invalid");
    return false;
  }

  try
  {
    const Order order = make_order(order_id, order_update_id, nodes, node_count, edges, edge_count);
    return as_navigation(handle)->dockTo(convert2CppOrder(order), std::string(marker),
                                         convert2CppPoseStamped(goal));
  }
  catch (const std::exception &e)
  {
    robot::log_error("navigation_dock_to_order_v2 failed: %s", e.what());
    return false;
  }
  catch (...)
  {
    return false;
  }
}

// ================================================================================================
// Releasing what the getters allocate
// ================================================================================================
//
// The converters allocate with malloc()/strdup(), so these release with free(). Mixing in delete[]
// here would be undefined behaviour, and the host cannot free these itself on Windows-style
// runtimes anyway — allocation and release must happen inside the same C runtime.

namespace
{

/// free() a buffer and clear both the pointer and its element count in one step.
template <typename T>
inline void free_array(T *&ptr, size_t &count)
{
  free(ptr);
  ptr = nullptr;
  count = 0;
}

inline void free_string_field(char *&str)
{
  free(str);
  str = nullptr;
}

}  // namespace

extern "C" void navigation_free_occupancy_grid(OccupancyGrid *grid)
{
  if (!grid)
  {
    return;
  }
  free_string_field(grid->header.frame_id);
  free_array(grid->data, grid->data_count);
}

extern "C" void navigation_free_laser_scan(LaserScan *scan)
{
  if (!scan)
  {
    return;
  }
  free_string_field(scan->header.frame_id);
  free_array(scan->ranges, scan->ranges_count);
  free_array(scan->intensities, scan->intensities_count);
}

extern "C" void navigation_free_point_cloud(PointCloud *cloud)
{
  if (!cloud)
  {
    return;
  }
  free_string_field(cloud->header.frame_id);
  free_array(cloud->points, cloud->points_count);

  // Each channel owns a name and a value buffer of its own; freeing the array alone leaks both.
  if (cloud->channels != nullptr)
  {
    for (size_t i = 0; i < cloud->channels_count; ++i)
    {
      free_string_field(cloud->channels[i].name);
      free_array(cloud->channels[i].values, cloud->channels[i].values_count);
    }
  }
  free_array(cloud->channels, cloud->channels_count);
}

extern "C" void navigation_free_point_cloud2(PointCloud2 *cloud)
{
  if (!cloud)
  {
    return;
  }
  free_string_field(cloud->header.frame_id);

  if (cloud->fields != nullptr)
  {
    for (size_t i = 0; i < cloud->fields_count; ++i)
    {
      free_string_field(cloud->fields[i].name);
    }
  }
  free_array(cloud->fields, cloud->fields_count);
  free_array(cloud->data, cloud->data_count);
}

extern "C" void navigation_free_planner_data(PlannerDataOutput *data)
{
  if (!data)
  {
    return;
  }

  if (data->plan.poses != nullptr)
  {
    for (size_t i = 0; i < data->plan.poses_count; ++i)
    {
      free_string_field(data->plan.poses[i].header.frame_id);
    }
  }
  free_array(data->plan.poses, data->plan.poses_count);
  free_string_field(data->plan.header.frame_id);

  navigation_free_occupancy_grid(&data->costmap);

  free_string_field(data->costmap_update.header.frame_id);
  free_array(data->costmap_update.data, data->costmap_update.data_count);

  free_string_field(data->footprint.header.frame_id);
  free_array(data->footprint.polygon.points, data->footprint.polygon.points_count);

  data->is_costmap_updated = false;
}
