#ifndef NAVIGATION_C_API_H
#define NAVIGATION_C_API_H

#ifdef __cplusplus
extern "C"
{
#endif

  #include <stdbool.h>
  #include <stdint.h>
  #include <stddef.h>
  /* C struct types only - do not include convertor.h here (it pulls C++ code into extern "C") */
  #include "std_msgs/Header.h"
  #include "geometry_msgs/Point.h"
  #include "geometry_msgs/Pose2D.h"
  #include "geometry_msgs/PoseStamped.h"
  #include "nav_msgs/OccupancyGrid.h"
  #include "nav_msgs/Odometry.h"
  #include "map_msgs/OccupancyGridUpdate.h"
  #include "nav_2d_msgs/Twist2D.h"
  #include "nav_2d_msgs/Twist2DStamped.h"
  #include "nav_2d_msgs/Path2D.h"
  #include "sensor_msgs/LaserScan.h"
  #include "sensor_msgs/PointCloud.h"
  #include "sensor_msgs/PointCloud2.h"
  #include "geometry_msgs/PolygonStamped.h"
  #include "protocol_msgs/Order.h"

  typedef struct { char *name; OccupancyGrid grid; } NamedOccupancyGrid;
  typedef struct { char *name; LaserScan scan; } NamedLaserScan;
  typedef struct { char *name; PointCloud cloud; } NamedPointCloud;
  typedef struct { char *name; PointCloud2 cloud; } NamedPointCloud2;
  typedef void* NavigationHandle;
  typedef void* TFListenerHandle;
  
  typedef enum
  {
    PENDING = 0,
    ACTIVE = 1,
    PREEMPTED = 2,
    SUCCEEDED = 3,
    ABORTED = 4,
    REJECTED = 5,
    PREEMPTING = 6,
    RECALLING = 7,
    RECALLED = 8,
    LOST = 9,
    PLANNING = 10,
    CONTROLLING = 11,
    CLEARING = 12,
    PAUSED = 13
  } NavigationState;
  
  typedef struct
  {
    NavigationState navigation_state;
    char *feed_back_str;
    Pose2D current_pose;
    bool goal_checked;
    bool is_ready;
  } NavFeedback;

  /**
   * @brief Planner data output structure (C version).
   * Mirrors robot::move_base_core::PlannerDataOutput in `move_base_core/navigation.h`.
   */
  typedef struct
  {
    Path2D plan;
    OccupancyGrid costmap;
    OccupancyGridUpdate costmap_update;
    bool is_costmap_updated;
    PolygonStamped footprint;
  } PlannerDataOutput;

  /**
   * @brief Free a string allocated by the API (e.g. feed_back_str, navigation_state_to_string result).
   * @param str Pointer from strdup; no-op if NULL.
   */
  void nav_c_api_free_string(char *str);

  /**
   * @brief Create a new navigation instance
   * @return Navigation handle, or NULL on failure
   */
  NavigationHandle navigation_create(void);

  /**
   * @brief Destroy a navigation instance
   * @param handle Navigation handle to destroy
   */
  void navigation_destroy(NavigationHandle handle);

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

  /**
   * @brief Initialize the navigation system using an existing tf3 buffer (from libtf3 tf3_buffer_create).
   * Caller retains ownership of the buffer and must call tf3_buffer_destroy when done (after navigation_destroy).
   * @param handle Navigation handle
   * @param tf3_buffer Pointer to tf3 BufferCore (TF3_BufferCore from libtf3)
   * @return true on success, false on failure
   */
  bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf3_buffer);

  /**
   * @brief Set the robot's footprint (outline shape)
   * @param handle Navigation handle
   * @param points Array of points representing the footprint polygon
   * @param point_count Number of points in the array
   * @return true on success, false on failure
   */
  bool navigation_set_robot_footprint(NavigationHandle handle, const Point *points, size_t point_count);

  /**
   * @brief Get the robot's footprint (outline shape)
   * @param handle Navigation handle
   * @param out_points Output array of points (allocated by library, free with navigation_free_points)
   * @param out_count Output number of points in the array
   * @return true on success, false on failure
   */
  bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count);


  /**
   * @brief Send a goal for the robot to navigate to
   * @param handle Navigation handle
   * @param goal Target pose in the global frame
   * @return true if goal was accepted and sent successfully
   */
  bool navigation_move_to(NavigationHandle handle, const PoseStamped goal);

  /**
   * @brief Send a goal for the robot to navigate to
   * @param handle Navigation handle
   * @param order Order message
   * @param goal Target pose in the global frame
   * @return true if goal was accepted and sent successfully
   * @note If order was obtained from convert2COrder(), call order_free(&order) when done
   */
  bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal);

  /**
   * @brief Send a goal for the robot to navigate to
   * @param handle Navigation handle
   * @param nodes Nodes array
   * @param node_count Number of nodes in the array
   * @param edges Edges array
   * @param edge_count Number of edges in the array
   * @param goal Target pose in the global frame
   * @return true if goal was accepted and sent successfully
   */
  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);

  /**
   * @brief Send a docking goal to a predefined marker
   * @param handle Navigation handle
   * @param marker Marker name or ID (null-terminated string)
   * @param goal Target pose for docking
   * @return true if docking command succeeded
   */
  bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal);

  /**
   * @brief Send a docking goal to a predefined marker
   * @param handle Navigation handle
   * @param order Order message
   * @param goal Target pose for docking
   * @return true if docking command succeeded
   */
  bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal);

    /**
   * @brief Send a goal for the robot to navigate to
   * @param handle Navigation handle
   * @param marker Marker name or ID (null-terminated string)
   * @param nodes Nodes array
   * @param node_count Number of nodes in the array
   * @param edges Edges array
   * @param edge_count Number of edges in the array
   * @param goal Target pose in the global frame
   * @return true if goal was accepted and sent successfully
   */
   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);


  /**
   * @brief Move straight toward the target position
   * @param handle Navigation handle
   * @param distance Distance to move (meters)
   * @return true if command issued successfully
   */
  bool navigation_move_straight_to(NavigationHandle handle, const double distance);

  /**
   * @brief Rotate in place to align with target orientation
   * @param handle Navigation handle
   * @param goal_yaw Desired heading (radians)
   * @return true if rotation command was sent successfully
   */
  bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal);

  /**
   * @brief Pause the robot's movement
   * @param handle Navigation handle
   */
  void navigation_pause(NavigationHandle handle);

  /**
   * @brief Resume motion after a pause
   * @param handle Navigation handle
   */
  void navigation_resume(NavigationHandle handle);

  /**
   * @brief Cancel the current goal and stop the robot
   * @param handle Navigation handle
   */
  void navigation_cancel(NavigationHandle handle);

  /**
   * @brief Send limited linear velocity command
   * @param handle Navigation handle
   * @param linear_x Linear velocity in X direction
   * @param linear_y Linear velocity in Y direction
   * @param linear_z Linear velocity in Z direction
   * @return true if the command was accepted
   */
  bool navigation_set_twist_linear(NavigationHandle handle,
                                   double linear_x, double linear_y, double linear_z);

  /**
   * @brief Send limited angular velocity command
   * @param handle Navigation handle
   * @param angular_x Angular velocity around X axis
   * @param angular_y Angular velocity around Y axis
   * @param angular_z Angular velocity around Z axis
   * @return true if the command was accepted
   */
  bool navigation_set_twist_angular(NavigationHandle handle,
                                    double angular_x, double angular_y, double angular_z);

  /**
   * @brief Get the robot's pose as a PoseStamped
   * @param handle Navigation handle
   * @param out_pose Output parameter with the robot's current pose
   * @return true if pose was successfully retrieved
   */
  bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose);

  /**
   * @brief Get the robot's pose as a 2D pose
   * @param handle Navigation handle
   * @param out_pose Output parameter with the robot's current 2D pose
   * @return true if pose was successfully retrieved
   */
  bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose);

  /**
   * @brief Get the robot's current twist
   * @param handle Navigation handle
   * @param out_twist Output parameter with the robot's current twist
   * @return true if twist was successfully retrieved
   * @note out_twist->header.frame_id must be freed using nav_c_api_free_string
   */
  bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist);

  /**
   * @brief Get navigation feedback
   * @param handle Navigation handle
   * @param out_feedback Output parameter with navigation feedback
   * @return true if feedback was successfully retrieved
   * @note The feed_back_str field must be freed using nav_c_api_free_string
   */
  bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback);

  /**
   * @brief Add a static map to the navigation system
   * @param handle Navigation handle
   * @param map_name Name of the map
   * @param map Occupancy grid handle
   * @return true if the map was added successfully
   */
  bool navigation_add_static_map(NavigationHandle handle, const char *map_name, const OccupancyGrid occupancy_grid);

  /**
   * @brief Add a laser scan to the navigation system
   * @param handle Navigation handle
   * @param laser_scan_name Name of the laser scan
   * @param laser_scan Laser scan handle
   * @return true if the laser scan was added successfully
   */
  bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, const LaserScan laser_scan);

  /**
   * @brief Add a point cloud to the navigation system
   * @param handle Navigation handle
   * @param point_cloud_name Name of the point cloud
   * @param point_cloud Point cloud handle
   * @return true if the point cloud was added successfully
   */
  bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud);

  /**
   * @brief Add a point cloud2 to the navigation system
   * @param handle Navigation handle
   * @param point_cloud2_name Name of the point cloud2
   * @param point_cloud2 Point cloud2 handle
   * @return true if the point cloud2 was added successfully
   */
  bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2);

  /**
   * @brief Add an odometry to the navigation system
   * @param handle Navigation handle
   * @param odometry_name Name of the odometry
   * @param odometry Odometry handle
   * @return true if the odometry was added successfully
   */
  bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry);

  /**
   * @brief Get a static map from the navigation system
   * @param handle Navigation handle
   * @param map_name Name of the map
   * @param out_map Output parameter for the map handle
   * @return true if the map was retrieved successfully
   */
  bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map);

  /**
   * @brief Get a laser scan from the navigation system
   * @param handle Navigation handle
   * @param laser_scan_name Name of the laser scan
   * @param out_scan Output parameter for the laser scan handle
   * @return true if the laser scan was retrieved successfully
   */
  bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan);

  /**
   * @brief Get a point cloud from the navigation system
   * @param handle Navigation handle
   * @param point_cloud_name Name of the point cloud
   * @param out_cloud Output parameter for the point cloud handle
   * @return true if the point cloud was retrieved successfully
   */
  bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud);

  /**
   * @brief Get a point cloud2 from the navigation system
   * @param handle Navigation handle
   * @param point_cloud2_name Name of the point cloud2
   * @param out_cloud Output parameter for the point cloud2 handle
   * @return true if the point cloud2 was retrieved successfully
   */
  bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud);

  /**
   * @brief Get all static maps from the navigation system
   * @param handle Navigation handle
   * @param out_maps Output parameter for the maps array
   * @param out_count Output parameter for the number of maps
   * @return true if the maps were retrieved successfully
   */
  bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count);

  /**
   * @brief Get all laser scans from the navigation system
   * @param handle Navigation handle
   * @param out_scans Output parameter for the scans array
   * @param out_count Output parameter for the number of scans
   * @return true if the scans were retrieved successfully
   */
  bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count);

  /**
   * @brief Get all point clouds from the navigation system
   * @param handle Navigation handle
   * @param out_clouds Output parameter for the clouds array
   * @param out_count Output parameter for the number of clouds
   * @return true if the clouds were retrieved successfully
   */
  bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count);

  /**
   * @brief Get all point cloud2s from the navigation system
   * @param handle Navigation handle
   * @param out_clouds Output parameter for the clouds array
   * @param out_count Output parameter for the number of clouds
   * @return true if the clouds were retrieved successfully
   */
  bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count);

  /**
   * @brief Remove a static map from the navigation system
   * @param handle Navigation handle
   * @param map_name Name of the map
   * @return true if the map was removed successfully
   */
  bool navigation_remove_static_map(NavigationHandle handle, const char *map_name);

  /**
   * @brief Remove a laser scan from the navigation system
   * @param handle Navigation handle
   * @param laser_scan_name Name of the laser scan
   * @return true if the laser scan was removed successfully
   */
  bool navigation_remove_laser_scan(NavigationHandle handle, const char *laser_scan_name);

  /**
   * @brief Remove a point cloud from the navigation system
   * @param handle Navigation handle
   * @param point_cloud_name Name of the point cloud
   * @return true if the point cloud was removed successfully
   */
  bool navigation_remove_point_cloud(NavigationHandle handle, const char *point_cloud_name);

  /**
   * @brief Remove a point cloud2 from the navigation system
   * @param handle Navigation handle
   * @param point_cloud2_name Name of the point cloud2
   * @return true if the point cloud2 was removed successfully
   */
  bool navigation_remove_point_cloud2(NavigationHandle handle, const char *point_cloud2_name);

  /**
   * @brief Remove all static maps from the navigation system
   * @param handle Navigation handle
   * @return true if the maps were removed successfully
   */
  bool navigation_remove_all_static_maps(NavigationHandle handle);

  /**
   * @brief Remove all laser scans from the navigation system
   * @param handle Navigation handle
   * @return true if the scans were removed successfully
   */
  bool navigation_remove_all_laser_scans(NavigationHandle handle);

  /**
   * @brief Remove all point clouds from the navigation system
   * @param handle Navigation handle
   * @return true if the clouds were removed successfully
   */
  bool navigation_remove_all_point_clouds(NavigationHandle handle);

  /**
   * @brief Remove all point cloud2s from the navigation system
   * @param handle Navigation handle
   * @return true if the cloud2s were removed successfully
   */
  bool navigation_remove_all_point_cloud2s(NavigationHandle handle);

  /**
   * @brief Remove all data from the navigation system
   * @param handle Navigation handle
   * @return true if the data was removed successfully
   */
  bool navigation_remove_all_data(NavigationHandle handle);

  /**
   * @brief Get the global data from the navigation system
   * @param handle Navigation handle
   * @param out_data Output parameter for the data
   * @return true if the data was retrieved successfully
   */
  bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput *out_data);

  /**
   * @brief Get the local data from the navigation system
   * @param handle Navigation handle
   * @param out_data Output parameter for the data
   * @return true if the data was retrieved successfully
   */
  bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput *out_data);

#ifdef __cplusplus
}
#endif

#endif // NAVIGATION_C_API_H
