using System;
using System.Runtime.InteropServices;
using System.Runtime.CompilerServices;

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
        }

        // ============================================================================
        // Structures
        // ============================================================================
        [StructLayout(LayoutKind.Sequential)]
        public struct Point
        {
            public double x;
            public double y;
            public double z;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Pose2D
        {
            public double x;
            public double y;
            public double theta;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Quaternion
        {
            public double x;
            public double y;
            public double z;
            public double w;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Position
        {
            public double x;
            public double y;
            public double z;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Pose
        {
            public Position position;
            public Quaternion orientation;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Header
        {
            public uint seq;
            public long sec;
            public uint nsec;
            public IntPtr frame_id; // char*
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct PoseStamped
        {
            public Header header;
            public Pose pose;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Vector3
        {
            public double x;
            public double y;
            public double z;
        }

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

        // ============================================================================
        // String Management
        // ============================================================================
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern void nav_c_api_free_string(IntPtr str);

        // ============================================================================
        // State Conversion
        // ============================================================================
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        public static extern IntPtr navigation_state_to_string(NavigationState state);

        // ============================================================================
        // Helper Functions
        // ============================================================================
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_offset_goal_2d(
            double pose_x, double pose_y, double pose_theta,
            string frame_id, double offset_distance,
            ref PoseStamped out_goal);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_offset_goal_stamped(
            ref PoseStamped in_pose, double offset_distance,
            ref PoseStamped out_goal);

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

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

        // ============================================================================
        // TF Listener Management
        // ============================================================================
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern IntPtr tf_listener_create();

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

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool tf_listener_set_static_transform(
            IntPtr tf_handle,
            string parent_frame,
            string child_frame,
            double x, double y, double z,
            double qx, double qy, double qz, double qw);

        // ============================================================================
        // Navigation Interface Methods
        // ============================================================================
        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);

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

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_move_to(
            IntPtr handle, ref PoseStamped goal,
            double xy_goal_tolerance, double yaw_goal_tolerance);

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

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_move_straight_to(
            IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_rotate_to(
            IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);

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

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

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

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

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

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

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

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

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern void navigation_free_feedback(ref NavFeedback feedback);

        // ============================================================================
        // Helper Methods for String Conversion
        // ============================================================================
        public static string MarshalString(IntPtr ptr)
        {
            if (ptr == IntPtr.Zero)
                return string.Empty;
            return Marshal.PtrToStringAnsi(ptr);
        }
    }

    // ============================================================================
    // Example Usage
    // ============================================================================
    class Program
    {
        // Helper method để hiển thị file và line number tự động
        static void LogError(string message,
            [CallerFilePath] string filePath = "",
            [CallerLineNumber] int lineNumber = 0,
            [CallerMemberName] string memberName = "")
        {
            // Lấy tên file từ đường dẫn đầy đủ
            string fileName = System.IO.Path.GetFileName(filePath);
            Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}");
        }

        static void Main(string[] args)
        {
            // Create TF listener
            IntPtr tfHandle = NavigationAPI.tf_listener_create();
            if (tfHandle == IntPtr.Zero)
            {
                LogError("Failed to create TF listener");
                return;
            }

            // Inject a static TF so costmap can immediately canTransform(map <-> base_link).
            // If you already publish TF from localization/odometry, you can remove this call.
            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "map", "odom",
                                                               0, 0, 0,
                                                               0, 0, 0, 1))
            {
                LogError("Failed to inject static TF map -> odom");
                NavigationAPI.tf_listener_destroy(tfHandle);
                return;
            }

            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_footprint",
                                                               0, 0, 0,
                                                               0, 0, 0, 1))
            {
                LogError("Failed to inject static TF map -> base_link");
                NavigationAPI.tf_listener_destroy(tfHandle);
                return;
            }

            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "base_footprint", "base_link",
                                                               0, 0, 0,
                                                               0, 0, 0, 1))
            {
                LogError("Failed to inject static TF map -> base_link");
                NavigationAPI.tf_listener_destroy(tfHandle);
                return;
            }

            // Create navigation instance
            IntPtr navHandle = NavigationAPI.navigation_create();
            if (navHandle == IntPtr.Zero)
            {
                LogError("Failed to create navigation instance");
                NavigationAPI.tf_listener_destroy(tfHandle);
                return;
            }

            // Initialize navigation
            if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
            {
                LogError("Failed to initialize navigation");
                NavigationAPI.navigation_destroy(navHandle);
                NavigationAPI.tf_listener_destroy(tfHandle);
                return;
            }

            // Set robot footprint
            NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
            {
                new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 },
                new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 },
                new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 },
                new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 }
            };
            NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));

            // Get robot pose
            NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D();
            if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose))
            {
                Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}");
            }

            // Get navigation feedback
            NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
            if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
            {
                string stateStr = NavigationAPI.MarshalString(
                    NavigationAPI.navigation_state_to_string(feedback.navigation_state));
                string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
                Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
                NavigationAPI.navigation_free_feedback(ref feedback);
            }

            // Cleanup
            NavigationAPI.navigation_destroy(navHandle);
            NavigationAPI.tf_listener_destroy(tfHandle);
        }
    }
}

