using System;
using System.Runtime.InteropServices;
using NavigationExample;

namespace NavigationExample
{
    /// <summary>
    /// C# P/Invoke wrapper for Navigation C API
    /// </summary>
    public class NavigationAPI
    {
        private const string DllName = "libnav_c_api.so"; // Linux
        // For Windows: "nav_c_api.dll"
        // For macOS: "libnav_c_api.dylib"

        // ============================================================================
        // Enums
        // ============================================================================
        public enum NavigationState
        {
            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
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct NavFeedback
        {
            public NavigationState navigation_state;
            public IntPtr feed_back_str;  // char*; free with nav_c_api_free_string
            public Pose2D current_pose;
            [MarshalAs(UnmanagedType.I1)]
            public bool goal_checked;
            [MarshalAs(UnmanagedType.I1)]
            public bool is_ready;
        }


        /// <summary>Planner data output (plan, costmap, footprint).</summary>
        [StructLayout(LayoutKind.Sequential)]
        public struct PlannerDataOutput
        {
            public Path2D plan;
            public OccupancyGrid costmap;
            public OccupancyGridUpdate costmap_update;
            [MarshalAs(UnmanagedType.I1)]
            public bool is_costmap_updated;
            public PolygonStamped footprint;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct NavigationHandle
        {
            public IntPtr ptr;
        }

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        public static extern Header header_create(string frame_id);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        public static extern Header header_set_data(
            uint seq,
            uint sec,
            uint nsec,
            string frame_id);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern Time time_create();

        /// <summary>Free a string allocated by the API (strdup).</summary>
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern void nav_c_api_free_string(IntPtr str);

        /// <summary>Convert NavigationState to string; caller must free with nav_c_api_free_string.</summary>
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        public static extern IntPtr navigation_state_to_string(NavigationState state);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);


        /// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
        public static string MarshalString(IntPtr p)
        {
            if (p == IntPtr.Zero) return string.Empty;
            return Marshal.PtrToStringAnsi(p) ?? string.Empty;
        }

        /// <summary>Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done.</summary>
        public static void navigation_free_feedback(ref NavFeedback feedback)
        {
            if (feedback.feed_back_str != IntPtr.Zero)
            {
                nav_c_api_free_string(feedback.feed_back_str);
                feedback.feed_back_str = IntPtr.Zero;
            }
        }

        // ============================================================================
        // Navigation Handle Management
        // ============================================================================
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern NavigationHandle navigation_create();

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern void navigation_destroy(NavigationHandle handle);

        /// <summary>Initialize navigation using an existing tf3 buffer (from libtf3). Caller owns the buffer and must call tf3_buffer_destroy after navigation_destroy.</summary>
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_initialize(NavigationHandle handle, IntPtr tf3_buffer);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);

        /// <summary>Send a goal for the robot to navigate to (global frame).</summary>
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal);

        /// <summary>Navigate using an Order message (graph nodes/edges). Order must be built or obtained from native side; call order_free when done if it was allocated by native.</summary>
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal);

        /// <remarks>Goal is passed by reference to match C API: navigation_dock_to_order(..., const PoseStamped &goal).</remarks>
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, ref PoseStamped goal);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);

        /// <summary>Rotate in place to align with target orientation (radians).</summary>
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_rotate_to(NavigationHandle handle, double goal_yaw);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_pause(NavigationHandle handle);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_resume(NavigationHandle handle);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_cancel(NavigationHandle handle);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist);

        // ============================================================================
        // Navigation Data Management
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan);
    
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_add_point_cloud(NavigationHandle handle, string point_cloud_name, PointCloud point_cloud);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_add_point_cloud2(NavigationHandle handle, string point_cloud2_name, PointCloud2 point_cloud2);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_laser_scan(NavigationHandle handle, string laser_scan_name, ref LaserScan out_scan);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_point_cloud(NavigationHandle handle, string point_cloud_name, ref PointCloud out_cloud);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_point_cloud2(NavigationHandle handle, string point_cloud2_name, ref PointCloud2 out_cloud);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_static_map(NavigationHandle handle, string map_name);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_laser_scan(NavigationHandle handle, string laser_scan_name);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_point_cloud(NavigationHandle handle, string point_cloud_name);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_point_cloud2(NavigationHandle handle, string point_cloud2_name);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_all_static_maps(NavigationHandle handle);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_all_laser_scans(NavigationHandle handle);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_all_point_clouds(NavigationHandle handle);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_all_point_cloud2s(NavigationHandle handle);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_remove_all_data(NavigationHandle handle);

        /// <summary>Get all static maps. out_maps must be pre-allocated; use navigation_get_all_static_maps_count or similar to get count first if needed.</summary>
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_all_static_maps(NavigationHandle handle, [Out] NamedOccupancyGrid[] out_maps, ref UIntPtr out_count);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_all_laser_scans(NavigationHandle handle, [Out] NamedLaserScan[] out_scans, ref UIntPtr out_count);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_all_point_clouds(NavigationHandle handle, [Out] NamedPointCloud[] out_clouds, ref UIntPtr out_count);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_all_point_cloud2s(NavigationHandle handle, [Out] NamedPointCloud2[] out_clouds, ref UIntPtr out_count);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_global_data(NavigationHandle handle, ref PlannerDataOutput out_data);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_get_local_data(NavigationHandle handle, ref PlannerDataOutput out_data);
    }
}