using Microsoft.Extensions.Logging;
using RobotNet10.RobotApp.SLAM.Cartographer;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

/// <summary>
/// Helper class để load occupancy grid từ PGM file (ROS map format)
/// PGM format: Portable Gray Map
/// </summary>
internal static class PgmLoader
{
    /// <summary>
    /// Load occupancy grid từ PGM file và YAML metadata file.
    /// UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM, Y-axis pointing UP).
    /// This ensures consistency across all map formats (PGM, PNG, JPG) and in-memory grids.
    /// </summary>
    /// <param name="pgmPath">Path to PGM file.</param>
    /// <param name="yamlPath">Optional path to YAML metadata.</param>
    /// <param name="logger">Optional logger.</param>
    public static OccupancyGrid? LoadFromPgm(
        string pgmPath,
        string? yamlPath = null,
        ILogger? logger = null)
    {
        try
        {
            if (!File.Exists(pgmPath))
            {
                logger?.LogError("PgmLoader: PGM file not found: {PgmPath}", pgmPath);
                return null;
            }

            // Try to find YAML file if not provided
            if (string.IsNullOrEmpty(yamlPath))
            {
                var yamlPathCandidate = Path.ChangeExtension(pgmPath, ".yaml");
                if (File.Exists(yamlPathCandidate))
                {
                    yamlPath = yamlPathCandidate;
                }
            }

            // Read YAML metadata if available
            double resolution = 0.05; // Default resolution
            Pose origin = new()
            {
                Position = new Vector3(0, 0, 0),
                Orientation = new Quaternion(0, 0, 0, 1)
            };

            if (!string.IsNullOrEmpty(yamlPath) && File.Exists(yamlPath))
            {
                var metadata = ParseYamlMetadata(yamlPath, logger);
                resolution = metadata.resolution;
                origin = metadata.origin;
            }

            // Read PGM file
            // Strategy: Read header using StreamReader to parse, then read file again byte-by-byte to find exact binary start position
            string magic = "";
            int width = 0, height = 0, maxValue = 0;
            long binaryDataStartPosition = 0;

            // First pass: Read header using StreamReader to parse values
            using (var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read))
            {
                using var reader = new StreamReader(fileStream, leaveOpen: true);
                
                // Read magic number (P5 or P2)
                magic = reader.ReadLine() ?? "";
                if (magic != "P5" && magic != "P2")
                {
                    logger?.LogError("PgmLoader: Invalid PGM format. Expected P5 or P2, got: {Magic}", magic);
                    return null;
                }

                // Skip comments
                string? line;
                while ((line = reader.ReadLine()) != null && line.StartsWith('#'))
                {
                    // Skip comment lines
                }

                // Read dimensions
                var dimensions = line?.Split(' ', StringSplitOptions.RemoveEmptyEntries);
                if (dimensions == null || dimensions.Length != 2)
                {
                    logger?.LogError("PgmLoader: Invalid dimensions line: {Line}", line);
                    return null;
                }

                width = int.Parse(dimensions[0]);
                height = int.Parse(dimensions[1]);

                // Read max value
                var maxValueLine = reader.ReadLine();
                if (string.IsNullOrEmpty(maxValueLine))
                {
                    logger?.LogError("PgmLoader: Missing max value");
                    return null;
                }

                maxValue = int.Parse(maxValueLine);
            }

            // Second pass: Read file from beginning byte-by-byte to find exact binary data start position
            // This is necessary because StreamReader buffering makes Position unreliable
            // Strategy: Read lines and count non-comment lines until we find the 3rd non-comment line (max value)
            using (var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read))
            {
                var lineBuffer = new List<byte>();
                var nonCommentLinesFound = 0;
                
                while (true)
                {
                    var buffer = new byte[1];
                    var bytesRead = fileStream.Read(buffer, 0, 1);
                    if (bytesRead == 0)
                    {
                        break; // End of file
                    }
                    
                    var currentChar = (char)buffer[0];
                    
                    // Check for newline (handle both \n and \r\n)
                    if (currentChar == '\n')
                    {
                        // Process line
                        if (lineBuffer.Count > 0)
                        {
                            var line = System.Text.Encoding.ASCII.GetString([.. lineBuffer]).TrimStart();
                            
                            if (!line.StartsWith('#'))
                            {
                                nonCommentLinesFound++;
                                
                                if (nonCommentLinesFound == 3)
                                {
                                    // Max value line - binary data starts after this newline
                                    binaryDataStartPosition = fileStream.Position;
                                    break;
                                }
                            }
                        }
                        
                        lineBuffer.Clear();
                    }
                    else if (currentChar == '\r')
                    {
                        // Handle \r\n - read next byte
                        var nextBytesRead = fileStream.Read(buffer, 0, 1);
                        if (nextBytesRead > 0 && buffer[0] == '\n')
                        {
                            // Process line
                            if (lineBuffer.Count > 0)
                            {
                                var line = System.Text.Encoding.ASCII.GetString([.. lineBuffer]).TrimStart();
                                
                                if (!line.StartsWith('#'))
                                {
                                    nonCommentLinesFound++;
                                    
                                    if (nonCommentLinesFound == 3)
                                    {
                                        // Max value line - binary data starts after this \r\n
                                        binaryDataStartPosition = fileStream.Position;
                                        break;
                                    }
                                }
                            }
                            
                            lineBuffer.Clear();
                        }
                        else
                        {
                            // Just \r without \n, add to line buffer
                            lineBuffer.Add((byte)currentChar);
                            if (nextBytesRead > 0)
                            {
                                lineBuffer.Add(buffer[0]);
                            }
                        }
                    }
                    else
                    {
                        // Add character to line buffer
                        lineBuffer.Add(buffer[0]);
                    }
                }
                
                if (binaryDataStartPosition == 0)
                {
                    logger?.LogError("PgmLoader: Failed to find max value line end position. Found {LinesFound} non-comment lines.", nonCommentLinesFound);
                    return null;
                }
            }

            // Create occupancy grid
            var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);

            // Second pass: Read pixel data
            if (magic == "P5")
            {
                // Binary format - read directly from file
                using var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read);
                fileStream.Position = binaryDataStartPosition;
                
                var buffer = new byte[width * height];
                var totalBytesRead = 0;
                
                // Read in chunks to handle large files
                while (totalBytesRead < buffer.Length)
                {
                    var bytesRead = fileStream.Read(buffer, totalBytesRead, buffer.Length - totalBytesRead);
                    if (bytesRead == 0)
                    {
                        // End of file reached
                        break;
                    }
                    totalBytesRead += bytesRead;
                }

                if (totalBytesRead != buffer.Length)
                {
                    logger?.LogError("PgmLoader: Unexpected end of file. Expected {Expected} bytes, got {Actual}. File may be corrupted or incomplete.", buffer.Length, totalBytesRead);
                    return null;
                }

                // Convert to occupancy values
                // UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM, Y-axis pointing UP)
                // Both PGM file and output grid use same convention → direct copy, no Y-flip needed
                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++)
                    {
                        int index = y * width + x;
                        var pixelValue = buffer[index];
                        var occupancyValue = ConvertPixelToOccupancy(pixelValue, maxValue);
                        occupancyGrid.Data[index] = occupancyValue;
                    }
                }
            }
            else // P2 - ASCII format
            {
                // ASCII format - read from file starting at binaryDataStartPosition
                using var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read);
                fileStream.Position = binaryDataStartPosition;
                using var reader = new StreamReader(fileStream);
                
                // Read ASCII values
                var data = new List<int>();
                string? line;
                while ((line = reader.ReadLine()) != null)
                {
                    var values = line.Split(' ', StringSplitOptions.RemoveEmptyEntries);
                    foreach (var value in values)
                    {
                        if (int.TryParse(value, out var intValue))
                        {
                            data.Add(intValue);
                        }
                    }
                }

                if (data.Count != width * height)
                {
                    logger?.LogError("PgmLoader: Invalid data count. Expected {Expected}, got {Actual}", width * height, data.Count);
                    return null;
                }

                // Convert to occupancy values (same Y convention as P5 branch)
                // UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM)
                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++)
                    {
                        int index = y * width + x;
                        var pixelValue = data[index];
                        var occupancyValue = ConvertPixelToOccupancy((byte)pixelValue, maxValue);
                        occupancyGrid.Data[index] = occupancyValue;
                    }
                }
            }

            return occupancyGrid;
        }
        catch (Exception ex)
        {
            logger?.LogError(ex, "PgmLoader: Failed to load PGM file: {PgmPath}", pgmPath);
            return null;
        }
    }

    /// <summary>
    /// Convert PGM pixel value to occupancy grid value
    /// - 0 (black) = occupied (100)
    /// - 254 (white) = free (0)
    /// - 205 (gray) = unknown (-1)
    /// </summary>
    private static sbyte ConvertPixelToOccupancy(int pixelValue, int maxValue)
    {
        // ROS map_server convention:
        // - 0 = occupied
        // - 254 = free
        // - 205 = unknown
        // Scale to 0-255 range
        var normalizedValue = pixelValue * 255 / maxValue;

        if (normalizedValue == 0)
        {
            return 100; // Occupied
        }
        else if (normalizedValue == 254 || normalizedValue == 255)
        {
            return 0; // Free
        }
        else if (normalizedValue >= 200 && normalizedValue <= 210)
        {
            return -1; // Unknown
        }
        else
        {
            // Interpolate: 0-200 = occupied (100-0), 210-254 = free (0)
            if (normalizedValue < 200)
            {
                return (sbyte)Math.Clamp(100 - (normalizedValue * 100 / 200), 0, 100);
            }
            else
            {
                return 0; // Free
            }
        }
    }

    /// <summary>
    /// Parse YAML metadata file (ROS map format)
    /// </summary>
    private static (double resolution, Pose origin) ParseYamlMetadata(string yamlPath, ILogger? logger)
    {
        double resolution = 0.05;
        Pose origin = new()
        {
            Position = new Vector3(0, 0, 0),
            Orientation = new Quaternion(0, 0, 0, 1)
        };

        try
        {
            var lines = File.ReadAllLines(yamlPath);
            foreach (var line in lines)
            {
                var trimmed = line.Trim();
                if (trimmed.StartsWith('#') || string.IsNullOrWhiteSpace(trimmed))
                    continue;

                // Parse key: value
                var colonIndex = trimmed.IndexOf(':');
                if (colonIndex < 0)
                    continue;

                var key = trimmed[..colonIndex].Trim();
                var value = trimmed[(colonIndex + 1)..].Trim();

                switch (key.ToLowerInvariant())
                {
                    case "resolution":
                        if (double.TryParse(value, out var res))
                        {
                            resolution = res;
                        }
                        break;

                    case "origin":
                        // Format: [x, y, yaw] or [x, y, 0]
                        value = value.TrimStart('[').TrimEnd(']');
                        var coords = value.Split(',');
                        if (coords.Length >= 2)
                        {
                            if (double.TryParse(coords[0].Trim(), out var x) &&
                                double.TryParse(coords[1].Trim(), out var y))
                            {
                                origin.Position = new Vector3(x, y, 0);
                            }
                        }
                        if (coords.Length >= 3)
                        {
                            if (double.TryParse(coords[2].Trim(), out var yaw))
                            {
                                // Convert yaw (radians) to quaternion
                                var halfYaw = yaw / 2.0;
                                origin.Orientation = new Quaternion(0, 0, Math.Sin(halfYaw), Math.Cos(halfYaw));
                            }
                        }
                        break;
                }
            }
        }
        catch (Exception ex)
        {
            logger?.LogWarning(ex, "PgmLoader: Failed to parse YAML metadata: {YamlPath}", yamlPath);
        }

        return (resolution, origin);
    }
}