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 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;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Twist2D
        {
            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 Vector3
        {
            public double x;
            public double y;
            public double z;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Twist
        {
            public Vector3 linear;
            public Vector3 angular;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Header
        {
            public uint seq;
            public uint 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 Twist2DStamped
        {
            public Header header;
            public Twist2D velocity;
        }


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

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

        [StructLayout(LayoutKind.Sequential)]
        public struct LaserScan
        {
            public Header header;
            public float angle_min;
            public float angle_max;
            public float angle_increment;
            public float time_increment;
            public float scan_time;
            public float range_min;
            public float range_max;
            public IntPtr ranges;
            public UIntPtr ranges_count;
            public IntPtr intensities;
            public UIntPtr intensities_count;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct PoseWithCovariance
        {
            public Pose pose;
            public IntPtr covariance;
            public UIntPtr covariance_count;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct TwistWithCovariance    {
            public Twist twist;
            public IntPtr covariance;
            public UIntPtr covariance_count;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Odometry
        {
            public Header header;
            public IntPtr child_frame_id;
            public PoseWithCovariance pose;
            public TwistWithCovariance twist;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct OccupancyGrid
        {
            public Header header;
            public MapMetaData info;
            public IntPtr data;
            public UIntPtr data_count;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct MapMetaData
        {
            public Time map_load_time;
            public float resolution;
            public uint width;
            public uint height;
            public Pose origin;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Time
        {
            public uint sec;
            public uint nsec;
        }

        [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;
            }
        }

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

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

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

        // ============================================================================
        // 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);

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

        [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);

        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool navigation_move_to(NavigationHandle handle, 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(NavigationHandle handle, string marker, 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_move_straight_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance);

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

        [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);
    
    }

    // ============================================================================
    // 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
            NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create();
            if (tfHandle.ptr == 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
            NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
            if (navHandle.ptr == 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 navigation feedback
            NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
            if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
            {
                IntPtr stateStrPtr = NavigationAPI.navigation_state_to_string(feedback.navigation_state);
                string stateStr = NavigationAPI.MarshalString(stateStrPtr);
                NavigationAPI.nav_c_api_free_string(stateStrPtr);
                string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
                Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
                NavigationAPI.navigation_free_feedback(ref feedback);
            }

            System.Threading.Thread.Sleep(1000);   
            IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
            NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
            NavigationAPI.LaserScan fscanHandle;
            fscanHandle.header = fscanHeader;
            fscanHandle.angle_min = -1.57f;
            fscanHandle.angle_max = 1.57f;
            fscanHandle.angle_increment = 0.785f;
            fscanHandle.time_increment = 0.0f;
            fscanHandle.scan_time = 0.1f;
            fscanHandle.range_min = 0.05f;
            fscanHandle.range_max = 10.0f;
            fscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5);
            Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, fscanHandle.ranges, 5);
            fscanHandle.ranges_count = new UIntPtr(5);
            fscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5);
            Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, fscanHandle.intensities, 5);
            fscanHandle.intensities_count = new UIntPtr(5);
            NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan",  fscanHandle);


            IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan");
            NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
            NavigationAPI.LaserScan bscanHandle;
            bscanHandle.header = bscanHeader;
            bscanHandle.angle_min = 1.57f;
            bscanHandle.angle_max = -1.57f;
            bscanHandle.angle_increment = -0.785f;
            bscanHandle.time_increment = 0.0f;
            bscanHandle.scan_time = 0.1f;
            bscanHandle.range_min = 0.05f;
            bscanHandle.range_max = 10.0f;
            bscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5);
            Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, bscanHandle.ranges, 5);
            bscanHandle.ranges_count = new UIntPtr(5);
            bscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5);
            Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, bscanHandle.intensities, 5);
            bscanHandle.intensities_count = new UIntPtr(5);
            NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan",  bscanHandle);

            System.Threading.Thread.Sleep(1000); 

            IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom");
            NavigationAPI.Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
            NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry();
            odometryHandle.header = odometryHeader;
            IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint");
            odometryHandle.child_frame_id = childFrameId;
            odometryHandle.pose.pose.position.x = 0.0;
            odometryHandle.pose.pose.position.y = 0.0;
            odometryHandle.pose.pose.position.z = 0.0;
            odometryHandle.pose.pose.orientation.x = 0.0;
            odometryHandle.pose.pose.orientation.y = 0.0;
            odometryHandle.pose.pose.orientation.z = 0.0;
            odometryHandle.pose.pose.orientation.w = 1.0;
            
            double[] pose_covariance = new double[36];
            for(int i = 0; i < pose_covariance.Length; i++) {
                pose_covariance[i] = 0.0;
            }
            odometryHandle.pose.covariance = Marshal.AllocHGlobal(sizeof(double) * pose_covariance.Length);
            Marshal.Copy(pose_covariance, 0, odometryHandle.pose.covariance, pose_covariance.Length);
            odometryHandle.pose.covariance_count = new UIntPtr((uint)pose_covariance.Length);

            odometryHandle.twist.twist.linear.x = 0.0;
            odometryHandle.twist.twist.linear.y = 0.0;
            odometryHandle.twist.twist.linear.z = 0.0;
            odometryHandle.twist.twist.angular.x = 0.0;
            odometryHandle.twist.twist.angular.y = 0.0;
            odometryHandle.twist.twist.angular.z = 0.0;
            double[] twist_covariance = new double[36];
            for(int i = 0; i < twist_covariance.Length; i++) {
                twist_covariance[i] = 0.0;
            }
            odometryHandle.twist.covariance = Marshal.AllocHGlobal(sizeof(double) * twist_covariance.Length);
            Marshal.Copy(twist_covariance, 0, odometryHandle.twist.covariance, twist_covariance.Length);
            odometryHandle.twist.covariance_count = new UIntPtr((uint)twist_covariance.Length);
            NavigationAPI.navigation_add_odometry(navHandle, "odometry",  odometryHandle);

            System.Threading.Thread.Sleep(1000);  
            // Add static map
            NavigationAPI.Time mapLoadTime = NavigationAPI.time_create();
            NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData();
            mapMetaData.map_load_time = mapLoadTime;
            mapMetaData.resolution = 0.05f;
            mapMetaData.width = 3;
            mapMetaData.height = 10;
            mapMetaData.origin = new NavigationAPI.Pose();
            mapMetaData.origin.position.x = 0.0;
            mapMetaData.origin.position.y = 0.0;
            mapMetaData.origin.position.z = 0.0;
            mapMetaData.origin.orientation.x = 0.0;
            mapMetaData.origin.orientation.y = 0.0;
            mapMetaData.origin.orientation.z = 0.0;
            mapMetaData.origin.orientation.w = 1.0;
            NavigationAPI.OccupancyGrid occupancyGrid = new NavigationAPI.OccupancyGrid();
            IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map");
            occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
            occupancyGrid.info = mapMetaData;
            byte[] data = new byte[30];
            for (int i = 0; i < data.Length; i++) {
                data[i] = 100;
            }
            occupancyGrid.data = Marshal.AllocHGlobal(sizeof(byte) * data.Length);
            Marshal.Copy(data, 0, occupancyGrid.data, data.Length);
            occupancyGrid.data_count = new UIntPtr((uint)data.Length);
            Console.WriteLine("data length: {0} {1}", data.Length, occupancyGrid.data_count);
            Console.WriteLine("C# OccupancyGrid sizeof={0} data_off={1} data_count_off={2}",
                Marshal.SizeOf<NavigationAPI.OccupancyGrid>(),
                Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data"),
                Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data_count"));
    
            NavigationAPI.navigation_add_static_map(navHandle, "/map",  occupancyGrid);

            System.Threading.Thread.Sleep(1000);

            NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
            if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
            {
                Console.WriteLine("Twist: {0}, {1}, {2}, {3}", NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
            }            // Cleanup
            NavigationAPI.navigation_destroy(navHandle);
            NavigationAPI.tf_listener_destroy(tfHandle);
        }
    }
}