using System.Runtime.InteropServices;
// using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.Navigation;

/// <summary>
/// P/Invoke declarations for Navigation C API
/// </summary>
public static class NavigationNativeInterface
{
    // Library path
    private const string LibraryPath = "/usr/local/lib/libnav_c_api.so";

    #region String Management

    /// <summary>
    /// Free a string allocated by the library
    /// </summary>
    /// <param name="str">String to free</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void nav_c_api_free_string(IntPtr str);

    #endregion

    #region State Conversion

    /// <summary>
    /// Convert a State enum to its string representation
    /// </summary>
    /// <param name="state">Enum value of NavigationState</param>
    /// <returns>String representation (caller must free with nav_c_api_free_string)</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr navigation_state_to_string(NavigationState state);

    #endregion

    #region Header Functions

    /// <summary>
    /// Create a new header
    /// </summary>
    /// <param name="frame_id">Frame id</param>
    /// <returns>Header</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
    public static extern Header header_create([MarshalAs(UnmanagedType.LPUTF8Str)] string frame_id);

    /// <summary>
    /// Set data for a header
    /// </summary>
    /// <param name="seq">Sequence</param>
    /// <param name="sec">Second</param>
    /// <param name="nsec">Nanosecond</param>
    /// <param name="frame_id">Frame id</param>
    /// <returns>Header</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
    public static extern Header header_set_data(
        uint seq,
        uint sec,
        uint nsec,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string frame_id);

    /// <summary>
    /// Create a new time
    /// </summary>
    /// <returns>Time</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern Time time_create();

    #endregion

    #region Helper Functions

    /// <summary>
    /// Creates a target pose by offsetting a given 2D pose along its heading direction
    /// </summary>
    /// <param name="pose_x">X coordinate of the original pose</param>
    /// <param name="pose_y">Y coordinate of the original pose</param>
    /// <param name="pose_theta">Heading angle in radians</param>
    /// <param name="frame_id">The coordinate frame ID (null-terminated string)</param>
    /// <param name="offset_distance">Distance to offset along heading (positive = forward, negative = backward)</param>
    /// <param name="out_goal">Output parameter for the offset pose</param>
    /// <returns>true on success, false on failure</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_offset_goal_2d(
        double pose_x, double pose_y, double pose_theta,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string frame_id,
        double offset_distance,
        out PoseStamped out_goal);

    /// <summary>
    /// Creates an offset target pose from a given PoseStamped
    /// </summary>
    /// <param name="in_pose">Input pose</param>
    /// <param name="offset_distance">Distance to offset along heading direction</param>
    /// <param name="out_goal">Output parameter for the offset pose</param>
    /// <returns>true on success, false on failure</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_offset_goal_stamped(
        ref PoseStamped in_pose,
        double offset_distance,
        out PoseStamped out_goal);

    #endregion

    #region Navigation Handle Management

    /// <summary>
    /// Create a new navigation instance
    /// </summary>
    /// <returns>Navigation handle, or IntPtr.Zero on failure</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr navigation_create();

    /// <summary>
    /// Destroy a navigation instance
    /// </summary>
    /// <param name="handle">Navigation handle to destroy</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_destroy(IntPtr handle);

    #endregion

    #region TF Listener Management

    /// <summary>
    /// Create a TF listener instance
    /// </summary>
    /// <returns>TF listener handle, or IntPtr.Zero on failure</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr tf_listener_create();

    /// <summary>
    /// Destroy a TF listener instance
    /// </summary>
    /// <param name="handle">TF listener handle to destroy</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void tf_listener_destroy(IntPtr handle);

    /// <summary>
    /// Inject a static transform into the TF buffer
    /// </summary>
    /// <param name="tf_handle">TF listener handle</param>
    /// <param name="parent_frame">Parent frame id (e.g. "map")</param>
    /// <param name="child_frame">Child frame id (e.g. "base_link")</param>
    /// <param name="x">Translation x (meters)</param>
    /// <param name="y">Translation y (meters)</param>
    /// <param name="z">Translation z (meters)</param>
    /// <param name="qx">Rotation quaternion x</param>
    /// <param name="qy">Rotation quaternion y</param>
    /// <param name="qz">Rotation quaternion z</param>
    /// <param name="qw">Rotation quaternion w</param>
    /// <returns>true on success, false on failure</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool tf_listener_set_static_transform(
        IntPtr tf_handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string parent_frame,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string child_frame,
        double x, double y, double z,
        double qx, double qy, double qz, double qw);

    #endregion

    #region Navigation Interface Methods

    /// <summary>
    /// Initialize the navigation system
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="tf_handle">TF listener handle</param>
    /// <returns>true on success, false on failure</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);

    /// <summary>
    /// Set the robot's footprint (outline shape)
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="points">Array of points representing the footprint polygon</param>
    /// <param name="point_count">Number of points in the array</param>
    /// <returns>true on success, false on failure</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_set_robot_footprint(
        IntPtr handle,
        IntPtr points,
        nuint point_count);

    /// <summary>
    /// Get the robot's footprint (outline shape)
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="out_points">Output array of points (allocated by library, free with navigation_free_points)</param>
    /// <param name="out_count">Output number of points in the array</param>
    /// <returns>true on success, false on failure</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_get_robot_footprint(
        IntPtr handle,
        out IntPtr out_points,
        out nuint out_count);

    /// <summary>
    /// Free a points array allocated by navigation_get_robot_footprint
    /// </summary>
    /// <param name="points">Pointer to point array</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_points(IntPtr points);

    /// <summary>
    /// Send a goal for the robot to navigate to
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="goal">Target pose in the global frame</param>
    /// <param name="xy_goal_tolerance">Acceptable error in X/Y (meters)</param>
    /// <param name="yaw_goal_tolerance">Acceptable angular error (radians)</param>
    /// <returns>true if goal was accepted and sent successfully</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_move_to(
        IntPtr handle,
        PoseStamped goal);

    /// <summary>
    /// Send a goal for the robot to navigate to with order
    /// Note: This function may not be available in all C API versions
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="order">Order handle</param>
    /// <param name="goal">Target pose in the global frame</param>
    /// <param name="xy_goal_tolerance">Acceptable error in X/Y (meters)</param>
    /// <param name="yaw_goal_tolerance">Acceptable angular error (radians)</param>
    /// <returns>true if goal was accepted and sent successfully</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_move_to_order(IntPtr handle, Order order, PoseStamped goal);

    /// <summary>
    /// Send a docking goal to a predefined marker
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="marker">Marker name or ID (null-terminated string)</param>
    /// <param name="goal">Target pose for docking</param>
    /// <param name="xy_goal_tolerance">Acceptable XY error (meters)</param>
    /// <param name="yaw_goal_tolerance">Acceptable heading error (radians)</param>
    /// <returns>true if docking command succeeded</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_dock_to(
        IntPtr handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string marker,
        PoseStamped goal);

    /// <summary>
    /// Move straight toward the target position
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="goal">Target pose</param>
    /// <param name="xy_goal_tolerance">Acceptable positional error (meters)</param>
    /// <returns>true if command issued successfully</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_move_straight_to(
        IntPtr handle,
        double distance);

    /// <summary>
    /// Rotate in place to align with target orientation
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="goal">Pose containing desired heading (only Z-axis used)</param>
    /// <param name="yaw_goal_tolerance">Acceptable angular error (radians)</param>
    /// <returns>true if rotation command was sent successfully</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_rotate_to(
        IntPtr handle,
        PoseStamped goal);

    /// <summary>
    /// Pause the robot's movement
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_pause(IntPtr handle);

    /// <summary>
    /// Resume motion after a pause
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_resume(IntPtr handle);

    /// <summary>
    /// Cancel the current goal and stop the robot
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_cancel(IntPtr handle);

    /// <summary>
    /// Send limited linear velocity command
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="linear_x">Linear velocity in X direction</param>
    /// <param name="linear_y">Linear velocity in Y direction</param>
    /// <param name="linear_z">Linear velocity in Z direction</param>
    /// <returns>true if the command was accepted</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_set_twist_linear(
        IntPtr handle,
        double linear_x, double linear_y, double linear_z);

    /// <summary>
    /// Send limited angular velocity command
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="angular_x">Angular velocity around X axis</param>
    /// <param name="angular_y">Angular velocity around Y axis</param>
    /// <param name="angular_z">Angular velocity around Z axis</param>
    /// <returns>true if the command was accepted</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_set_twist_angular(
        IntPtr handle,
        double angular_x, double angular_y, double angular_z);

    /// <summary>
    /// Get the robot's pose as a PoseStamped
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="out_pose">Output parameter with the robot's current pose</param>
    /// <returns>true if pose was successfully retrieved</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_get_robot_pose_stamped(
        IntPtr handle,
        ref PoseStamped out_pose);

    /// <summary>
    /// Get the robot's pose as a 2D pose
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="out_pose">Output parameter with the robot's current 2D pose</param>
    /// <returns>true if pose was successfully retrieved</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_get_robot_pose_2d(
        IntPtr handle,
        ref Pose2D out_pose);

    /// <summary>
    /// Get the robot's current twist
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="out_twist">Output parameter with the robot's current twist</param>
    /// <returns>true if twist was successfully retrieved</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_get_twist(
        IntPtr handle,
        ref Twist2DStamped ref_twist);

    /// <summary>
    /// Get navigation feedback
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="out_feedback">Output parameter with navigation feedback</param>
    /// <returns>true if feedback was successfully retrieved</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_get_feedback(
        IntPtr handle,
        ref NavFeedback out_feedback);

    /// <summary>
    /// Get global planner data from navigation system
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_get_global_data(
        IntPtr handle,
        ref PlannerDataOutput out_data);

    /// <summary>
    /// Get local planner data from navigation system
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_get_local_data(
        IntPtr handle,
        ref PlannerDataOutput out_data);

    /// <summary>
    /// Free navigation feedback structure
    /// </summary>
    /// <param name="feedback">Feedback structure to free</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern void navigation_free_feedback(ref NavFeedback feedback);

    /// <summary>
    /// Free an occupancy grid handle
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_occupancy_grid(IntPtr handle);

    /// <summary>
    /// Free an occupancy grid update handle
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_occupancy_grid_update(IntPtr handle);

    /// <summary>
    /// Free a laser scan handle
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_laser_scan(IntPtr handle);

    /// <summary>
    /// Free an odometry handle
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_odometry(IntPtr handle);

    /// <summary>
    /// Free a path2d handle
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_path2d(IntPtr handle);

    /// <summary>
    /// Free a polygon stamped handle
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_polygon_stamped(IntPtr handle);

    /// <summary>
    /// Free an order handle
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_order(IntPtr handle);

    /// <summary>
    /// Free an array of named occupancy grids
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_named_occupancy_grids(IntPtr maps, nuint count);

    /// <summary>
    /// Free an array of named laser scans
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void navigation_free_named_laser_scans(IntPtr scans, nuint count);

    // [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    // [return: MarshalAs(UnmanagedType.Bool)]
    // public static extern void navigation_free_planner_data(PlannerDataOutputHandle handle);

    /// <summary>
    /// Add a static map to the navigation system
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="map_name">Name of the map</param>
    /// <param name="occupancy_grid">Occupancy grid handle</param>
    /// <returns>true if the map was added successfully</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_add_static_map(
        IntPtr handle, 
        [MarshalAs(UnmanagedType.LPUTF8Str)] string map_name, 
        OccupancyGrid occupancy_grid);

    /// <summary>
    /// Add a laser scan to the navigation system
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="laser_scan_name">Name of the laser scan</param>
    /// <param name="laser_scan">Laser scan handle</param>
    /// <returns>true if the laser scan was added successfully</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_add_laser_scan(
        IntPtr handle, 
        [MarshalAs(UnmanagedType.LPUTF8Str)] string laser_scan_name, 
        LaserScan laser_scan);

    /// <summary>
    /// Add an odometry to the navigation system
    /// </summary>
    /// <param name="handle">Navigation handle</param>
    /// <param name="odometry_name">Name of the odometry</param>
    /// <param name="odometry">Odometry handle</param>
    /// <returns>true if the odometry was added successfully</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.Bool)]
    public static extern bool navigation_add_odometry(
        IntPtr handle, 
        [MarshalAs(UnmanagedType.LPUTF8Str)] string odometry_name, 
        Odometry odometry);

    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    [return: MarshalAs(UnmanagedType.I1)]
    public static extern bool navigation_dock_to_order(
        IntPtr handle,
        Order order,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string marker,
        PoseStamped goal);
    #endregion
}