#ifndef NAVIGATION_C_API_2_H
#define NAVIGATION_C_API_2_H

/**
 * @file nav_c_api_2.h
 * @brief Additional C entry points required by the move_base2 navigation runtime.
 *
 * ## What this file is
 *
 * `nav_c_api.h` covers the navigation contract as the previous runtime exposed it. Every one of
 * those functions keeps working against move_base2 — move_base2 implements
 * `robot::move_base_core::BaseNavigation` with the signatures unchanged, so no existing binding
 * breaks. This file adds what that contract gained but the C surface never exposed, plus the two
 * entry points move_base2 makes newly necessary:
 *
 *  - navigation_shutdown()               — quiesce the core before destroying it.
 *  - navigation_is_ready()               — did initialize() actually succeed?
 *  - navigation_initialize_checked()     — initialize + verify, in one call.
 *  - navigation_get_state()              — poll the state without allocating a string.
 *  - navigation_get_status_text()        — the core's own explanation, for logging.
 *  - navigation_add_depth_camera_data()  — the depth-camera input path, never exposed before.
 *  - navigation_move_to_order_v2()       — an order carrying a real orderId / orderUpdateId.
 *  - navigation_dock_to_order_v2()       — same, for docking.
 *
 * Include this header **in addition to** nav_c_api.h. It adds symbols; it replaces none.
 *
 * ## What changed underneath the functions you already call
 *
 * These keep their signatures and their meaning, but behave differently enough to matter:
 *
 *  1. navigation_initialize() is now heavy and blocking — it builds both costmaps, loads the
 *     planners, the controller, the recovery behaviours and the mission layer, then starts the
 *     control thread. It still returns `true` unconditionally because the underlying contract
 *     returns void. Use navigation_initialize_checked(), or call navigation_is_ready() after it.
 *
 *  2. A static map must be pushed **before** initialize(). The lattice planner blocks until the
 *     costmap has a non-zero size, and that size only exists once a static map has reached the
 *     static layer. It aborts the whole process if it waits too long.
 *
 *  3. navigation_set_robot_footprint() may now be called before initialize() — the footprint is
 *     cached and applied while the costmaps are built. Under the old runtime the same call was
 *     dropped silently because the costmaps did not exist yet.
 *
 *  4. navigation_pause() / _resume() / _cancel() no longer take effect inside the call. They record
 *     the request; the control thread applies it on its next cycle. Returning from the call does
 *     not mean the robot stopped — watch the state, or watch the twist.
 *
 *  5. navigation_set_twist_linear() / _angular() set a velocity **ceiling**, and the sign of the
 *     linear x component selects which direction the ceiling applies to. They now only reject
 *     NaN/Inf; the old runtime also returned false while a cancel was in flight, and that signal no
 *     longer exists. The new ceiling takes effect on the next control cycle.
 *
 *  6. navigation_get_twist() returns the command being issued, not a measurement. While no request
 *     is running its stamp stops advancing; a zero stamp means the core has never commanded
 *     anything. Check the stamp before republishing it as a velocity command.
 *
 *  7. NavFeedback.feed_back_str now carries the core's own reason strings — different text from the
 *     old runtime. Branch on NavigationState, never on the string.
 *
 *  8. NavFeedback.navigation_state deliberately reports ACTIVE between the legs of a multi-leg
 *     order, where the core itself is momentarily SUCCEEDED or PENDING. Treating SUCCEEDED as "the
 *     order finished" stays correct only because of that; do not bypass it.
 *
 *  9. navigation_add_odometry() feeds the controller's measured velocity every cycle, not just a
 *     stored value. Push it at the rate the odometry source produces it.
 *
 * 10. The `footprint` field of navigation_get_global_data() / _local_data() is the raw footprint the
 *     host set, in the robot base frame, and it is empty if the host never set one. The old runtime
 *     returned the padded footprint already transformed to the robot pose in the global frame.
 *
 * 11. navigation_destroy() alone is no longer enough — call navigation_shutdown() first, from a
 *     point in your own shutdown sequence where the process is still fully alive.
 *
 * See NAV_C_API_2_GUIDE.md in this directory for the per-function walkthrough and the startup and
 * shutdown sequences.
 */

#ifdef __cplusplus
extern "C"
{
#endif

#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>

#include "nav_c_api.h"
#include "sensor_msgs/DepthCameraData.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "nav_msgs/OccupancyGrid.h"
#include "protocol_msgs/Node.h"
#include "protocol_msgs/Edge.h"

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

  /**
   * @brief Initialize the navigation core and report whether it actually came up.
   *
   * Wraps navigation_initialize(). The underlying contract method returns void, so every failure
   * path inside the runtime — costmap construction, sensor configuration, plugin loading, control
   * loop configuration, control thread startup — returns normally and only records the reason in
   * the feedback. This function reads that back and turns it into a return value.
   *
   * Blocking and expensive: it builds two costmaps, loads every plugin named in the YAML, starts
   * the map-update threads, the mission threads and the control thread. Do not call it from a UI
   * thread.
   *
   * @param handle       Navigation handle from navigation_create().
   * @param tf3_buffer   tf3 BufferCore the host owns; must outlive the navigation instance.
   * @return true only when the core is ready to accept goals.
   *
   * @pre At least one static map has been pushed with navigation_add_static_map().
   * @pre The robot footprint has been set with navigation_set_robot_footprint(), if the host wants
   *      its own footprint rather than the one in the YAML.
   * @see navigation_get_status_text() for the reason behind a false return.
   */
  bool navigation_initialize_checked(NavigationHandle handle, TFListenerHandle tf3_buffer);

  /**
   * @brief Is the core initialized and accepting goals?
   *
   * Cheap enough to poll. Reads the same flag navigation_initialize_checked() checks, without
   * allocating the feedback string.
   *
   * @return false if the handle is null, the core never initialized, or initialization failed.
   */
  bool navigation_is_ready(NavigationHandle handle);

  /**
   * @brief Stop the core's threads without destroying the instance.
   *
   * Gives the host a point inside its own shutdown sequence — while the process is still fully
   * alive — to stop the control thread, the costmap update threads and the mission threads.
   * Destroying the instance from a finalizer, after the runtime has already begun tearing down, is
   * the race this exists to avoid.
   *
   * After this call the core issues no further velocity commands, and every getter
   * (navigation_get_feedback, navigation_get_twist, navigation_get_robot_pose_*) remains safe to
   * call. Safe to call more than once.
   *
   * @warning No thread may be calling navigation_add_* while this runs, and none may call them
   *          afterwards. The sensor path is not thread-safe and there is no longer a control thread
   *          consuming what it produces.
   */
  void navigation_shutdown(NavigationHandle handle);

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

  /**
   * @brief Current navigation state, without allocating anything.
   *
   * navigation_get_feedback() strdup()s its message on every call, so polling it at control rate
   * means one allocation and one free per poll — and one leak per missed free. Use this when only
   * the state matters, which is the common case.
   *
   * @param handle    Navigation handle.
   * @param out_state Receives the current state; untouched on failure.
   * @return false if the handle is null or the core has no feedback yet.
   */
  bool navigation_get_state(NavigationHandle handle, NavigationState *out_state);

  /**
   * @brief The core's own description of what it is doing, or why it refused.
   *
   * For logging and diagnostics only. The wording comes from the runtime and changes between
   * versions — never branch on it. Branch on navigation_get_state().
   *
   * @return Newly allocated string, or NULL. Free with nav_c_api_free_string().
   */
  char *navigation_get_status_text(NavigationHandle handle);

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

  /**
   * @brief Push one coherent depth-camera sample into the navigation core.
   *
   * This is the only way to reach the depth-camera path from C. The voxel layer projects the depth
   * image through the intrinsics to build the camera frustum, which is what lets it *clear* stale
   * obstacles inside the field of view. A point cloud cannot substitute: it carries no intrinsics,
   * so no frustum can be reconstructed and nothing gets cleared.
   *
   * @param handle Navigation handle.
   * @param topic  Logical name of the stream. Must match an `observation_source` declared for the
   *               voxel layer in the costmap YAML — a name no layer subscribes to is accepted here
   *               and then dropped, silently, further down.
   * @param data   Depth image and its matching intrinsics. Deep-copied; the caller may release its
   *               buffers as soon as this returns.
   * @return true if the sample was accepted.
   *
   * @pre `data.header.frame_id` is the camera's optical frame, and a transform from it to the robot
   *      base frame exists in the tf3 buffer at `data.header.stamp`.
   * @pre `data.depth.encoding` matches the real encoding ("16UC1" or "32FC1") and `step` is the
   *      real row stride in bytes.
   * @pre `data.camera_info` describes the resolution actually being sent, not the colour stream's.
   * @pre The depth image and the camera info come from the same instant.
   *
   * @note Push at the camera's real rate. This is the most expensive sensor path in the core;
   *       pushing faster than the source only burns CPU.
   */
  bool navigation_add_depth_camera_data(NavigationHandle handle, const char *topic,
                                        const DepthCameraData data);

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

  /**
   * @brief Send a multi-node order with a real identity.
   *
   * Under move_base2 an order goes through the mission layer, which splits it into legs at the
   * nodes that carry actions, filters by `released`, and can extend the route when the fleet master
   * releases more of the horizon. `orderId` plus `orderUpdateId` is what identifies an order there:
   * the same `orderId` with a higher `orderUpdateId` is an update of the route in progress, while a
   * different `orderId` is a new order that replaces it.
   *
   * navigation_move_to_nodes_edges() cannot express that — it has nowhere to put either field — so
   * every call looks like a different order and no update can ever be sent. Use this instead
   * whenever the fleet master sends order updates or releases the horizon progressively.
   *
   * @param handle           Navigation handle.
   * @param order_id         VDA5050 orderId. Must not be NULL or empty.
   * @param order_update_id  VDA5050 orderUpdateId. Must increase within one orderId.
   * @param nodes            Node array; the host keeps ownership.
   * @param node_count       Number of nodes.
   * @param edges            Edge array; the host keeps ownership.
   * @param edge_count       Number of edges.
   * @param goal             Final pose, in the global frame.
   * @return true if the order was accepted.
   */
  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);

  /**
   * @brief Docking counterpart of navigation_move_to_order_v2().
   *
   * @param handle           Navigation handle.
   * @param marker           Docking marker name or id. Must not be NULL.
   * @param order_id         VDA5050 orderId. Must not be NULL or empty.
   * @param order_update_id  VDA5050 orderUpdateId.
   * @param nodes            Node array; the host keeps ownership.
   * @param node_count       Number of nodes.
   * @param edges            Edge array; the host keeps ownership.
   * @param edge_count       Number of edges.
   * @param goal             Docking pose.
   * @return true if the docking order was accepted.
   */
  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);

  // ============================================================================================
  // Releasing what the getters allocate
  // ============================================================================================
  //
  // Every navigation_get_* function that returns a grid, a scan, a cloud or a plan fills it with
  // buffers allocated inside the library. Only two release helpers ever shipped —
  // nav_c_api_free_string() and order_free() — so the rest had no counterpart and leaked one
  // allocation per call. At four display timers polling at 10 Hz that is a steady leak, not a
  // rounding error. These close it.
  //
  // All are null-safe, clear the pointers and counts they release, and are safe to call twice.

  /** @brief Release the buffers inside an OccupancyGrid filled by navigation_get_static_map(). */
  void navigation_free_occupancy_grid(OccupancyGrid *grid);

  /** @brief Release the buffers inside a LaserScan filled by navigation_get_laser_scan(). */
  void navigation_free_laser_scan(LaserScan *scan);

  /** @brief Release the buffers inside a PointCloud filled by navigation_get_point_cloud(). */
  void navigation_free_point_cloud(PointCloud *cloud);

  /** @brief Release the buffers inside a PointCloud2 filled by navigation_get_point_cloud2(). */
  void navigation_free_point_cloud2(PointCloud2 *cloud);

  /**
   * @brief Release the buffers inside a PlannerDataOutput.
   *
   * navigation_get_global_data() / _local_data() already release the previous contents of the
   * struct they are handed, so reusing one struct across polls does not leak. What does leak is the
   * last fill, once the host stops polling — free it here.
   */
  void navigation_free_planner_data(PlannerDataOutput *data);

#ifdef __cplusplus
}
#endif

#endif // NAVIGATION_C_API_2_H
