#ifndef NAVIGATION_C_API_H
#define NAVIGATION_C_API_H

#ifdef __cplusplus
extern "C" {
#endif

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

// Forward declarations
typedef void* NavigationHandle;
typedef void* TFListenerHandle;

// ============================================================================
// Enums
// ============================================================================

/**
 * @brief Navigation states, including planning and controller status
 */
typedef enum {
    NAV_STATE_PENDING = 0,
    NAV_STATE_ACTIVE = 1,
    NAV_STATE_PREEMPTED = 2,
    NAV_STATE_SUCCEEDED = 3,
    NAV_STATE_ABORTED = 4,
    NAV_STATE_REJECTED = 5,
    NAV_STATE_PREEMPTING = 6,
    NAV_STATE_RECALLING = 7,
    NAV_STATE_RECALLED = 8,
    NAV_STATE_LOST = 9,
    NAV_STATE_PLANNING = 10,
    NAV_STATE_CONTROLLING = 11,
    NAV_STATE_CLEARING = 12,
    NAV_STATE_PAUSED = 13
} NavigationState;

// ============================================================================
// Structures
// ============================================================================

/**
 * @brief Point structure (x, y, z)
 */
typedef struct {
    double x;
    double y;
    double z;
} Point;

/**
 * @brief Pose2D structure (x, y, theta)
 */
typedef struct {
    double x;
    double y;
    double theta;
} Pose2D;

/**
 * @brief Quaternion structure
 */
typedef struct {
    double x;
    double y;
    double z;
    double w;
} Quaternion;

/**
 * @brief Position structure
 */
typedef struct {
    double x;
    double y;
    double z;
} Position;

/**
 * @brief Pose structure
 */
typedef struct {
    Position position;
    Quaternion orientation;
} Pose;

/**
 * @brief Header structure
 */
typedef struct {
    uint32_t seq;
    int64_t sec;
    uint32_t nsec;
    char* frame_id;
} Header;

/**
 * @brief PoseStamped structure
 */
typedef struct {
    Header header;
    Pose pose;
} PoseStamped;

/**
 * @brief Vector3 structure
 */
typedef struct {
    double x;
    double y;
    double z;
} Vector3;

/**
 * @brief Navigation feedback structure
 */
typedef struct {
    NavigationState navigation_state;
    char* feed_back_str;
    Pose2D current_pose;
    bool goal_checked;
    bool is_ready;
} NavFeedback;

// ============================================================================
// String Management
// ============================================================================

/**
 * @brief Free a string allocated by the library
 * @param str String to free
 */
void nav_c_api_free_string(char* str);

// ============================================================================
// State Conversion
// ============================================================================

/**
 * @brief Convert a State enum to its string representation
 * @param state Enum value of NavigationState
 * @return String representation (caller must free with nav_c_api_free_string)
 */
char* navigation_state_to_string(NavigationState state);

// ============================================================================
// Helper Functions
// ============================================================================

/**
 * @brief Creates a target pose by offsetting a given 2D pose along its heading direction
 * @param pose_x X coordinate of the original pose
 * @param pose_y Y coordinate of the original pose
 * @param pose_theta Heading angle in radians
 * @param frame_id The coordinate frame ID (null-terminated string)
 * @param offset_distance Distance to offset along heading (positive = forward, negative = backward)
 * @param out_goal Output parameter for the offset pose
 * @return true on success, false on failure
 */
bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
                               const char* frame_id, double offset_distance,
                               PoseStamped* out_goal);

/**
 * @brief Creates an offset target pose from a given PoseStamped
 * @param in_pose Input pose
 * @param offset_distance Distance to offset along heading direction
 * @param out_goal Output parameter for the offset pose
 * @return true on success, false on failure
 */
bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
                                     PoseStamped* out_goal);

// ============================================================================
// Navigation Handle Management
// ============================================================================

/**
 * @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);

// ============================================================================
// TF Listener Management
// ============================================================================

/**
 * @brief Create a TF listener instance
 * @return TF listener handle, or NULL on failure
 */
TFListenerHandle tf_listener_create(void);

/**
 * @brief Destroy a TF listener instance
 * @param handle TF listener handle to destroy
 */
void tf_listener_destroy(TFListenerHandle handle);

/**
 * @brief Inject a static transform into the TF buffer.
 *
 * This is a convenience for standalone usage where no external TF publisher exists yet.
 * It will create/ensure the frames exist and become transformable.
 *
 * @param tf_handle TF listener handle
 * @param parent_frame Parent frame id (e.g. "map")
 * @param child_frame Child frame id (e.g. "base_link")
 * @param x Translation x (meters)
 * @param y Translation y (meters)
 * @param z Translation z (meters)
 * @param qx Rotation quaternion x
 * @param qy Rotation quaternion y
 * @param qz Rotation quaternion z
 * @param qw Rotation quaternion w
 * @return true on success, false on failure
 */
bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
                                     const char* parent_frame,
                                     const char* child_frame,
                                     double x, double y, double z,
                                     double qx, double qy, double qz, double qw);

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

/**
 * @brief Initialize the navigation system
 * @param handle Navigation handle
 * @param tf_handle TF listener handle
 * @return true on success, false on failure
 */
bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);

/**
 * @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 Send a goal for the robot to navigate to
 * @param handle Navigation handle
 * @param goal Target pose in the global frame
 * @param xy_goal_tolerance Acceptable error in X/Y (meters)
 * @param yaw_goal_tolerance Acceptable angular error (radians)
 * @return true if goal was accepted and sent successfully
 */
bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
                        double xy_goal_tolerance, double yaw_goal_tolerance);

/**
 * @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
 * @param xy_goal_tolerance Acceptable XY error (meters)
 * @param yaw_goal_tolerance Acceptable heading error (radians)
 * @return true if docking command succeeded
 */
bool navigation_dock_to(NavigationHandle handle, const char* marker,
                        const PoseStamped* goal,
                        double xy_goal_tolerance, double yaw_goal_tolerance);

/**
 * @brief Move straight toward the target position
 * @param handle Navigation handle
 * @param goal Target pose
 * @param xy_goal_tolerance Acceptable positional error (meters)
 * @return true if command issued successfully
 */
bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
                                  double xy_goal_tolerance);

/**
 * @brief Rotate in place to align with target orientation
 * @param handle Navigation handle
 * @param goal Pose containing desired heading (only Z-axis used)
 * @param yaw_goal_tolerance Acceptable angular error (radians)
 * @return true if rotation command was sent successfully
 */
bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
                         double yaw_goal_tolerance);

/**
 * @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 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 Free navigation feedback structure
 * @param feedback Feedback structure to free
 */
void navigation_free_feedback(NavFeedback* feedback);

#ifdef __cplusplus
}
#endif

#endif // NAVIGATION_C_API_H

