/*
 * Copyright 2017 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using CartographerSharp.Mapping;
using CartographerSharp.Mapping.D2D;
using CartographerSharp.Transform;
using RobotNet10.Shared.Numbers;
using System.IO.Compression;
using System.Runtime.CompilerServices;

// Use aliases to avoid ambiguity between Mapping.D2D.Submap2D and Models.Mapping.Submap2D
using Submap2DClass = CartographerSharp.Mapping.D2D.Submap2D;
using SubmapQueryModel = CartographerSharp.Models.Mapping.SubmapQuery;

namespace CartographerSharp.IO;

/// <summary>
/// Represents unpacked texture pixel data.
/// Match C++: SubmapTexture::Pixels
/// </summary>
public readonly struct SubmapTexturePixels
{
    public readonly byte[] Intensity;
    public readonly byte[] Alpha;

    public SubmapTexturePixels(byte[] intensity, byte[] alpha)
    {
        Intensity = intensity;
        Alpha = alpha;
    }
}

/// <summary>
/// Represents a submap slice ready for painting.
/// Match C++: SubmapSlice
/// </summary>
public class SubmapSlice
{
    // Texture data
    public int Width { get; set; }
    public int Height { get; set; }
    public int Version { get; set; }
    public double Resolution { get; set; }
    public Rigid3d SlicePose { get; set; }

    // Pixel data (ARGB format, uint32 per pixel)
    public uint[]? PixelData { get; set; }

    // Metadata
    public Rigid3d Pose { get; set; }
    public int MetadataVersion { get; set; } = -1;
}

/// <summary>
/// Result of painting submap slices.
/// Match C++: PaintSubmapSlicesResult
/// </summary>
public class PaintSubmapSlicesResult
{
    /// <summary>
    /// Pixel data in ARGB format (row-major, top-to-bottom).
    /// </summary>
    public uint[] PixelData { get; }

    /// <summary>
    /// Width of the result image in pixels.
    /// </summary>
    public int Width { get; }

    /// <summary>
    /// Height of the result image in pixels.
    /// </summary>
    public int Height { get; }

    /// <summary>
    /// Top-left pixel of 'surface' in map frame (world coordinates).
    /// </summary>
    public Vector2 Origin { get; }

    public PaintSubmapSlicesResult(uint[] pixelData, int width, int height, Vector2 origin)
    {
        PixelData = pixelData;
        Width = width;
        Height = height;
        Origin = origin;
    }
}

/// <summary>
/// Submap painting utilities for generating occupancy grids from submap textures.
/// Enhanced implementation matching Cairo Graphics Library quality:
/// - Bilinear interpolation for smooth sampling
/// - Porter-Duff Source-Over compositing for proper alpha blending
/// - Inverse mapping for sub-pixel accuracy (no holes)
/// - Affine transformation matrix support
/// </summary>
public static class SubmapPainter
{
    private const int kPaddingPixel = 5;

    /// <summary>
    /// Unpacks cell data as provided by DrawToSubmapTexture into intensity and alpha arrays.
    /// Match C++: UnpackTextureData
    /// </summary>
    /// <param name="compressedCells">GZip compressed cells data (value + alpha pairs)</param>
    /// <param name="width">Texture width</param>
    /// <param name="height">Texture height</param>
    /// <returns>Unpacked intensity and alpha arrays</returns>
    public static SubmapTexturePixels UnpackTextureData(IList<byte> compressedCells, int width, int height)
    {
        // Decompress GZip data
        byte[] cells;
        using (var compressedStream = new MemoryStream(compressedCells.ToArray()))
        using (var gzipStream = new GZipStream(compressedStream, CompressionMode.Decompress))
        using (var resultStream = new MemoryStream())
        {
            gzipStream.CopyTo(resultStream);
            cells = resultStream.ToArray();
        }

        var numPixels = width * height;
        if (cells.Length != 2 * numPixels)
        {
            throw new ArgumentException(
                $"Decompressed cells size mismatch: expected {2 * numPixels}, got {cells.Length}");
        }

        var intensity = new byte[numPixels];
        var alpha = new byte[numPixels];

        // Match C++: cells[(i * width + j) * 2] for intensity, +1 for alpha
        for (int i = 0; i < height; i++)
        {
            for (int j = 0; j < width; j++)
            {
                var index = i * width + j;
                intensity[index] = cells[index * 2];
                alpha[index] = cells[index * 2 + 1];
            }
        }

        return new SubmapTexturePixels(intensity, alpha);
    }

    /// <summary>
    /// Creates pixel data from intensity and alpha arrays.
    /// Match C++: DrawTexture (without Cairo, using raw pixel arrays)
    /// </summary>
    /// <param name="intensity">Intensity values</param>
    /// <param name="alpha">Alpha values</param>
    /// <param name="width">Texture width</param>
    /// <param name="height">Texture height</param>
    /// <returns>ARGB pixel data (uint32 per pixel)</returns>
    public static uint[] DrawTexture(byte[] intensity, byte[] alpha, int width, int height)
    {
        var pixelData = new uint[width * height];

        for (int i = 0; i < intensity.Length; i++)
        {
            var intensityValue = intensity[i];
            var alphaValue = alpha[i];

            // Match C++: We use the red channel to track intensity information.
            // The green channel we use to track if a cell was ever observed.
            byte observed = (intensityValue == 0 && alphaValue == 0) ? (byte)0 : (byte)255;

            // ARGB format: (alpha << 24) | (red << 16) | (green << 8) | blue
            // Match C++: (alpha_value << 24) | (intensity_value << 16) | (observed << 8) | 0
            pixelData[i] = ((uint)alphaValue << 24) | ((uint)intensityValue << 16) | ((uint)observed << 8) | 0;
        }

        return pixelData;
    }

    /// <summary>
    /// Fills a SubmapSlice from a Submap2D.
    /// Match C++: Part of FillSubmapSlice functionality
    /// </summary>
    public static SubmapSlice CreateSubmapSlice(Submap2DClass submap, Rigid3d globalPose)
    {
        var slice = new SubmapSlice
        {
            Pose = globalPose,
            MetadataVersion = submap.NumRangeData
        };

        var grid = submap.Grid;
        if (grid == null)
        {
            return slice;
        }

        // Get texture from grid
        SubmapQueryModel.Texture texture;
        if (grid is ProbabilityGrid probabilityGrid)
        {
            texture = probabilityGrid.DrawToSubmapTexture(submap.LocalPose);
        }
        else if (grid is TSDF2D tsdf2D)
        {
            texture = tsdf2D.DrawToSubmapTexture(submap.LocalPose);
        }
        else
        {
            throw new NotSupportedException($"Unsupported grid type: {grid.GetType().Name}");
        }

        // Unpack texture data
        var pixels = UnpackTextureData(texture.Cells, texture.Width, texture.Height);

        slice.Width = texture.Width;
        slice.Height = texture.Height;
        slice.Resolution = texture.Resolution;
        slice.SlicePose = texture.SlicePose;
        slice.Version = submap.NumRangeData;

        // Draw texture to pixel data
        slice.PixelData = DrawTexture(pixels.Intensity, pixels.Alpha, texture.Width, texture.Height);

        return slice;
    }

    /// <summary>
    /// Paints all submap slices into a single image using Cairo-style rendering:
    /// - Inverse mapping for sub-pixel accuracy
    /// - Bilinear interpolation for smooth sampling
    /// - Porter-Duff Source-Over compositing
    /// Match C++: PaintSubmapSlices
    /// </summary>
    /// <param name="submapSlices">Dictionary of submap slices keyed by SubmapId</param>
    /// <param name="resolution">Output resolution in meters per pixel</param>
    /// <returns>Combined image result with pixel data and origin</returns>
    public static PaintSubmapSlicesResult? PaintSubmapSlices(
        Dictionary<SubmapId, SubmapSlice> submapSlices,
        double resolution)
    {
        if (submapSlices.Count == 0)
        {
            return null;
        }

        // First pass: compute bounding box using all corner transforms
        double minX = double.MaxValue, minY = double.MaxValue;
        double maxX = double.MinValue, maxY = double.MinValue;

        foreach (var (_, slice) in submapSlices)
        {
            if (slice.PixelData == null || slice.Width <= 0 || slice.Height <= 0)
            {
                continue;
            }

            // Transform the four corners of the submap texture to global coordinates
            var corners = new Vector2[]
            {
                new(0, 0),
                new(slice.Width, 0),
                new(0, slice.Height),
                new(slice.Width, slice.Height)
            };

            // Combined transform: globalPose * slicePose
            var submapTransform = slice.Pose * slice.SlicePose;

            foreach (var corner in corners)
            {
                // Convert pixel coordinates to submap local coordinates
                // Match C++ Cairo matrix: cairo_matrix_init(&matrix, homo(1,0), homo(0,0),
                //   -homo(1,1), -homo(0,1), homo(0,3), -homo(1,3))
                // In Cartographer's grid convention:
                //   x-index (column) corresponds to world Y axis (decreasing)
                //   y-index (row) corresponds to world X axis (decreasing)
                // So pixel (col, row) maps to local (-row * res, -col * res) + slice_pose translation
                var localPoint = new Vector3(
                    -corner.Y * slice.Resolution,
                    -corner.X * slice.Resolution,
                    0);

                // Transform to global coordinates
                var globalPoint = submapTransform.TransformPoint(localPoint);

                // Update bounding box
                // Match C++: cairo uses (x, -y) convention for map coordinates
                var mapX = globalPoint.X / resolution;
                var mapY = -globalPoint.Y / resolution;

                minX = Math.Min(minX, mapX);
                minY = Math.Min(minY, mapY);
                maxX = Math.Max(maxX, mapX);
                maxY = Math.Max(maxY, mapY);
            }
        }

        if (minX >= maxX || minY >= maxY)
        {
            return null;
        }

        // Calculate output size with padding
        var width = (int)Math.Ceiling(maxX - minX) + 2 * kPaddingPixel;
        var height = (int)Math.Ceiling(maxY - minY) + 2 * kPaddingPixel;

        // Origin offset (translation to apply to bring min corner to (padding, padding))
        var originX = -minX + kPaddingPixel;
        var originY = -minY + kPaddingPixel;

        // Create output pixel buffer
        // Match C++: cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.); - dark red background
        // For occupancy grid: observed=0 indicates unknown
        var outputPixels = new uint[width * height];
        // Initialize to unknown (gray color, observed=0)
        for (int i = 0; i < outputPixels.Length; i++)
        {
            outputPixels[i] = 0xFF800000; // Alpha=255, Red=128 (gray), Green=0 (not observed), Blue=0
        }

        // Second pass: paint each submap slice using inverse mapping + bilinear interpolation
        foreach (var (_, slice) in submapSlices)
        {
            if (slice.PixelData == null || slice.Width <= 0 || slice.Height <= 0)
            {
                continue;
            }

            PaintSubmapSliceCairoStyle(slice, resolution, originX, originY, outputPixels, width, height);
        }

        // Calculate the origin in world coordinates
        var worldOrigin = new Vector2(
            (minX - kPaddingPixel) * resolution,
            -(minY - kPaddingPixel) * resolution);

        return new PaintSubmapSlicesResult(outputPixels, width, height, worldOrigin);
    }

    /// <summary>
    /// Paints a single submap slice using Cairo-style rendering:
    /// - Inverse mapping: for each output pixel, compute source position
    /// - Bilinear interpolation: sample from 4 neighboring pixels
    /// - Porter-Duff Source-Over: proper alpha compositing
    /// </summary>
    private static void PaintSubmapSliceCairoStyle(
        SubmapSlice slice,
        double resolution,
        double originX,
        double originY,
        uint[] outputPixels,
        int outputWidth,
        int outputHeight)
    {
        if (slice.PixelData == null)
        {
            return;
        }

        // Build affine transformation matrix (Cairo style)
        // Transform chain: output_pixel -> world -> submap_local -> submap_pixel
        var submapTransform = slice.Pose * slice.SlicePose;
        var inverseTransform = submapTransform.Inverse();

        // Pre-compute scale factors
        var outputToWorld = resolution;
        var worldToSubmap = 1.0 / slice.Resolution;

        // Compute the bounding box of this slice in output coordinates
        // to avoid iterating over the entire output
        var sliceCorners = new Vector2[]
        {
            new(0, 0),
            new(slice.Width, 0),
            new(0, slice.Height),
            new(slice.Width, slice.Height)
        };

        int outMinX = outputWidth, outMinY = outputHeight;
        int outMaxX = 0, outMaxY = 0;

        foreach (var corner in sliceCorners)
        {
            // Match C++ Cairo matrix pixel-to-local mapping
            var localPoint = new Vector3(-corner.Y * slice.Resolution, -corner.X * slice.Resolution, 0);
            var globalPoint = submapTransform.TransformPoint(localPoint);
            var outX = (int)Math.Floor(globalPoint.X / resolution + originX);
            var outY = (int)Math.Floor(-globalPoint.Y / resolution + originY);

            outMinX = Math.Min(outMinX, outX - 2);
            outMinY = Math.Min(outMinY, outY - 2);
            outMaxX = Math.Max(outMaxX, outX + 2);
            outMaxY = Math.Max(outMaxY, outY + 2);
        }

        // Clamp to output bounds
        outMinX = Math.Max(0, outMinX);
        outMinY = Math.Max(0, outMinY);
        outMaxX = Math.Min(outputWidth - 1, outMaxX);
        outMaxY = Math.Min(outputHeight - 1, outMaxY);

        // Inverse mapping: for each output pixel in the bounding box
        for (int outY = outMinY; outY <= outMaxY; outY++)
        {
            for (int outX = outMinX; outX <= outMaxX; outX++)
            {
                // Convert output pixel to world coordinates
                // Match C++ cairo convention: output uses (x, -y)
                var worldX = (outX - originX) * outputToWorld;
                var worldY = -(outY - originY) * outputToWorld;

                // Transform world to submap local coordinates
                var worldPoint = new Vector3(worldX, worldY, 0);
                var submapLocalPoint = inverseTransform.TransformPoint(worldPoint);

                // Convert submap local to pixel coordinates
                // Inverse of the forward mapping: local = (-row * res, -col * res)
                // So: col = -local.Y / res, row = -local.X / res
                var srcX = -submapLocalPoint.Y * worldToSubmap;
                var srcY = -submapLocalPoint.X * worldToSubmap;

                // Check if within source bounds (with margin for bilinear)
                if (srcX < 0 || srcX >= slice.Width - 1 || srcY < 0 || srcY >= slice.Height - 1)
                {
                    continue;
                }

                // Bilinear interpolation
                var sampledPixel = SampleBilinear(slice.PixelData, slice.Width, slice.Height, srcX, srcY);

                // Skip if not observed
                var srcObserved = (sampledPixel >> 8) & 0xFF;
                if (srcObserved == 0)
                {
                    continue;
                }

                // Porter-Duff Source-Over compositing
                var dstIndex = outY * outputWidth + outX;
                var dstPixel = outputPixels[dstIndex];

                outputPixels[dstIndex] = BlendSourceOver(sampledPixel, dstPixel);
            }
        }
    }

    /// <summary>
    /// Bilinear interpolation sampling from a pixel array.
    /// Returns interpolated ARGB pixel value.
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static uint SampleBilinear(uint[] pixels, int width, int height, double x, double y)
    {
        // Get integer and fractional parts
        int x0 = (int)Math.Floor(x);
        int y0 = (int)Math.Floor(y);
        int x1 = Math.Min(x0 + 1, width - 1);
        int y1 = Math.Min(y0 + 1, height - 1);

        double fx = x - x0;
        double fy = y - y0;

        // Get four neighboring pixels
        var p00 = pixels[y0 * width + x0];
        var p10 = pixels[y0 * width + x1];
        var p01 = pixels[y1 * width + x0];
        var p11 = pixels[y1 * width + x1];

        // Check if all neighbors are observed (optimization: skip interpolation if any is unknown)
        var obs00 = (p00 >> 8) & 0xFF;
        var obs10 = (p10 >> 8) & 0xFF;
        var obs01 = (p01 >> 8) & 0xFF;
        var obs11 = (p11 >> 8) & 0xFF;

        // If any corner is unobserved, use nearest neighbor with observed pixel
        if (obs00 == 0 || obs10 == 0 || obs01 == 0 || obs11 == 0)
        {
            // Find the nearest observed pixel
            var nearestX = fx < 0.5 ? x0 : x1;
            var nearestY = fy < 0.5 ? y0 : y1;
            var nearest = pixels[nearestY * width + nearestX];
            if (((nearest >> 8) & 0xFF) != 0)
            {
                return nearest;
            }

            // Try other corners
            if (obs00 != 0) return p00;
            if (obs10 != 0) return p10;
            if (obs01 != 0) return p01;
            if (obs11 != 0) return p11;

            return 0; // All unobserved
        }

        // Bilinear interpolation weights
        double w00 = (1 - fx) * (1 - fy);
        double w10 = fx * (1 - fy);
        double w01 = (1 - fx) * fy;
        double w11 = fx * fy;

        // Interpolate each channel
        var a = (uint)Math.Round(
            ((p00 >> 24) & 0xFF) * w00 +
            ((p10 >> 24) & 0xFF) * w10 +
            ((p01 >> 24) & 0xFF) * w01 +
            ((p11 >> 24) & 0xFF) * w11);

        var r = (uint)Math.Round(
            ((p00 >> 16) & 0xFF) * w00 +
            ((p10 >> 16) & 0xFF) * w10 +
            ((p01 >> 16) & 0xFF) * w01 +
            ((p11 >> 16) & 0xFF) * w11);

        var g = (uint)Math.Round(
            ((p00 >> 8) & 0xFF) * w00 +
            ((p10 >> 8) & 0xFF) * w10 +
            ((p01 >> 8) & 0xFF) * w01 +
            ((p11 >> 8) & 0xFF) * w11);

        var b = (uint)Math.Round(
            (p00 & 0xFF) * w00 +
            (p10 & 0xFF) * w10 +
            (p01 & 0xFF) * w01 +
            (p11 & 0xFF) * w11);

        return (Math.Min(255u, a) << 24) |
               (Math.Min(255u, r) << 16) |
               (Math.Min(255u, g) << 8) |
               Math.Min(255u, b);
    }

    /// <summary>
    /// Porter-Duff Source-Over compositing matching Cairo's OVER operator.
    ///
    /// Cairo uses premultiplied alpha OVER: result = src + dst * (1 - srcA/255)
    ///
    /// This naturally produces the correct behavior for occupancy grids:
    /// - Free cells (srcA=0): additive blending (factor=1.0) → R gets brighter with more observations
    /// - Occupied cells (srcA>0): standard OVER → R gets darker with more observations
    /// - Multiple free observations → brighter (lower occupancy = more confident free space)
    /// - Multiple occupied observations → darker (higher occupancy = more confident wall)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static uint BlendSourceOver(uint src, uint dst)
    {
        // Extract channels
        // Format: (alpha << 24) | (intensity/red << 16) | (observed/green << 8) | blue
        var srcA = (src >> 24) & 0xFF;
        var srcR = (src >> 16) & 0xFF;
        var srcG = (src >> 8) & 0xFF;
        var srcB = src & 0xFF;

        var dstA = (dst >> 24) & 0xFF;
        var dstR = (dst >> 16) & 0xFF;
        var dstG = (dst >> 8) & 0xFF;
        var dstB = dst & 0xFF;

        // Cairo OVER operator (premultiplied alpha):
        // result = src + dst * (1 - srcA / 255)
        var factor = 1.0 - srcA / 255.0;

        var outA = (uint)Math.Min(255, (int)Math.Round(srcA + dstA * factor));
        var outR = (uint)Math.Min(255, (int)Math.Round(srcR + dstR * factor));
        var outG = (uint)Math.Min(255, (int)Math.Round(srcG + dstG * factor));
        var outB = (uint)Math.Min(255, (int)Math.Round(srcB + dstB * factor));

        return (outA << 24) | (outR << 16) | (outG << 8) | outB;
    }

    /// <summary>
    /// Converts painted result to occupancy grid values.
    /// Returns array of occupancy values: 0 = free, 100 = occupied, -1 = unknown.
    /// Match C++: CreateOccupancyGrid in data_conversion.cc
    /// </summary>
    /// <param name="result">Paint result from PaintSubmapSlices</param>
    /// <returns>Array of sbyte occupancy values</returns>
    public static sbyte[] ConvertToOccupancyValues(PaintSubmapSlicesResult result)
    {
        var occupancyValues = new sbyte[result.Width * result.Height];

        for (int i = 0; i < result.PixelData.Length; i++)
        {
            var pixel = result.PixelData[i];
            // Match C++ pixel format: (alpha << 24) | (intensity/color << 16) | (observed << 8) | 0
            var color = (pixel >> 16) & 0xFF;     // RED channel = intensity/color
            var observed = (pixel >> 8) & 0xFF;   // GREEN channel = observed flag

            if (observed == 0)
            {
                // Unknown cell - not observed
                occupancyValues[i] = -1;
            }
            else
            {
                // Match C++ formula from data_conversion.cc line 386-389:
                // const int value = observed == 0
                //     ? -1
                //     : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
                //
                // color = 0 (black) → occupancy = 100 (occupied)
                // color = 255 (white) → occupancy = 0 (free)
                var occupancy = (int)Math.Round((1.0 - color / 255.0) * 100.0);
                occupancyValues[i] = (sbyte)Math.Clamp(occupancy, 0, 100);
            }
        }

        return occupancyValues;
    }
}