using RobotNet10.Shared.Localization;
using SkiaSharp;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Helper class for saving OccupancyGrid to various file formats.
/// Consolidates file saving logic used by both CartographerService and MapSaveProcessor.
/// </summary>
public static class OccupancyGridFileHelper
{
    #region Public API

    /// <summary>
    /// Save all map file formats (PGM, YAML, PNG, JPG) to the specified directory.
    /// </summary>
    public static async Task SaveAllFormatsAsync(
        OccupancyGrid occupancyGrid,
        string mapPath,
        CancellationToken cancellationToken = default,
        ILogger? logger = null)
    {
        var pgmPath = Path.Combine(mapPath, "map.pgm");
        var yamlPath = Path.Combine(mapPath, "map.yaml");
        var pngPath = Path.Combine(mapPath, "map.png");
        var jpgPath = Path.Combine(mapPath, "map.jpg");

        SaveAsPgm(occupancyGrid, pgmPath, logger);
        SaveAsYaml(occupancyGrid, yamlPath, "map.png", logger);
        await Task.Run(() => SaveAsPng(occupancyGrid, pngPath, cancellationToken, logger), cancellationToken);
        await Task.Run(() => SaveAsJpg(occupancyGrid, jpgPath, cancellationToken, logger), cancellationToken);
    }

    /// <summary>
    /// Save occupancy grid as PGM file (binary format P5)
    /// </summary>
    public static void SaveAsPgm(OccupancyGrid occupancyGrid, string pgmPath, ILogger? logger = null)
    {
        try
        {
            using var fileStream = new FileStream(pgmPath, FileMode.Create, FileAccess.Write);
            using var writer = new StreamWriter(fileStream);

            // Write PGM header
            writer.WriteLine("P5"); // Binary format
            writer.WriteLine($"{occupancyGrid.Width} {occupancyGrid.Height}");
            writer.WriteLine("255"); // Max value

            // Flush header before writing binary data
            writer.Flush();

            // Write pixel data directly without Y-flip.
            // Both OccupancyGrid and PGM file use same convention for consistency with PgmLoader.
            var buffer = new byte[occupancyGrid.Width * occupancyGrid.Height];
            for (int y = 0; y < occupancyGrid.Height; y++)
            {
                for (int x = 0; x < occupancyGrid.Width; x++)
                {
                    int index = y * occupancyGrid.Width + x;
                    var occupancyValue = occupancyGrid.Data[index];

                    if (occupancyValue == -1)
                    {
                        buffer[index] = 205; // Unknown (gray)
                    }
                    else
                    {
                        // Convert occupancy (0-100) to PGM (0-255)
                        // occupancy 0 (free) -> PGM 254 (white)
                        // occupancy 100 (occupied) -> PGM 0 (black)
                        buffer[index] = (byte)(254 - (occupancyValue * 254 / 100));
                    }
                }
            }

            fileStream.Write(buffer, 0, buffer.Length);
            fileStream.Flush();
        }
        catch (Exception ex)
        {
            logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save PGM file: {Path}", pgmPath);
            throw;
        }
    }

    /// <summary>
    /// Save occupancy grid as YAML file (ROS map format)
    /// </summary>
    public static void SaveAsYaml(OccupancyGrid occupancyGrid, string yamlPath, string imageFilename, ILogger? logger = null)
    {
        try
        {
            var originX = occupancyGrid.Origin.Position.X;
            var originY = occupancyGrid.Origin.Position.Y;

            // Write YAML file (ROS map format)
            var yamlContent = $"image: {imageFilename}\n" +
                            $"resolution: {occupancyGrid.Resolution:F10}\n" +
                            $"origin: [{originX:F10}, {originY:F10}, 0.0]\n" +
                            $"negate: 0\n" +
                            $"occupied_thresh: 0.65\n" +
                            $"free_thresh: 0.196\n";

            File.WriteAllText(yamlPath, yamlContent);
        }
        catch (Exception ex)
        {
            logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save YAML file: {Path}", yamlPath);
            throw;
        }
    }

    /// <summary>
    /// Save occupancy grid as PNG file
    /// </summary>
    public static void SaveAsPng(OccupancyGrid occupancyGrid, string pngPath, CancellationToken cancellationToken = default, ILogger? logger = null)
    {
        RenderAndSaveImage(occupancyGrid, pngPath, SKEncodedImageFormat.Png, 100, cancellationToken, logger);
    }

    /// <summary>
    /// Save occupancy grid as JPG file
    /// </summary>
    public static void SaveAsJpg(OccupancyGrid occupancyGrid, string jpgPath, CancellationToken cancellationToken = default, ILogger? logger = null)
    {
        RenderAndSaveImage(occupancyGrid, jpgPath, SKEncodedImageFormat.Jpeg, 95, cancellationToken, logger);
    }

    #endregion

    #region Image Rendering

    /// <summary>
    /// Render occupancy grid to an image and save in the specified format.
    /// Shared implementation for both PNG and JPG output.
    /// </summary>
    private static void RenderAndSaveImage(
        OccupancyGrid occupancyGrid,
        string outputPath,
        SKEncodedImageFormat format,
        int quality,
        CancellationToken cancellationToken,
        ILogger? logger)
    {
        var formatName = format == SKEncodedImageFormat.Png ? "PNG" : "JPG";
        try
        {
            if (occupancyGrid == null || occupancyGrid.Width <= 0 || occupancyGrid.Height <= 0)
            {
                logger?.LogWarning("OccupancyGridFileHelper: Cannot save {Format} - invalid occupancy grid", formatName);
                return;
            }

            cancellationToken.ThrowIfCancellationRequested();

            var width = occupancyGrid.Width;
            var height = occupancyGrid.Height;

            // Create SKBitmap with RGBA_8888 format
            using var bitmap = new SKBitmap(width, height, SKColorType.Rgba8888, SKAlphaType.Opaque);

            // Get pixel buffer pointer for direct memory access
            var pixelsPtr = bitmap.GetPixels();
            if (pixelsPtr == IntPtr.Zero)
            {
                throw new InvalidOperationException("Failed to get pixel buffer from bitmap");
            }

            // Convert occupancy grid data to image pixels
            // Flip Y: OccupancyGrid uses ROS convention (row 0 = world bottom, Y up)
            // but PNG/JPG image convention is row 0 = top, Y down.
            unsafe
            {
                var pixels = (byte*)pixelsPtr.ToPointer();

                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++)
                    {
                        // Read from flipped Y in grid (bottom-up) to write top-down in image
                        var srcIndex = (height - 1 - y) * width + x;
                        var occupancyValue = occupancyGrid.Data[srcIndex];

                        byte intensity;
                        if (occupancyValue == -1)
                        {
                            intensity = 205; // Unknown (gray)
                        }
                        else if (occupancyValue == 0)
                        {
                            intensity = 254; // Free space (white)
                        }
                        else
                        {
                            // Occupied space: Convert occupancy (0-100) to pixel intensity (0-255)
                            var intensityValue = 254.0 - (occupancyValue * 254.0 / 100.0);
                            intensity = (byte)Math.Clamp((int)Math.Round(intensityValue), 0, 254);
                        }

                        // Write RGBA bytes directly (destination index uses image row y)
                        var dstIndex = y * width + x;
                        var pixelOffset = dstIndex * 4;
                        pixels[pixelOffset] = intensity;     // R
                        pixels[pixelOffset + 1] = intensity; // G
                        pixels[pixelOffset + 2] = intensity; // B
                        pixels[pixelOffset + 3] = 255;       // A (opaque)
                    }
                }
            }

            // Encode and save
            using var image = SKImage.FromBitmap(bitmap) ?? throw new InvalidOperationException("Failed to create SKImage from bitmap");
            using var data = image.Encode(format, quality) ?? throw new InvalidOperationException($"Failed to encode {formatName} image");

            var directory = Path.GetDirectoryName(outputPath);
            if (!string.IsNullOrEmpty(directory))
            {
                Directory.CreateDirectory(directory);
            }

            if (File.Exists(outputPath))
            {
                File.Delete(outputPath);
            }

            using var stream = File.Create(outputPath);
            data.SaveTo(stream);
            stream.Flush();
        }
        catch (OperationCanceledException)
        {
            logger?.LogWarning("OccupancyGridFileHelper: {Format} save cancelled: {Path}", formatName, outputPath);
            throw;
        }
        catch (Exception ex)
        {
            logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save {Format} file: {Path}", formatName, outputPath);
            // Don't throw - image files are optional
        }
    }

    #endregion
}