using System;
using System.Globalization;
using System.IO;
using System.Runtime.InteropServices;
using System.Runtime.CompilerServices;
using System.Text.RegularExpressions;
using SixLabors.ImageSharp;
using SixLabors.ImageSharp.PixelFormats;
using NavigationExample;


namespace NavigationExample
{
    /// <summary>Thông tin map đọc từ file YAML (maze.yaml).</summary>
    public struct MazeMapConfig
    {
        public string ImagePath;       // đường dẫn đầy đủ tới file ảnh
        public float Resolution;
        public double OriginX, OriginY, OriginZ;
        public int Negate;            // 0 hoặc 1
        public double OccupiedThresh;
        public double FreeThresh;
    }

    // ============================================================================
    // Example Usage
    // ============================================================================
    class Program
    {
        /// <summary>
        /// Đọc file maze.yaml (image, resolution, origin, negate, occupied_thresh, free_thresh).
        /// ImagePath được resolve tuyệt đối theo thư mục chứa file yaml.
        /// </summary>
        static bool TryLoadMazeYaml(string yamlPath, out MazeMapConfig config)
        {
            config = default;
            if (string.IsNullOrEmpty(yamlPath) || !File.Exists(yamlPath))
                return false;
            string dir = Path.GetDirectoryName(Path.GetFullPath(yamlPath)) ?? "";
            try
            {
                string[] lines = File.ReadAllLines(yamlPath);
                foreach (string line in lines)
                {
                    string trimmed = line.Trim();
                    if (trimmed.Length == 0 || trimmed.StartsWith("#")) continue;
                    int colon = trimmed.IndexOf(':');
                    if (colon <= 0) continue;
                    string key = trimmed.Substring(0, colon).Trim();
                    string value = trimmed.Substring(colon + 1).Trim();
                    if (key == "image")
                        config.ImagePath = Path.Combine(dir, value);
                    else if (key == "resolution" && float.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out float res))
                        config.Resolution = res;
                    else if (key == "origin")
                    {
                        var m = Regex.Match(value, @"\[\s*([-\d.]+\s*),\s*([-\d.]+\s*),\s*([-\d.]+)\s*\]");
                        if (m.Success)
                        {
                            config.OriginX = double.Parse(m.Groups[1].Value, CultureInfo.InvariantCulture);
                            config.OriginY = double.Parse(m.Groups[2].Value, CultureInfo.InvariantCulture);
                            config.OriginZ = double.Parse(m.Groups[3].Value, CultureInfo.InvariantCulture);
                        }
                    }
                    else if (key == "negate" && int.TryParse(value, out int neg))
                        config.Negate = neg;
                    else if (key == "occupied_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ot))
                        config.OccupiedThresh = ot;
                    else if (key == "free_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ft))
                        config.FreeThresh = ft;
                }
                return !string.IsNullOrEmpty(config.ImagePath);
            }
            catch
            {
                return false;
            }
        }

        /// <summary>
        /// Đọc file ảnh PNG và chuyển pixel sang byte[] occupancy (0=free, 100=occupied, 255=unknown).
        /// Dùng negate, occupied_thresh, free_thresh từ maze.yaml nếu cung cấp.
        /// </summary>
        static byte[] LoadMazePixelsAsOccupancy(string imagePath, out int width, out int height,
            int negate = 0, double occupiedThresh = 0.65, double freeThresh = 0.196)
        {
            width = 0;
            height = 0;
            if (string.IsNullOrEmpty(imagePath) || !File.Exists(imagePath))
            {
                LogError($"File not found: {imagePath}");
                return null;
            }
            try
            {
                using var image = Image.Load<Rgba32>(imagePath);
                int w = image.Width;
                int h = image.Height;
                width = w;
                height = h;
                byte[] data = new byte[w * h];
                image.ProcessPixelRows(accessor =>
                {
                    for (int y = 0; y < accessor.Height; y++)
                    {
                        Span<Rgba32> row = accessor.GetRowSpan(y);
                        for (int x = 0; x < row.Length; x++)
                        {
                            ref readonly Rgba32 p = ref row[x];
                            double gray = (p.R + p.G + p.B) / 255.0 / 3.0;
                            if (negate != 0) gray = 1.0 - gray;
                            byte occ;
                            if (gray <= freeThresh) occ = 0;
                            else if (gray >= occupiedThresh) occ = 100;
                            else occ = 255; // unknown
                            data[y * w + x] = occ;
                        }
                    }
                });
                return data;
            }
            catch (Exception ex)
            {
                LogError($"Load image failed: {ex.Message}");
                return null;
            }
        }

        // 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 tf3 buffer (replaces TF listener; used for all static transforms and navigation init)
            IntPtr tf3Buffer = TF3API.tf3_buffer_create(10);
            if (tf3Buffer == IntPtr.Zero)
            {
                LogError("Failed to create tf3 buffer (libtf3.so may be missing)");
                return;
            }
            Console.WriteLine($"[NavigationExample] TF3 buffer created, handle = 0x{tf3Buffer.ToInt64():X16}");
            string version = Marshal.PtrToStringAnsi(TF3API.tf3_get_version()) ?? "?";
            Console.WriteLine($"[TF3] {version}");

            // Inject static transforms: map -> odom -> base_footprint -> base_link
            var tMapOdom = TF3API.CreateStaticTransform("map", "odom", 0, 0, 0, 0, 0, 0, 1);
            var tOdomFoot = TF3API.CreateStaticTransform("odom", "base_footprint", 0, 0, 0, 0, 0, 0, 1);
            var tFootLink = TF3API.CreateStaticTransform("base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1);
            if (!TF3API.tf3_set_transform(tf3Buffer, ref tMapOdom, "NavigationExample", true) ||
                !TF3API.tf3_set_transform(tf3Buffer, ref tOdomFoot, "NavigationExample", true) ||
                !TF3API.tf3_set_transform(tf3Buffer, ref tFootLink, "NavigationExample", true))
            {
                LogError("Failed to set static TF");
                TF3API.tf3_buffer_destroy(tf3Buffer);
                return;
            }

            // Create navigation instance and initialize with tf3 buffer
            NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
            if (navHandle.ptr == IntPtr.Zero)
            {
                LogError("Failed to create navigation instance");
                TF3API.tf3_buffer_destroy(tf3Buffer);
                return;
            }

            if (!NavigationAPI.navigation_initialize(navHandle, tf3Buffer))
            {
                LogError("Failed to initialize navigation with tf3 buffer");
                NavigationAPI.navigation_destroy(navHandle);
                TF3API.tf3_buffer_destroy(tf3Buffer);
                return;
            }

            while (true)
            {
                NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
                if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
                {
                    if (feedback.is_ready)
                    {
                        Console.WriteLine("Navigation is ready");
                        break;
                    }
                    else
                    {
                        Console.WriteLine("Navigation is not ready");
                    }
                }
                System.Threading.Thread.Sleep(100);
            }
            Console.WriteLine("[NavigationExample] Navigation initialized successfully");


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

  
            IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
            Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
            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");
            Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
            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);

            IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom");
            Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
            Odometry odometryHandle = new 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);

            // Add static map: đọc maze.yaml rồi load ảnh và cập nhật mapMetaData
            string mapYamlPath =  "maze.yaml";
            int mapWidth, mapHeight;
            byte[] data;
            MazeMapConfig mapConfig;
            if (TryLoadMazeYaml(mapYamlPath, out mapConfig))
            {
                data = LoadMazePixelsAsOccupancy(mapConfig.ImagePath, out mapWidth, out mapHeight,
                    mapConfig.Negate, mapConfig.OccupiedThresh, mapConfig.FreeThresh);
                if (data == null)
                {
                    mapWidth = 3;
                    mapHeight = 10;
                    data = new byte[30];
                    for (int i = 0; i < data.Length; i++) data[i] = 100;
                    Console.WriteLine("YAML loaded but image failed; using default map 3x10");
                }
                else
                {
                    Console.WriteLine("Loaded map from {0}: {1}x{2}, resolution={3}, origin=({4},{5},{6})",
                        mapConfig.ImagePath, mapWidth, mapHeight, mapConfig.Resolution,
                        mapConfig.OriginX, mapConfig.OriginY, mapConfig.OriginZ);
                }
            }
            else
            {
                mapWidth = 3;
                mapHeight = 10;
                data = new byte[30];
                for (int i = 0; i < data.Length; i++) data[i] = 100;
                mapConfig = default;
                mapConfig.Resolution = 0.05f;
                mapConfig.OriginX = mapConfig.OriginY = mapConfig.OriginZ = 0.0;
                Console.WriteLine("maze.yaml not found, using default map 3x10");
            }

            Time mapLoadTime = NavigationAPI.time_create();
            MapMetaData mapMetaData = new MapMetaData();
            mapMetaData.map_load_time = mapLoadTime;
            mapMetaData.resolution = mapConfig.Resolution;
            mapMetaData.width = (uint)mapWidth;
            mapMetaData.height = (uint)mapHeight;
            mapMetaData.origin = new Pose();
            mapMetaData.origin.position.x = mapConfig.OriginX;
            mapMetaData.origin.position.y = mapConfig.OriginY;
            mapMetaData.origin.position.z = mapConfig.OriginZ;
            mapMetaData.origin.orientation.x = 0.0;
            mapMetaData.origin.orientation.y = 0.0;
            mapMetaData.origin.orientation.z = 0.0;
            mapMetaData.origin.orientation.w = 1.0;
            OccupancyGrid occupancyGrid = new OccupancyGrid();
            IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map");
            occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
            occupancyGrid.info = mapMetaData;
            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<OccupancyGrid>(),
                Marshal.OffsetOf<OccupancyGrid>("data"),
                Marshal.OffsetOf<OccupancyGrid>("data_count"));
    
            NavigationAPI.navigation_add_static_map(navHandle, "/map",  occupancyGrid);

            Twist2DStamped twist = new 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);
            }  

            // Build order (thao cách bom order): header + nodes + edges giống C++
            Order order = new Order();
            order.headerId = 1;
            order.timestamp = Marshal.StringToHGlobalAnsi("2026-02-28 10:00:00");
            order.version = Marshal.StringToHGlobalAnsi("1.0.0");
            order.manufacturer = Marshal.StringToHGlobalAnsi("Manufacturer");
            order.serialNumber = Marshal.StringToHGlobalAnsi("Serial Number");
            order.orderId = Marshal.StringToHGlobalAnsi("Order ID");
            order.orderUpdateId = 1;

            // Nodes: giống for (auto node : order.nodes) { node_msg.nodeId = ...; node_msg.nodePosition.x = ...; order_msg.nodes.push_back(node_msg); }
            int nodeCount = 1;
            order.nodes = Marshal.AllocHGlobal(Marshal.SizeOf<Node>() * nodeCount);
            order.nodes_count = new UIntPtr((uint)nodeCount);
            Node node1 = new Node();
            node1.nodeId = Marshal.StringToHGlobalAnsi("node-1");
            node1.sequenceId = 0;
            node1.nodeDescription = Marshal.StringToHGlobalAnsi("Goal node");
            node1.released = 0;
            node1.nodePosition.x = 1.0;
            node1.nodePosition.y = 1.0;
            node1.nodePosition.theta = 0.0;
            node1.nodePosition.allowedDeviationXY = 0.1f;
            node1.nodePosition.allowedDeviationTheta = 0.05f;
            node1.nodePosition.mapId = Marshal.StringToHGlobalAnsi("map");
            node1.nodePosition.mapDescription = Marshal.StringToHGlobalAnsi("");
            node1.actions = IntPtr.Zero;
            node1.actions_count = UIntPtr.Zero;
            Marshal.StructureToPtr(node1, order.nodes, false);

            // Edges: rỗng trong ví dụ này; nếu cần thì alloc và fill tương tự (edge_msg.edgeId, trajectory.controlPoints, ...)
            order.edges = IntPtr.Zero;
            order.edges_count = UIntPtr.Zero;
            order.zoneSetId = Marshal.StringToHGlobalAnsi("");

            PoseStamped goal = new PoseStamped();
            goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
            goal.pose.position.x = 0.01;
            goal.pose.position.y = 0.01;
            goal.pose.position.z = 0.0;
            goal.pose.orientation.x = 0.0;
            goal.pose.orientation.y = 0.0;
            goal.pose.orientation.z = 0.0;
            goal.pose.orientation.w = 1.0;


            // Console.WriteLine("Docking to docking_point");
            NavigationAPI.navigation_dock_to(navHandle, "charger", goal);

            // NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);

            // NavigationAPI.navigation_move_to_order(navHandle, order, goal);
            NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
            NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2);

            // NavigationAPI.navigation_move_straight_to(navHandle, 1.0);

            while (true) 
            {
                System.Threading.Thread.Sleep(100);
            //     NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
            //     if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
            //     {
            //         if (feedback.navigation_state == NavigationAPI.NavigationState.Succeeded)
            //         {
            //             Console.WriteLine("Navigation is Succeeded");
            //             break;
            //         }
            //     }

            //     NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput();
            //     if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData))
            //     {
            //         int n = (int)(uint)globalData.plan.poses_count;
            //         int poseSize = Marshal.SizeOf<Pose2DStamped>();
            //         for (int i = 0; i < n; i++)
            //         {
            //             IntPtr posePtr = IntPtr.Add(globalData.plan.poses, i * poseSize);
            //             Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
            //             double p_x = p.pose.x;
            //             double p_y = p.pose.y;
            //             double p_theta = p.pose.theta;
            //             Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
            //         }
            //         if(globalData.is_costmap_updated) {
            //             for(int i = 0; i < (int)(uint)globalData.costmap.data_count; i++) {
            //                 byte cellValue = Marshal.ReadByte(globalData.costmap.data, i);
            //                 Console.WriteLine("Costmap: {0} {1}", i, cellValue);
            //             }
            //         }
            //         else {
            //             Console.WriteLine("Global Costmap is not updated");
            //         }
            //     }

            //     NavigationAPI.PlannerDataOutput localData = new NavigationAPI.PlannerDataOutput();
            //     if(NavigationAPI.navigation_get_local_data(navHandle, ref localData)) 
            //     {
            //         int n = (int)(uint)localData.plan.poses_count;
            //         int poseSize = Marshal.SizeOf<Pose2DStamped>();
            //         for (int i = 0; i < n; i++)
            //         {
            //             IntPtr posePtr = IntPtr.Add(localData.plan.poses, i * poseSize);
            //             Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
            //             double p_x = p.pose.x;
            //             double p_y = p.pose.y;
            //             double p_theta = p.pose.theta;
            //             Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
            //         }
            //         if(localData.is_costmap_updated) {
            //             for(int i = 0; i < (int)(uint)localData.costmap.data_count; i++) {
            //                 byte cellValue = Marshal.ReadByte(localData.costmap.data, i);
            //                 Console.WriteLine("Costmap: {0} {1}", i, cellValue);
            //             }
            //         }
            //         else {
            //             Console.WriteLine("Local Costmap is not updated");
            //         }
            //     }
            }
            // Cleanup (destroy nav first, then tf3 buffer)

            NavigationAPI.navigation_destroy(navHandle);
            TF3API.tf3_buffer_destroy(tf3Buffer);
            Console.WriteLine("Press any key to exit...");
            try
            {
                Console.ReadKey(intercept: true);
            }
            catch (InvalidOperationException)
            {
                // Running without a real console (e.g. redirected/automated run).
            }
        }
    }
}