using CartographerSharp.IO;
using CartographerSharp.Mapping;
using CartographerSharp.Mapping.D2D;
using CartographerSharp.Transform;
using RobotNet10.RobotApp.SLAM.Cartographer;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Unified occupancy grid generator from MapBuilder submaps.
/// UNIFIED CONVENTION: All grids use ROS convention (row 0 = world BOTTOM, Y-axis pointing UP).
/// </summary>
public static class OccupancyGridGenerator
{
    #region Public API

    /// <summary>
    /// Generate occupancy grid from MapBuilder using ROS convention.
    /// </summary>
    /// <param name="mapBuilder">MapBuilder containing submaps</param>
    /// <param name="resolution">Target grid resolution</param>
    /// <param name="padding">Padding around map bounds</param>
    /// <param name="logger">Optional logger</param>
    /// <returns>OccupancyGrid with ROS convention (row 0 = world bottom), or null if generation fails</returns>
    public static OccupancyGrid? GenerateFromMapBuilder(
        IMapBuilder mapBuilder,
        double resolution,
        double padding,
        ILogger? logger = null)
    {
        try
        {
            var poseGraph = mapBuilder.PoseGraph;
            var allSubmapData = poseGraph.GetAllSubmapData();

            // Get TransformToMap and compute its inverse for applying to all poses
            // This matches C++ xloc.cc behavior where poses are transformed using:
            // transformedPose = GetTransformToMap().inverse() * pose
            var transformToMap = poseGraph.GetTransformToMap();
            var transformToMapInverse = transformToMap.Inverse();

            // Collect all 2D submaps from pose graph (finished submaps)
            // IMPORTANT: We store localToMapTransform (NOT globalPose) because:
            // - GetCellCenter() returns coordinates in LOCAL MAP FRAME (trajectory local frame)
            // - NOT in submap frame as previously assumed
            // - localToMapTransform = TransformToMapInverse * LocalToGlobalTransform
            // - This correctly transforms from local map frame to map frame
            var submap2DList = new List<(Submap2D Submap, Rigid3d LocalToMapTransform)>();

            int submapIndexDebug = 0;
            foreach (var idDataRef in allSubmapData)
            {
                var submapData = idDataRef.Data;
                if (submapData.Submap is Submap2D submap2D)
                {
                    var submapId = idDataRef.Id;
                    var trajectoryId = submapId.TrajectoryId;

                    // FIX: Use LocalToGlobalTransform instead of SubmapData.Pose
                    // Cell coordinates from GetCellCenter() are in LOCAL MAP FRAME (L),
                    // so we need T_map_local = TransformToMapInverse * LocalToGlobalTransform
                    // Previously used: T_map_submap = TransformToMapInverse * SubmapData.Pose (WRONG)
                    var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
                    Rigid3d localToMapTransform = transformToMapInverse * localToGlobal;

                    // Validate that the transform is valid (not NaN/Infinity)
                    if (!localToMapTransform.IsValid())
                    {
                        logger?.LogWarning(
                            "OccupancyGridGenerator: Submap {TrajectoryId}:{SubmapIndex} has invalid localToMapTransform, skipping",
                            submapId.TrajectoryId, submapId.SubmapIndex);
                        continue;
                    }

                    submap2DList.Add((submap2D, localToMapTransform));
                    submapIndexDebug++;
                }
            }

            if (submap2DList.Count == 0)
            {
                logger?.LogWarning("OccupancyGridGenerator: No 2D submaps found");
                return null;
            }

            // Calculate bounds from all submaps
            var (minX, minY, maxX, maxY) = CalculateBoundsFromSubmaps(submap2DList);

            // Validate bounds
            if (minX >= maxX || minY >= maxY || double.IsInfinity(minX) || double.IsInfinity(maxX) ||
                double.IsInfinity(minY) || double.IsInfinity(maxY))
            {
                logger?.LogWarning(
                    "OccupancyGridGenerator: Invalid bounds - minX={MinX}, minY={MinY}, maxX={MaxX}, maxY={MaxY}",
                    minX, minY, maxX, maxY);
                return null;
            }

            // Add padding
            var adjustedMinX = minX - padding;
            var adjustedMinY = minY - padding;
            var adjustedMaxX = maxX + padding;
            var adjustedMaxY = maxY + padding;

            var width = (int)Math.Ceiling((adjustedMaxX - adjustedMinX) / resolution);
            var height = (int)Math.Ceiling((adjustedMaxY - adjustedMinY) / resolution);

            // Set origin to bottom-left corner (cell (0,0)) according to ROS occupancy grid convention
            var gridOriginPosition = new Vector3(adjustedMinX, adjustedMinY, 0.0);
            var origin = new Pose
            {
                Position = gridOriginPosition,
                Orientation = new Quaternion(0, 0, 0, 1)
            };

            var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);

            // Merge all submaps into occupancy grid
            int totalCellsMerged = 0;
            int totalCellsSkipped = 0;

            foreach (var (submap2D, localToMapTransform) in submap2DList)
            {
                var (merged, skipped) = MergeSubmapIntoOccupancyGrid(submap2D, localToMapTransform, occupancyGrid, resolution);
                totalCellsMerged += merged;
                totalCellsSkipped += skipped;
            }

            return occupancyGrid;
        }
        catch (Exception ex)
        {
            logger?.LogError(ex, "OccupancyGridGenerator: Failed to generate occupancy grid");
            return null;
        }
    }

    #endregion

    #region Bounds Calculation

    /// <summary>
    /// Calculate bounds from submaps.
    /// LocalToMapTransform transforms points from LOCAL MAP FRAME to MAP FRAME.
    /// </summary>
    private static (double minX, double minY, double maxX, double maxY) CalculateBoundsFromSubmaps(
        List<(Submap2D Submap, Rigid3d LocalToMapTransform)> submapList,
        ILogger? logger = null)
    {
        // Delegate to the snapshot-aware overload with null snapshots
        var extended = submapList
            .Select(t => (t.Submap, t.LocalToMapTransform, (Grid2D.GridCellSnapshot?)null))
            .ToList();
        return CalculateBoundsFromSubmaps(extended, logger);
    }

    private static (double minX, double minY, double maxX, double maxY) CalculateBoundsFromSubmaps(
        List<(Submap2D Submap, Rigid3d LocalToMapTransform, Grid2D.GridCellSnapshot? Snapshot)> submapList,
        ILogger? logger = null)
    {
        double minX = double.MaxValue, minY = double.MaxValue;
        double maxX = double.MinValue, maxY = double.MinValue;

        int submapIdx = 0;
        foreach (var (submap, localToMapTransform, snapshot) in submapList)
        {
            MapLimits limits;
            CartographerSharp.Common.Math.Array2i croppedOffset;
            CellLimits croppedLimits;

            if (snapshot != null)
            {
                // Use snapshot data (active submap – thread-safe)
                limits = snapshot.Limits;
                snapshot.ComputeCroppedLimits(out croppedOffset, out croppedLimits);
            }
            else
            {
                var grid = submap.Grid;
                if (grid == null) continue;
                limits = grid.Limits;
                grid.ComputeCroppedLimits(out croppedOffset, out croppedLimits);
            }

            if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
                continue;

            // Get bounds of cropped (known) cells in submap local frame
            var cornerIndices = new[]
            {
                new CartographerSharp.Common.Math.Array2i(croppedOffset.X, croppedOffset.Y),
                new CartographerSharp.Common.Math.Array2i(croppedOffset.X + croppedLimits.NumXCells - 1, croppedOffset.Y),
                new CartographerSharp.Common.Math.Array2i(croppedOffset.X, croppedOffset.Y + croppedLimits.NumYCells - 1),
                new CartographerSharp.Common.Math.Array2i(croppedOffset.X + croppedLimits.NumXCells - 1, croppedOffset.Y + croppedLimits.NumYCells - 1)
            };

            double croppedMinX = double.MaxValue, croppedMinY = double.MaxValue;
            double croppedMaxX = double.MinValue, croppedMaxY = double.MinValue;

            foreach (var cornerIndex in cornerIndices)
            {
                var cellCenter = limits.GetCellCenter(cornerIndex);
                croppedMinX = Math.Min(croppedMinX, cellCenter.X);
                croppedMinY = Math.Min(croppedMinY, cellCenter.Y);
                croppedMaxX = Math.Max(croppedMaxX, cellCenter.X);
                croppedMaxY = Math.Max(croppedMaxY, cellCenter.Y);
            }

            // Transform corners from local map frame to map frame
            var corners = new[]
            {
                new Vector2(croppedMinX, croppedMinY),
                new Vector2(croppedMaxX, croppedMinY),
                new Vector2(croppedMaxX, croppedMaxY),
                new Vector2(croppedMinX, croppedMaxY)
            };

            double submapGlobalMinX = double.MaxValue, submapGlobalMinY = double.MaxValue;
            double submapGlobalMaxX = double.MinValue, submapGlobalMaxY = double.MinValue;

            foreach (var corner in corners)
            {
                var cornerPoint = new Vector3(corner.X, corner.Y, 0);
                var mapCorner = localToMapTransform.TransformPoint(cornerPoint);

                submapGlobalMinX = Math.Min(submapGlobalMinX, mapCorner.X);
                submapGlobalMinY = Math.Min(submapGlobalMinY, mapCorner.Y);
                submapGlobalMaxX = Math.Max(submapGlobalMaxX, mapCorner.X);
                submapGlobalMaxY = Math.Max(submapGlobalMaxY, mapCorner.Y);

                minX = Math.Min(minX, mapCorner.X);
                minY = Math.Min(minY, mapCorner.Y);
                maxX = Math.Max(maxX, mapCorner.X);
                maxY = Math.Max(maxY, mapCorner.Y);
            }

            submapIdx++;
        }

        return (minX, minY, maxX, maxY);
    }

    #endregion

    #region Submap Processing

    /// <summary>
    /// Merge submap into occupancy grid using ROS convention.
    /// Supports both ProbabilityGrid and TSDF2D grid types.
    /// </summary>
    /// <param name="submap2D">The submap to merge</param>
    /// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
    /// <param name="occupancyGrid">Target occupancy grid</param>
    /// <param name="targetResolution">Target grid resolution</param>
    /// <returns>(mergedCells, skippedCells) for statistics</returns>
    private static (int mergedCells, int skippedCells) MergeSubmapIntoOccupancyGrid(
        Submap2D submap2D,
        Rigid3d localToMapTransform,
        OccupancyGrid occupancyGrid,
        double targetResolution)
    {
        var grid = submap2D.Grid;
        if (grid == null)
            return (0, 0);

        // Dispatch to appropriate handler based on grid type
        if (grid is ProbabilityGrid probabilityGrid)
        {
            return MergeSubmapIntoOccupancyGridFromProbabilityGrid(
                probabilityGrid, localToMapTransform, occupancyGrid, targetResolution);
        }
        else if (grid is TSDF2D tsdfGrid)
        {
            return MergeSubmapIntoOccupancyGridFromTSDF(
                tsdfGrid, localToMapTransform, occupancyGrid, targetResolution);
        }

        return (0, 0);
    }

    /// <summary>
    /// Merge ProbabilityGrid submap into occupancy grid.
    /// </summary>
    /// <param name="probabilityGrid">The probability grid to merge</param>
    /// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
    /// <param name="occupancyGrid">Target occupancy grid</param>
    /// <param name="targetResolution">Target grid resolution</param>
    private static (int mergedCells, int skippedCells) MergeSubmapIntoOccupancyGridFromProbabilityGrid(
        ProbabilityGrid probabilityGrid,
        Rigid3d localToMapTransform,
        OccupancyGrid occupancyGrid,
        double targetResolution)
    {
        var limits = probabilityGrid.Limits;
        int mergedCells = 0;
        int skippedCells = 0;

        // Use ComputeCroppedLimits to get only known cells bounds
        probabilityGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);

        if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
            return (0, 0);

        // Iterate through known cells region only
        for (int y = 0; y < croppedLimits.NumYCells; y++)
        {
            for (int x = 0; x < croppedLimits.NumXCells; x++)
            {
                var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);

                // Skip unknown cells
                if (!probabilityGrid.IsKnown(cellIndex))
                {
                    skippedCells++;
                    continue;
                }

                // Get cell center in LOCAL MAP FRAME (NOT submap frame!)
                // GetCellCenter returns coordinates relative to grid's Max, which is set
                // based on LocalPose.Translation when the submap is created.
                var cellCenter = limits.GetCellCenter(cellIndex);
                var cellCenterInLocalFrame = new Vector3(cellCenter.X, cellCenter.Y, 0.0);

                // Transform from LOCAL MAP FRAME to MAP FRAME
                var mapPoint = localToMapTransform.TransformPoint(cellCenterInLocalFrame);

                // Convert to occupancy grid coordinates
                var gridX = (int)Math.Floor((mapPoint.X - occupancyGrid.Origin.Position.X) / targetResolution);
                var gridY = (int)Math.Floor((mapPoint.Y - occupancyGrid.Origin.Position.Y) / targetResolution);

                if (gridX >= 0 && gridX < occupancyGrid.Width && gridY >= 0 && gridY < occupancyGrid.Height)
                {
                    // Get probability value from submap
                    var probability = probabilityGrid.GetProbability(cellIndex);

                    // Convert probability to occupancy value using Cartographer's texture-based conversion
                    var logOddsInteger = CartographerSharp.Mapping.SubmapProbabilityUtils.ProbabilityToLogOddsInteger(probability);
                    int delta = 128 - logOddsInteger;

                    byte textureValue = (byte)(delta > 0 ? delta : 0);
                    byte textureAlpha = (byte)(delta > 0 ? 0 : -delta);

                    // Convert texture value/alpha to occupancy value (0 = free, 100 = occupied, -1 = unknown)
                    sbyte occupancyValue;
                    if (textureAlpha > 0)
                    {
                        // Occupied space (high confidence)
                        occupancyValue = 100;
                    }
                    else if (textureValue >= 100)
                    {
                        // Free space (high confidence)
                        occupancyValue = 0;
                    }
                    else
                    {
                        // Skip ambiguous cells
                        skippedCells++;
                        continue;
                    }

                    var currentValue = occupancyGrid.GetCell(gridX, gridY);

                    // Merge logic: Protect known cells from being overwritten by unknown/ambiguous cells
                    bool shouldMerge = false;

                    if (currentValue == -1)
                    {
                        if (occupancyValue != -1)
                        {
                            shouldMerge = true;
                        }
                    }
                    else
                    {
                        if (occupancyValue != -1 && occupancyValue > currentValue)
                        {
                            shouldMerge = true;
                        }
                    }

                    if (shouldMerge)
                    {
                        occupancyGrid.Data[gridY * occupancyGrid.Width + gridX] = occupancyValue;
                        mergedCells++;
                    }
                    else
                    {
                        skippedCells++;
                    }
                }
                else
                {
                    skippedCells++;
                }
            }
        }

        return (mergedCells, skippedCells);
    }

    /// <summary>
    /// Merge TSDF2D submap into occupancy grid.
    /// TSDF convention:
    /// - tsd > 0: Free space (away from obstacles)
    /// - tsd < 0: Occupied space (inside/near obstacles)
    /// - tsd = 0: On obstacle surface
    /// </summary>
    /// <param name="tsdfGrid">The TSDF grid to merge</param>
    /// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
    /// <param name="occupancyGrid">Target occupancy grid</param>
    /// <param name="targetResolution">Target grid resolution</param>
    /// <param name="config">Optional configuration</param>
    private static (int mergedCells, int skippedCells) MergeSubmapIntoOccupancyGridFromTSDF(
        TSDF2D tsdfGrid,
        Rigid3d localToMapTransform,
        OccupancyGrid occupancyGrid,
        double targetResolution,
        OccupancyGridConfiguration? config = null)
    {
        var limits = tsdfGrid.Limits;
        int mergedCells = 0;
        int skippedCells = 0;

        // Get TSDF-specific thresholds from config
        var freeThreshold = config?.TsdfFreeThreshold ?? 0.05;
        var occupiedThreshold = config?.TsdfOccupiedThreshold ?? -0.02;
        var minWeight = config?.TsdfMinWeight ?? 0.1;

        // Use ComputeCroppedLimits to get only known cells bounds
        tsdfGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);

        if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
            return (0, 0);

        // Iterate through known cells region only
        for (int y = 0; y < croppedLimits.NumYCells; y++)
        {
            for (int x = 0; x < croppedLimits.NumXCells; x++)
            {
                var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);

                // Skip unknown cells
                if (!tsdfGrid.IsKnown(cellIndex))
                {
                    skippedCells++;
                    continue;
                }

                // Get cell center in LOCAL MAP FRAME (NOT submap frame!)
                var cellCenter = limits.GetCellCenter(cellIndex);
                var cellCenterInLocalFrame = new Vector3(cellCenter.X, cellCenter.Y, 0.0);

                // Transform from LOCAL MAP FRAME to MAP FRAME
                var mapPoint = localToMapTransform.TransformPoint(cellCenterInLocalFrame);

                // Convert to occupancy grid coordinates
                var gridX = (int)Math.Floor((mapPoint.X - occupancyGrid.Origin.Position.X) / targetResolution);
                var gridY = (int)Math.Floor((mapPoint.Y - occupancyGrid.Origin.Position.Y) / targetResolution);

                if (gridX >= 0 && gridX < occupancyGrid.Width && gridY >= 0 && gridY < occupancyGrid.Height)
                {
                    // Get TSD and weight from TSDF grid
                    var (tsd, weight) = tsdfGrid.GetTSDAndWeight(cellIndex);

                    // Skip cells with very low weight (not enough observations)
                    if (weight < minWeight)
                    {
                        skippedCells++;
                        continue;
                    }

                    // Convert TSD to occupancy value
                    // tsd > 0: Free space → occupancy = 0
                    // tsd < 0: Occupied → occupancy = 100
                    // tsd ≈ 0: Surface/ambiguous
                    sbyte occupancyValue;

                    if (tsd > freeThreshold)
                    {
                        // Free space (high confidence)
                        occupancyValue = 0;
                    }
                    else if (tsd < occupiedThreshold)
                    {
                        // Occupied space (high confidence)
                        occupancyValue = 100;
                    }
                    else
                    {
                        // Ambiguous (near surface) - skip
                        skippedCells++;
                        continue;
                    }

                    var currentValue = occupancyGrid.GetCell(gridX, gridY);

                    // Merge logic
                    bool shouldMerge = false;

                    if (currentValue == -1)
                    {
                        if (occupancyValue != -1)
                        {
                            shouldMerge = true;
                        }
                    }
                    else
                    {
                        if (occupancyValue != -1 && occupancyValue > currentValue)
                        {
                            shouldMerge = true;
                        }
                    }

                    if (shouldMerge)
                    {
                        occupancyGrid.Data[gridY * occupancyGrid.Width + gridX] = occupancyValue;
                        mergedCells++;
                    }
                    else
                    {
                        skippedCells++;
                    }
                }
                else
                {
                    skippedCells++;
                }
            }
        }

        return (mergedCells, skippedCells);
    }

    #endregion

    #region Helpers

    /// <summary>
    /// Extract yaw angle in degrees from quaternion (for debugging)
    /// </summary>
    private static double QuaternionToYawDegrees(RobotNet10.Shared.Geometry.Quaternion q)
    {
        // Yaw (Z-axis rotation) = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))
        var siny_cosp = 2.0 * (q.W * q.Z + q.X * q.Y);
        var cosy_cosp = 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
        var yawRad = Math.Atan2(siny_cosp, cosy_cosp);
        return yawRad * 180.0 / Math.PI;
    }

    #endregion

    #region Texture-Based Generation

    /// <summary>
    /// Generate occupancy grid from MapBuilder using texture-based approach.
    /// This method uses DrawToSubmapTexture (from ProbabilityGrid/TSDF2D) and SubmapPainter
    /// to generate the occupancy grid, matching the C++ xloc.cc implementation exactly.
    /// </summary>
    /// <param name="mapBuilder">MapBuilder containing submaps</param>
    /// <param name="resolution">Target grid resolution</param>
    /// <param name="padding">Padding around map bounds</param>
    /// <param name="logger">Optional logger</param>
    /// <param name="config">Optional occupancy grid configuration for threshold and post-processing</param>
    /// <returns>OccupancyGrid with ROS convention (row 0 = world bottom), or null if generation fails</returns>
    public static OccupancyGrid? GenerateFromMapBuilderUsingTextures(
        IMapBuilder mapBuilder,
        double resolution,
        double padding,
        ILogger? logger = null,
        OccupancyGridConfiguration? config = null)
    {
        try
        {
            var poseGraph = mapBuilder.PoseGraph;
            var allSubmapData = poseGraph.GetAllSubmapData();

            // Get TransformToMap and compute its inverse for applying to all poses
            // This matches C++ xloc.cc behavior where poses are transformed using:
            // transformedPose = GetTransformToMap().inverse() * pose
            var transformToMap = poseGraph.GetTransformToMap();
            var transformToMapInverse = transformToMap.Inverse();

            // Collect submap slices
            var submapSlices = new Dictionary<SubmapId, SubmapSlice>();
            int submapIndexDebug = 0;

            foreach (var idDataRef in allSubmapData)
            {
                var submapData = idDataRef.Data;
                if (submapData.Submap is Submap2D submap2D)
                {
                    var submapId = idDataRef.Id;
                    var trajectoryId = submapId.TrajectoryId;

                    // Apply TransformToMap.Inverse() to transform from internal coordinate to map frame
                    // This matches C++ xloc.cc: GetTransformToMap().inverse() * pose
                    var localPose = submap2D.LocalPose;
                    Rigid3d globalPose = transformToMapInverse * submapData.Pose;

                    // Validate pose
                    if (!globalPose.IsValid())
                    {
                        logger?.LogWarning(
                            "OccupancyGridGenerator: Submap {TrajectoryId}:{SubmapIndex} has invalid pose, attempting fallback",
                            submapId.TrajectoryId, submapId.SubmapIndex);

                        var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
                        var globalPoseComputed = transformToMapInverse * (localToGlobal * localPose);

                        if (globalPoseComputed.IsValid())
                        {
                            globalPose = globalPoseComputed;
                        }
                        else
                        {
                            logger?.LogError(
                                "OccupancyGridGenerator: Both poses invalid for submap {TrajectoryId}:{SubmapIndex}, skipping",
                                submapId.TrajectoryId, submapId.SubmapIndex);
                            continue;
                        }
                    }

                    // Create submap slice using DrawToSubmapTexture
                    try
                    {
                        var slice = SubmapPainter.CreateSubmapSlice(submap2D, globalPose);
                        if (slice.PixelData != null && slice.Width > 0 && slice.Height > 0)
                        {
                            submapSlices[submapId] = slice;
                        }
                    }
                    catch (Exception ex)
                    {
                        logger?.LogWarning(ex,
                            "OccupancyGridGenerator: Failed to create slice for submap {TrajectoryId}:{SubmapIndex}",
                            submapId.TrajectoryId, submapId.SubmapIndex);
                    }

                    submapIndexDebug++;
                }
            }

            if (submapSlices.Count == 0)
            {
                logger?.LogWarning("OccupancyGridGenerator: No valid submap slices created");
                return null;
            }

            // Paint all submap slices into combined image
            var paintResult = SubmapPainter.PaintSubmapSlices(submapSlices, resolution);
            if (paintResult == null)
            {
                logger?.LogWarning("OccupancyGridGenerator: PaintSubmapSlices returned null");
                return null;
            }

            // Add padding to the result
            var paddingPixels = (int)Math.Ceiling(padding / resolution);
            var paddedWidth = paintResult.Width + 2 * paddingPixels;
            var paddedHeight = paintResult.Height + 2 * paddingPixels;

            // Create occupancy grid with padding
            // paintResult.Origin.Y is the world Y at the TOP of the paint image
            // OccupancyGrid origin must be at the BOTTOM-LEFT (ROS convention)
            // Match C++: origin.y = (-height + originY_device) * resolution
            // The bottom of the image in world Y = paintResult.Origin.Y - height * resolution
            // Then subtract extra padding below
            var origin = new Pose
            {
                Position = new Vector3(
                    paintResult.Origin.X - padding,
                    paintResult.Origin.Y - paintResult.Height * resolution - padding,
                    0.0),
                Orientation = new Quaternion(0, 0, 0, 1)
            };

            var occupancyGrid = new OccupancyGrid(resolution, paddedWidth, paddedHeight, origin);

            // Convert paint result to occupancy values with optional config
            var occupancyValues = ConvertToOccupancyValuesWithConfig(paintResult, config);

            // Copy occupancy values with padding offset
            // Note: PaintSubmapSlices uses image coordinates (Y increases downward)
            // OccupancyGrid uses ROS convention (row 0 = world bottom, Y increases upward)
            // So we need to flip Y when copying
            for (int py = 0; py < paintResult.Height; py++)
            {
                for (int px = 0; px < paintResult.Width; px++)
                {
                    var srcIndex = py * paintResult.Width + px;
                    var occupancyValue = occupancyValues[srcIndex];

                    // Flip Y for ROS convention
                    var gridX = px + paddingPixels;
                    var gridY = (paintResult.Height - 1 - py) + paddingPixels;

                    if (gridX >= 0 && gridX < paddedWidth && gridY >= 0 && gridY < paddedHeight)
                    {
                        var dstIndex = gridY * paddedWidth + gridX;
                        occupancyGrid.Data[dstIndex] = occupancyValue;
                    }
                }
            }

            // Apply post-processing if configured
            if (config != null)
            {
                ApplyPostProcessing(occupancyGrid, config, logger);
            }

            logger?.LogDebug(
                "OccupancyGridGenerator: Generated grid {W}x{H} from {Count} submaps using DrawToSubmapTexture",
                paddedWidth, paddedHeight, submapSlices.Count);

            return occupancyGrid;
        }
        catch (Exception ex)
        {
            logger?.LogError(ex, "OccupancyGridGenerator: Failed to generate occupancy grid using textures");
            return null;
        }
    }

    #endregion

    #region Config-Based Conversion

    /// <summary>
    /// Convert paint result to occupancy values using configuration thresholds.
    /// </summary>
    private static sbyte[] ConvertToOccupancyValuesWithConfig(
        PaintSubmapSlicesResult paintResult,
        OccupancyGridConfiguration? config)
    {
        // Use default conversion if no config provided
        if (config == null)
        {
            return SubmapPainter.ConvertToOccupancyValues(paintResult);
        }

        var occupancyValues = new sbyte[paintResult.Width * paintResult.Height];

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

            if (observed == 0)
            {
                // Unknown cell - not observed
                occupancyValues[i] = -1;
                continue;
            }

            // Calculate texture-like values for threshold comparison
            // logOddsInteger approximation from color: color=0 → high occupied, color=255 → high free
            // delta = 128 - logOddsInteger
            // If color is high (white/free), delta is positive → textureValue = delta, textureAlpha = 0
            // If color is low (black/occupied), delta is negative → textureValue = 0, textureAlpha = -delta

            // Approximate: textureValue ≈ color (for free space)
            // textureAlpha ≈ 255 - color (for occupied space)
            int textureValue = color;
            int textureAlpha = alpha > 0 ? alpha : (color < 128 ? 128 - color : 0);

            sbyte occupancyValue;

            if (config.UseBinaryOutput)
            {
                // Binary output mode with configurable thresholds
                if (textureAlpha > config.OccupiedSpaceThreshold)
                {
                    // Occupied space
                    occupancyValue = 100;
                }
                else if (textureValue >= config.FreeSpaceThreshold)
                {
                    // Free space
                    occupancyValue = 0;
                }
                else
                {
                    // Ambiguous cell - check probability range
                    // Approximate probability from color: p ≈ 1 - color/255
                    double approxProbability = 1.0 - color / 255.0;

                    if (approxProbability >= config.AmbiguousRangeLower &&
                        approxProbability <= config.AmbiguousRangeUpper)
                    {
                        // In ambiguous range - use configured value
                        occupancyValue = config.AmbiguousCellValue;
                    }
                    else if (approxProbability < config.AmbiguousRangeLower)
                    {
                        // Below ambiguous range = more likely free
                        occupancyValue = 0;
                    }
                    else
                    {
                        // Above ambiguous range = more likely occupied
                        occupancyValue = 100;
                    }
                }
            }
            else
            {
                // Gradient output mode
                // Match C++ formula: occupancy = round((1 - color/255) * 100)
                var occupancy = (int)Math.Round((1.0 - color / 255.0) * 100.0);
                occupancyValue = (sbyte)Math.Clamp(occupancy, 0, 100);
            }

            occupancyValues[i] = occupancyValue;
        }

        return occupancyValues;
    }

    #endregion

    #region Post-Processing

    /// <summary>
    /// Apply post-processing operations based on configuration.
    /// </summary>
    private static void ApplyPostProcessing(
        OccupancyGrid grid,
        OccupancyGridConfiguration config,
        ILogger? logger)
    {
        // Apply median filter first (noise reduction)
        if (config.EnableMedianFilter)
        {
            ApplyMedianFilter(grid, config.MedianFilterKernelSize);
            logger?.LogDebug("OccupancyGridGenerator: Applied median filter with kernel size {Size}",
                config.MedianFilterKernelSize);
        }

        // Apply wall thinning (erosion)
        if (config.EnableWallThinning && config.WallThinningIterations > 0)
        {
            for (int i = 0; i < config.WallThinningIterations; i++)
            {
                ApplyWallThinning(grid, config.MinWallThicknessPixels);
            }
            logger?.LogDebug("OccupancyGridGenerator: Applied wall thinning with {Iterations} iterations",
                config.WallThinningIterations);
        }
    }

    /// <summary>
    /// Apply median filter to reduce noise.
    /// </summary>
    private static void ApplyMedianFilter(OccupancyGrid grid, int kernelSize)
    {
        if (kernelSize < 3 || kernelSize % 2 == 0)
            kernelSize = 3; // Ensure odd kernel size

        var halfKernel = kernelSize / 2;
        var newData = new sbyte[grid.Data.Length];
        Array.Copy(grid.Data, newData, grid.Data.Length);

        var neighbors = new List<sbyte>(kernelSize * kernelSize);

        for (int y = halfKernel; y < grid.Height - halfKernel; y++)
        {
            for (int x = halfKernel; x < grid.Width - halfKernel; x++)
            {
                neighbors.Clear();

                // Collect neighbors
                for (int ky = -halfKernel; ky <= halfKernel; ky++)
                {
                    for (int kx = -halfKernel; kx <= halfKernel; kx++)
                    {
                        var idx = (y + ky) * grid.Width + (x + kx);
                        var value = grid.Data[idx];
                        if (value != -1) // Only consider known cells
                        {
                            neighbors.Add(value);
                        }
                    }
                }

                // Apply median if enough neighbors
                if (neighbors.Count >= kernelSize)
                {
                    neighbors.Sort();
                    var median = neighbors[neighbors.Count / 2];
                    newData[y * grid.Width + x] = median;
                }
            }
        }

        Array.Copy(newData, grid.Data, grid.Data.Length);
    }

    /// <summary>
    /// Apply morphological erosion to thin walls.
    /// Only erodes occupied cells (value = 100) that have free neighbors.
    /// </summary>
    private static void ApplyWallThinning(OccupancyGrid grid, int minThickness)
    {
        var newData = new sbyte[grid.Data.Length];
        Array.Copy(grid.Data, newData, grid.Data.Length);

        // 4-connectivity erosion kernel (up, down, left, right)
        int[] dx = { 0, 0, -1, 1 };
        int[] dy = { -1, 1, 0, 0 };

        for (int y = 1; y < grid.Height - 1; y++)
        {
            for (int x = 1; x < grid.Width - 1; x++)
            {
                var idx = y * grid.Width + x;
                var value = grid.Data[idx];

                // Only process occupied cells
                if (value != 100)
                    continue;

                // Check if this cell should be eroded
                // Count free neighbors
                int freeNeighbors = 0;
                int occupiedNeighbors = 0;

                for (int d = 0; d < 4; d++)
                {
                    var nx = x + dx[d];
                    var ny = y + dy[d];
                    var neighborIdx = ny * grid.Width + nx;
                    var neighborValue = grid.Data[neighborIdx];

                    if (neighborValue == 0)
                        freeNeighbors++;
                    else if (neighborValue == 100)
                        occupiedNeighbors++;
                }

                // Erode if:
                // 1. Has at least one free neighbor (is on wall boundary)
                // 2. Has enough occupied neighbors to maintain minimum thickness
                if (freeNeighbors > 0 && occupiedNeighbors >= minThickness)
                {
                    // Check perpendicular thickness to ensure we don't break thin walls
                    bool canErode = true;

                    // Check horizontal thickness
                    if (freeNeighbors == 1 || freeNeighbors == 2)
                    {
                        int hThickness = 1;
                        for (int tx = x - 1; tx >= 0 && grid.Data[y * grid.Width + tx] == 100; tx--)
                            hThickness++;
                        for (int tx = x + 1; tx < grid.Width && grid.Data[y * grid.Width + tx] == 100; tx++)
                            hThickness++;

                        int vThickness = 1;
                        for (int ty = y - 1; ty >= 0 && grid.Data[ty * grid.Width + x] == 100; ty--)
                            vThickness++;
                        for (int ty = y + 1; ty < grid.Height && grid.Data[ty * grid.Width + x] == 100; ty++)
                            vThickness++;

                        // Don't erode if it would make wall too thin
                        if (Math.Min(hThickness, vThickness) <= minThickness)
                            canErode = false;
                    }

                    if (canErode)
                    {
                        newData[idx] = 0; // Erode to free space
                    }
                }
            }
        }

        Array.Copy(newData, grid.Data, grid.Data.Length);
    }

    #endregion

    #region Log-Odds Based Generation

    /// <summary>
    /// Generate occupancy grid from MapBuilder using log-odds summation (Bayesian approach).
    /// This method directly reads probability values from each submap's ProbabilityGrid,
    /// sums log-odds for overlapping cells, and converts back to occupancy values.
    /// Provides clearer free/occupied distinction compared to texture-based blending.
    /// </summary>
    /// <param name="mapBuilder">MapBuilder containing submaps</param>
    /// <param name="resolution">Target grid resolution</param>
    /// <param name="padding">Padding around map bounds</param>
    /// <param name="logger">Optional logger</param>
    /// <param name="config">Configuration for thresholds and log-odds clamp</param>
    /// <returns>OccupancyGrid with ROS convention (row 0 = world bottom), or null if generation fails</returns>
    public static OccupancyGrid? GenerateFromMapBuilderUsingLogOdds(
        IMapBuilder mapBuilder,
        double resolution,
        double padding,
        ILogger? logger = null,
        OccupancyGridConfiguration? config = null,
        MapById<SubmapId, IPoseGraph.SubmapData>? prefetchedSubmapData = null,
        Rigid3d? prefetchedTransformToMap = null)
    {
        try
        {
            var poseGraph = mapBuilder.PoseGraph;
            var allSubmapData = prefetchedSubmapData ?? poseGraph.GetAllSubmapData();

            // Get TransformToMap inverse for coordinate transformation
            // NOTE: During ScanMapping, TransformToMap should be Identity.
            // The laser scan and robot pose from GlobalTrajectoryBuilder use localToGlobal only
            // (optimization frame), so TransformToMap.Inverse() must equal Identity for alignment.
            // If non-identity, the occupancy grid will be in a different frame from pose/laser.
            var transformToMap = prefetchedTransformToMap ?? poseGraph.GetTransformToMap();
            var transformToMapInverse = transformToMap.Inverse();

            // Warn if TransformToMap is non-identity (could cause frame mismatch with pose/laser)
            if (Math.Abs(transformToMap.Translation.X) > 0.001 ||
                Math.Abs(transformToMap.Translation.Y) > 0.001 ||
                Math.Abs(transformToMap.Translation.Z) > 0.001 ||
                Math.Abs(transformToMap.Rotation.X) > 0.001 ||
                Math.Abs(transformToMap.Rotation.Y) > 0.001 ||
                Math.Abs(transformToMap.Rotation.Z) > 0.001)
            {
                logger?.LogWarning(
                    "OccupancyGridGenerator: TransformToMap is NON-IDENTITY! " +
                    "Grid frame may not match pose/laser frame. " +
                    "T=[{TX:F4},{TY:F4},{TZ:F4}], R=[{RW:F4},{RX:F4},{RY:F4},{RZ:F4}]",
                    transformToMap.Translation.X, transformToMap.Translation.Y, transformToMap.Translation.Z,
                    transformToMap.Rotation.W, transformToMap.Rotation.X, transformToMap.Rotation.Y, transformToMap.Rotation.Z);
            }

            // Collect all 2D submaps with their local-to-map transforms.
            // IMPORTANT: We use LocalToGlobalTransform (NOT SubmapData.Pose) because:
            // - GetCellCenter() returns coordinates in LOCAL MAP FRAME (trajectory local frame)
            // - NOT in submap frame as previously assumed
            // - localToMapTransform = TransformToMapInverse * LocalToGlobalTransform
            // For active (non-finished) submaps, snapshot the grid cell data to avoid
            // race conditions with concurrent InsertRangeData on the sensor thread.
            // Finished submaps are immutable and can be read directly.
            var submap2DList = new List<(Submap2D Submap, Rigid3d LocalToMapTransform, Grid2D.GridCellSnapshot? Snapshot)>();

            foreach (var idDataRef in allSubmapData)
            {
                var submapData = idDataRef.Data;
                if (submapData.Submap is Submap2D submap2D)
                {
                    var submapId = idDataRef.Id;
                    var trajectoryId = submapId.TrajectoryId;

                    // FIX: Use LocalToGlobalTransform instead of SubmapData.Pose
                    // Cell coordinates from GetCellCenter() are in LOCAL MAP FRAME (L),
                    // so we need T_map_local = TransformToMapInverse * LocalToGlobalTransform
                    var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
                    Rigid3d localToMapTransform = transformToMapInverse * localToGlobal;

                    // Validate transform
                    if (!localToMapTransform.IsValid())
                    {
                        logger?.LogWarning(
                            "OccupancyGridGenerator: Skipping submap {TrajectoryId}:{SubmapIndex} with invalid localToMapTransform",
                            submapId.TrajectoryId, submapId.SubmapIndex);
                        continue;
                    }

                    // Snapshot active submaps to decouple from sensor thread writes
                    Grid2D.GridCellSnapshot? snapshot = null;
                    if (!submap2D.InsertionFinished && submap2D.Grid != null)
                    {
                        snapshot = submap2D.Grid.SnapshotCellData();
                    }

                    submap2DList.Add((submap2D, localToMapTransform, snapshot));
                }
            }

            if (submap2DList.Count == 0)
            {
                logger?.LogWarning("OccupancyGridGenerator: No 2D submaps found for log-odds generation");
                return null;
            }

            // Log per-submap details for debugging
            for (int si = 0; si < submap2DList.Count; si++)
            {
                var (sm, localToMap, snap) = submap2DList[si];
                var smGrid = sm.Grid;
                var gridType = smGrid?.GetType().Name ?? "null";
                var numCellsX = smGrid?.Limits.CellLimits.NumXCells ?? 0;
                var numCellsY = smGrid?.Limits.CellLimits.NumYCells ?? 0;

                // Get local pose and grid max point for debugging
                var localPose = sm.LocalPose;
                var gridMaxX = smGrid?.Limits.Max.X ?? 0;
                var gridMaxY = smGrid?.Limits.Max.Y ?? 0;
            }

            // Calculate bounds from all submaps
            var (minX, minY, maxX, maxY) = CalculateBoundsFromSubmaps(submap2DList, logger);

            // Validate bounds
            if (minX >= maxX || minY >= maxY || double.IsInfinity(minX) || double.IsInfinity(maxX) ||
                double.IsInfinity(minY) || double.IsInfinity(maxY))
            {
                logger?.LogWarning(
                    "OccupancyGridGenerator: Invalid bounds for log-odds generation: " +
                    "min=[{MinX:F3},{MinY:F3}], max=[{MaxX:F3},{MaxY:F3}]",
                    minX, minY, maxX, maxY);
                return null;
            }

            // Add padding
            var adjustedMinX = minX - padding;
            var adjustedMinY = minY - padding;
            var adjustedMaxX = maxX + padding;
            var adjustedMaxY = maxY + padding;

            var width = (int)Math.Ceiling((adjustedMaxX - adjustedMinX) / resolution);
            var height = (int)Math.Ceiling((adjustedMaxY - adjustedMinY) / resolution);

            // Set origin to bottom-left corner (ROS convention)
            var gridOriginPosition = new Vector3(adjustedMinX, adjustedMinY, 0.0);
            var origin = new Pose
            {
                Position = gridOriginPosition,
                Orientation = new Quaternion(0, 0, 0, 1)
            };

            // Create accumulation buffers
            // logOddsSum: sum of log-odds for each cell
            // observationCount: number of observations per cell
            var logOddsSum = new double[width * height];
            var observationCount = new int[width * height];

            // Initialize to zero (prior = 0.5, logOdds = 0)
            Array.Fill(logOddsSum, 0.0);
            Array.Fill(observationCount, 0);

            // Get log-odds clamp value
            var logOddsClamp = config?.LogOddsClamp ?? 10.0;

            // Merge all submaps using log-odds summation.
            // Active submaps use their pre-captured snapshot to avoid racing with
            // concurrent InsertRangeData; finished submaps read the grid directly.
            foreach (var (submap2D, localToMapTransform, snapshot) in submap2DList)
            {
                if (snapshot != null)
                {
                    // Active submap → use thread-safe snapshot
                    MergeSnapshotUsingLogOdds(
                        snapshot, localToMapTransform,
                        logOddsSum, observationCount,
                        width, height,
                        adjustedMinX, adjustedMinY,
                        resolution, logOddsClamp);
                }
                else
                {
                    // Finished submap → grid is immutable, safe to read directly
                    MergeSubmapUsingLogOdds(
                        submap2D, localToMapTransform,
                        logOddsSum, observationCount,
                        width, height,
                        adjustedMinX, adjustedMinY,
                        resolution, logOddsClamp, config);
                }
            }

            // Debug: count total observations
            int totalObservations = 0;
            int observedCells = 0;
            double minLogOdds = double.MaxValue;
            double maxLogOdds = double.MinValue;
            for (int i = 0; i < observationCount.Length; i++)
            {
                if (observationCount[i] > 0)
                {
                    observedCells++;
                    totalObservations += observationCount[i];
                    minLogOdds = Math.Min(minLogOdds, logOddsSum[i]);
                    maxLogOdds = Math.Max(maxLogOdds, logOddsSum[i]);
                }
            }

            // Convert log-odds to occupancy values
            var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);
            var stats = ConvertLogOddsToOccupancy(
                logOddsSum, observationCount,
                occupancyGrid, config, logger);

            // Apply post-processing if configured
            if (config != null)
            {
                ApplyPostProcessing(occupancyGrid, config, logger);
            }

            return occupancyGrid;
        }
        catch (Exception ex)
        {
            logger?.LogError(ex, "OccupancyGridGenerator: Failed to generate occupancy grid using log-odds");
            return null;
        }
    }

    /// <summary>
    /// Merge a single submap into log-odds accumulation buffers.
    /// Supports both ProbabilityGrid and TSDF2D grid types.
    /// </summary>
    /// <param name="submap2D">The submap to merge</param>
    /// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
    private static void MergeSubmapUsingLogOdds(
        Submap2D submap2D,
        Rigid3d localToMapTransform,
        double[] logOddsSum,
        int[] observationCount,
        int gridWidth,
        int gridHeight,
        double originX,
        double originY,
        double resolution,
        double logOddsClamp,
        OccupancyGridConfiguration? config = null)
    {
        var grid = submap2D.Grid;
        if (grid == null)
            return;

        // Dispatch to appropriate handler based on grid type
        if (grid is ProbabilityGrid probabilityGrid)
        {
            MergeSubmapUsingLogOddsFromProbabilityGrid(
                probabilityGrid, localToMapTransform, logOddsSum, observationCount,
                gridWidth, gridHeight, originX, originY, resolution, logOddsClamp);
        }
        else if (grid is TSDF2D tsdfGrid)
        {
            MergeSubmapUsingLogOddsFromTSDF(
                tsdfGrid, localToMapTransform, logOddsSum, observationCount,
                gridWidth, gridHeight, originX, originY, resolution, logOddsClamp, config);
        }
    }

    /// <summary>
    /// Merge ProbabilityGrid submap into log-odds accumulation buffers.
    /// </summary>
    /// <param name="probabilityGrid">The probability grid to merge</param>
    /// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
    private static void MergeSubmapUsingLogOddsFromProbabilityGrid(
        ProbabilityGrid probabilityGrid,
        Rigid3d localToMapTransform,
        double[] logOddsSum,
        int[] observationCount,
        int gridWidth,
        int gridHeight,
        double originX,
        double originY,
        double resolution,
        double logOddsClamp)
    {
        var limits = probabilityGrid.Limits;

        // Use ComputeCroppedLimits to get only known cells bounds
        probabilityGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);

        if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
            return;

        // Iterate through known cells region only
        for (int y = 0; y < croppedLimits.NumYCells; y++)
        {
            for (int x = 0; x < croppedLimits.NumXCells; x++)
            {
                var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);

                // Skip unknown cells
                if (!probabilityGrid.IsKnown(cellIndex))
                    continue;

                // Get cell center in LOCAL MAP FRAME (NOT submap frame!)
                var cellCenter = limits.GetCellCenter(cellIndex);
                var cellCenterInLocalFrame = new Vector3(cellCenter.X, cellCenter.Y, 0.0);

                // Transform from LOCAL MAP FRAME to MAP FRAME
                var mapPoint = localToMapTransform.TransformPoint(cellCenterInLocalFrame);

                // Convert to grid coordinates (ROS convention)
                var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
                var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);

                if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
                {
                    // Get probability value from submap
                    var probability = probabilityGrid.GetProbability(cellIndex);

                    // Clamp probability to avoid log(0) or log(infinity)
                    probability = Math.Clamp(probability, 0.001, 0.999);

                    // Convert probability to log-odds: log(p / (1 - p))
                    var logOdds = Math.Log(probability / (1.0 - probability));

                    // Clamp log-odds to prevent extreme values
                    logOdds = Math.Clamp(logOdds, -logOddsClamp, logOddsClamp);

                    // Accumulate
                    var idx = gridY * gridWidth + gridX;
                    logOddsSum[idx] += logOdds;
                    observationCount[idx]++;
                }
            }
        }
    }

    /// <summary>
    /// Merge a GridCellSnapshot (from an active submap) into log-odds accumulation buffers.
    /// Works identically to MergeSubmapUsingLogOddsFromProbabilityGrid but reads from the
    /// snapshot arrays rather than the live grid, eliminating race conditions.
    /// </summary>
    /// <param name="snapshot">The grid cell snapshot to merge</param>
    /// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
    private static void MergeSnapshotUsingLogOdds(
        Grid2D.GridCellSnapshot snapshot,
        Rigid3d localToMapTransform,
        double[] logOddsSum,
        int[] observationCount,
        int gridWidth,
        int gridHeight,
        double originX,
        double originY,
        double resolution,
        double logOddsClamp)
    {
        var limits = snapshot.Limits;
        snapshot.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);

        if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
            return;

        var cells = snapshot.Cells;
        var numXCells = limits.CellLimits.NumXCells;

        for (int y = 0; y < croppedLimits.NumYCells; y++)
        {
            for (int x = 0; x < croppedLimits.NumXCells; x++)
            {
                var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);

                if (!snapshot.IsKnown(cellIndex))
                    continue;

                // Read cell value from snapshot and convert to probability
                // (matches ProbabilityGrid.GetProbability: ValueToCorrespondenceCost → CorrespondenceCostToProbability)
                var flatIndex = numXCells * cellIndex.Y + cellIndex.X;
                if (flatIndex < 0 || flatIndex >= cells.Length)
                    continue;

                var value = cells[flatIndex];
                const ushort kUpdateMarker = (ushort)(1u << 15);
                if (value >= kUpdateMarker)
                    value -= kUpdateMarker;
                if (value == 0) continue; // unknown

                var probability = ProbabilityValues.CorrespondenceCostToProbability(
                    ProbabilityValues.ValueToCorrespondenceCost(value));

                // Get cell center in LOCAL MAP FRAME (NOT submap frame!)
                var cellCenter = limits.GetCellCenter(cellIndex);
                var mapPoint = localToMapTransform.TransformPoint(new Vector3(cellCenter.X, cellCenter.Y, 0.0));

                var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
                var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);

                if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
                {
                    probability = Math.Clamp(probability, 0.001, 0.999);
                    var logOdds = Math.Log(probability / (1.0 - probability));
                    logOdds = Math.Clamp(logOdds, -logOddsClamp, logOddsClamp);

                    var idx = gridY * gridWidth + gridX;
                    logOddsSum[idx] += logOdds;
                    observationCount[idx]++;
                }
            }
        }
    }

    /// <summary>
    /// Merge TSDF2D submap into log-odds accumulation buffers.
    /// Uses TSDF-specific thresholds for direct classification:
    ///   tsd > TsdfFreeThreshold → FREE (strong negative logOdds)
    ///   tsd &lt; TsdfOccupiedThreshold → OCCUPIED (strong positive logOdds)
    ///   else → near surface, use linear mapping
    /// Weight is used only as a minimum confidence filter.
    /// </summary>
    /// <param name="tsdfGrid">The TSDF grid to merge</param>
    /// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
    private static void MergeSubmapUsingLogOddsFromTSDF(
        TSDF2D tsdfGrid,
        Rigid3d localToMapTransform,
        double[] logOddsSum,
        int[] observationCount,
        int gridWidth,
        int gridHeight,
        double originX,
        double originY,
        double resolution,
        double logOddsClamp,
        OccupancyGridConfiguration? config = null)
    {
        var limits = tsdfGrid.Limits;

        // Get TSDF-specific thresholds from config
        var tsdfMaxTsd = config?.TsdfMaxTsd ?? 0.3;
        var tsdfMinWeight = config?.TsdfMinWeight ?? 0.1;
        var tsdfFreeThreshold = config?.TsdfFreeThreshold ?? 0.05;
        var tsdfOccupiedThreshold = config?.TsdfOccupiedThreshold ?? -0.02;

        // Use ComputeCroppedLimits to get only known cells bounds
        tsdfGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);

        if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
            return;

        // Iterate through known cells region only
        for (int y = 0; y < croppedLimits.NumYCells; y++)
        {
            for (int x = 0; x < croppedLimits.NumXCells; x++)
            {
                var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);

                // Skip unknown cells
                if (!tsdfGrid.IsKnown(cellIndex))
                    continue;

                // Get cell center in LOCAL MAP FRAME (NOT submap frame!)
                var cellCenter = limits.GetCellCenter(cellIndex);
                var cellCenterInLocalFrame = new Vector3(cellCenter.X, cellCenter.Y, 0.0);

                // Transform from LOCAL MAP FRAME to MAP FRAME
                var mapPoint = localToMapTransform.TransformPoint(cellCenterInLocalFrame);

                // Convert to grid coordinates (ROS convention)
                var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
                var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);

                if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
                {
                    // Get TSD and weight from TSDF grid
                    var (tsd, weight) = tsdfGrid.GetTSDAndWeight(cellIndex);

                    // Skip cells with very low weight (not enough observations to be reliable)
                    if (weight < tsdfMinWeight)
                        continue;

                    // Convert TSD directly to probability using thresholds
                    // TSDF convention:
                    //   tsd > 0: Free space (away from obstacles)
                    //   tsd < 0: Occupied space (inside/near obstacles)
                    //   tsd = 0: On obstacle surface
                    double probability;
                    if (tsd > tsdfFreeThreshold)
                    {
                        // Clearly free: map tsd [freeThreshold, maxTsd] → probability [0.2, 0.01]
                        var t = Math.Clamp((tsd - tsdfFreeThreshold) / (tsdfMaxTsd - tsdfFreeThreshold), 0.0, 1.0);
                        probability = 0.2 - t * 0.19; // 0.2 → 0.01
                    }
                    else if (tsd < tsdfOccupiedThreshold)
                    {
                        // Clearly occupied: map tsd [occupiedThreshold, -maxTsd] → probability [0.8, 0.99]
                        var t = Math.Clamp((tsdfOccupiedThreshold - tsd) / (tsdfMaxTsd + tsdfOccupiedThreshold), 0.0, 1.0);
                        probability = 0.8 + t * 0.19; // 0.8 → 0.99
                    }
                    else
                    {
                        // Near surface: steep linear mapping between thresholds
                        // tsd = occupiedThreshold → probability = 0.95 (deep into occupied territory)
                        // tsd = freeThreshold → probability = 0.05 (deep into free territory)
                        // This steep mapping minimizes cells in ambiguous range [0.35, 0.65]
                        var range = tsdfFreeThreshold - tsdfOccupiedThreshold;
                        if (range > 0)
                        {
                            var t = (tsd - tsdfOccupiedThreshold) / range; // 0 at occupied, 1 at free
                            probability = 0.95 - t * 0.9; // 0.95 → 0.05
                        }
                        else
                        {
                            probability = 0.5;
                        }
                    }

                    // Clamp probability to valid range
                    probability = Math.Clamp(probability, 0.001, 0.999);

                    // Convert probability to log-odds
                    var logOdds = Math.Log(probability / (1.0 - probability));

                    // Clamp log-odds
                    logOdds = Math.Clamp(logOdds, -logOddsClamp, logOddsClamp);

                    // Accumulate
                    var idx = gridY * gridWidth + gridX;
                    logOddsSum[idx] += logOdds;
                    observationCount[idx]++;
                }
            }
        }
    }

    /// <summary>
    /// Convert accumulated log-odds to occupancy values.
    /// Returns statistics about the conversion.
    /// </summary>
    private static (int freeCells, int occupiedCells, int unknownCells) ConvertLogOddsToOccupancy(
        double[] logOddsSum,
        int[] observationCount,
        OccupancyGrid occupancyGrid,
        OccupancyGridConfiguration? config,
        ILogger? logger = null)
    {
        var useBinary = config?.UseBinaryOutput ?? true;
        var ambiguousValue = config?.AmbiguousCellValue ?? (sbyte)-1;
        var ambiguousLower = config?.AmbiguousRangeLower ?? 0.35;
        var ambiguousUpper = config?.AmbiguousRangeUpper ?? 0.65;
        var useAverage = config?.UseLogOddsAverage ?? true;

        int freeCells = 0, occupiedCells = 0, unknownCells = 0;
        double minProb = 1.0, maxProb = 0.0;

        for (int i = 0; i < logOddsSum.Length; i++)
        {
            if (observationCount[i] == 0)
            {
                // No observations - unknown
                occupancyGrid.Data[i] = -1;
                unknownCells++;
                continue;
            }

            // Convert log-odds to probability
            // If useAverage=true, divide by observation count to get average log-odds
            // This prevents amplification when a cell is observed by many submaps
            var logOdds = useAverage ? logOddsSum[i] / observationCount[i] : logOddsSum[i];
            // P = 1 / (1 + exp(-logOdds))
            var probability = 1.0 / (1.0 + Math.Exp(-logOdds));

            minProb = Math.Min(minProb, probability);
            maxProb = Math.Max(maxProb, probability);

            // Convert probability to occupancy value [0, 100]
            // probability = 0 → occupancy = 0 (free)
            // probability = 1 → occupancy = 100 (occupied)
            var occupancy = (int)Math.Round(probability * 100.0);

            if (useBinary)
            {
                // Binary output mode using ambiguous range thresholds directly
                // probability < ambiguousLower → FREE (high confidence free)
                // probability > ambiguousUpper → OCCUPIED (high confidence occupied)
                // else → AMBIGUOUS
                if (probability < ambiguousLower)
                {
                    occupancyGrid.Data[i] = 0;
                    freeCells++;
                }
                else if (probability > ambiguousUpper)
                {
                    occupancyGrid.Data[i] = 100;
                    occupiedCells++;
                }
                else
                {
                    // Ambiguous range
                    occupancyGrid.Data[i] = ambiguousValue;
                    unknownCells++;
                }
            }
            else
            {
                // Gradient output mode
                occupancyGrid.Data[i] = (sbyte)Math.Clamp(occupancy, 0, 100);
                if (occupancy < 35)
                    freeCells++;
                else if (occupancy > 65)
                    occupiedCells++;
                else
                    unknownCells++;
            }
        }

        return (freeCells, occupiedCells, unknownCells);
    }

    #endregion

    #region Max Probability Generation

    /// <summary>
    /// Generate occupancy grid using max probability strategy.
    /// Takes the maximum (most occupied) probability for overlapping cells.
    /// Conservative approach good for navigation safety.
    /// </summary>
    public static OccupancyGrid? GenerateFromMapBuilderUsingMaxProbability(
        IMapBuilder mapBuilder,
        double resolution,
        double padding,
        ILogger? logger = null,
        OccupancyGridConfiguration? config = null)
    {
        try
        {
            var poseGraph = mapBuilder.PoseGraph;
            var allSubmapData = poseGraph.GetAllSubmapData();

            var transformToMap = poseGraph.GetTransformToMap();
            var transformToMapInverse = transformToMap.Inverse();

            // IMPORTANT: We store localToMapTransform (NOT globalPose) because:
            // - GetCellCenter() returns coordinates in LOCAL MAP FRAME (trajectory local frame)
            // - NOT in submap frame as previously assumed
            // - localToMapTransform = TransformToMapInverse * LocalToGlobalTransform
            // - This correctly transforms from local map frame to map frame
            var submap2DList = new List<(Submap2D Submap, Rigid3d LocalToMapTransform)>();

            foreach (var idDataRef in allSubmapData)
            {
                var submapData = idDataRef.Data;
                if (submapData.Submap is Submap2D submap2D)
                {
                    var submapId = idDataRef.Id;
                    var trajectoryId = submapId.TrajectoryId;

                    // FIX: Use LocalToGlobalTransform instead of SubmapData.Pose
                    // Cell coordinates from GetCellCenter() are in LOCAL MAP FRAME (L),
                    // so we need T_map_local = TransformToMapInverse * LocalToGlobalTransform
                    // Previously used: T_map_submap = TransformToMapInverse * SubmapData.Pose (WRONG)
                    var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
                    Rigid3d localToMapTransform = transformToMapInverse * localToGlobal;

                    if (!localToMapTransform.IsValid())
                    {
                        continue;
                    }

                    submap2DList.Add((submap2D, localToMapTransform));
                }
            }

            if (submap2DList.Count == 0)
                return null;

            var (minX, minY, maxX, maxY) = CalculateBoundsFromSubmaps(submap2DList);

            if (minX >= maxX || minY >= maxY || double.IsInfinity(minX) || double.IsInfinity(maxX) ||
                double.IsInfinity(minY) || double.IsInfinity(maxY))
                return null;

            var adjustedMinX = minX - padding;
            var adjustedMinY = minY - padding;
            var adjustedMaxX = maxX + padding;
            var adjustedMaxY = maxY + padding;

            var width = (int)Math.Ceiling((adjustedMaxX - adjustedMinX) / resolution);
            var height = (int)Math.Ceiling((adjustedMaxY - adjustedMinY) / resolution);

            var origin = new Pose
            {
                Position = new Vector3(adjustedMinX, adjustedMinY, 0.0),
                Orientation = new Quaternion(0, 0, 0, 1)
            };

            // Max probability buffer (initialized to -1 meaning no observation)
            var maxProbability = new double[width * height];
            Array.Fill(maxProbability, -1.0);

            foreach (var (submap2D, localToMapTransform) in submap2DList)
            {
                MergeSubmapUsingMaxProbability(
                    submap2D, localToMapTransform,
                    maxProbability,
                    width, height,
                    adjustedMinX, adjustedMinY,
                    resolution);
            }

            var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);
            ConvertMaxProbabilityToOccupancy(maxProbability, occupancyGrid, config);

            if (config != null)
            {
                ApplyPostProcessing(occupancyGrid, config, logger);
            }

            logger?.LogDebug(
                "OccupancyGridGenerator: Generated grid {W}x{H} from {Count} submaps using max probability",
                width, height, submap2DList.Count);

            return occupancyGrid;
        }
        catch (Exception ex)
        {
            logger?.LogError(ex, "OccupancyGridGenerator: Failed to generate occupancy grid using max probability");
            return null;
        }
    }

    /// <summary>
    /// Merge a single submap into max probability buffer.
    /// Supports both ProbabilityGrid and TSDF2D grid types.
    /// IMPORTANT: localToMapTransform transforms from LOCAL MAP FRAME to map frame,
    /// NOT from submap frame. GetCellCenter() returns local map frame coordinates.
    /// </summary>
    private static void MergeSubmapUsingMaxProbability(
        Submap2D submap2D,
        Rigid3d localToMapTransform,
        double[] maxProbability,
        int gridWidth,
        int gridHeight,
        double originX,
        double originY,
        double resolution)
    {
        var grid = submap2D.Grid;
        if (grid == null)
            return;

        // Dispatch to appropriate handler based on grid type
        if (grid is ProbabilityGrid probabilityGrid)
        {
            MergeSubmapUsingMaxProbabilityFromProbabilityGrid(
                probabilityGrid, localToMapTransform, maxProbability,
                gridWidth, gridHeight, originX, originY, resolution);
        }
        else if (grid is TSDF2D tsdfGrid)
        {
            MergeSubmapUsingMaxProbabilityFromTSDF(
                tsdfGrid, localToMapTransform, maxProbability,
                gridWidth, gridHeight, originX, originY, resolution);
        }
    }

    /// <summary>
    /// Merge ProbabilityGrid submap into max probability buffer.
    /// IMPORTANT: localToMapTransform transforms from LOCAL MAP FRAME to map frame.
    /// GetCellCenter() returns coordinates in local map frame (NOT submap frame).
    /// </summary>
    private static void MergeSubmapUsingMaxProbabilityFromProbabilityGrid(
        ProbabilityGrid probabilityGrid,
        Rigid3d localToMapTransform,
        double[] maxProbability,
        int gridWidth,
        int gridHeight,
        double originX,
        double originY,
        double resolution)
    {
        var limits = probabilityGrid.Limits;
        probabilityGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);

        if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
            return;

        for (int y = 0; y < croppedLimits.NumYCells; y++)
        {
            for (int x = 0; x < croppedLimits.NumXCells; x++)
            {
                var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);

                if (!probabilityGrid.IsKnown(cellIndex))
                    continue;

                // cellCenter is in LOCAL MAP FRAME (because limits.Max is set from LocalPose.Translation)
                var cellCenter = limits.GetCellCenter(cellIndex);
                // Transform from local map frame to map frame
                var mapPoint = localToMapTransform.TransformPoint(new Vector3(cellCenter.X, cellCenter.Y, 0.0));

                var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
                var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);

                if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
                {
                    var probability = probabilityGrid.GetProbability(cellIndex);
                    var idx = gridY * gridWidth + gridX;

                    // Take maximum probability (most pessimistic)
                    if (maxProbability[idx] < 0 || probability > maxProbability[idx])
                    {
                        maxProbability[idx] = probability;
                    }
                }
            }
        }
    }

    /// <summary>
    /// Merge TSDF2D submap into max probability buffer.
    /// Converts TSD to probability: tsd > 0 (free) → low probability, tsd < 0 (occupied) → high probability
    /// IMPORTANT: localToMapTransform transforms from LOCAL MAP FRAME to map frame.
    /// GetCellCenter() returns coordinates in local map frame (NOT submap frame).
    /// </summary>
    private static void MergeSubmapUsingMaxProbabilityFromTSDF(
        TSDF2D tsdfGrid,
        Rigid3d localToMapTransform,
        double[] maxProbability,
        int gridWidth,
        int gridHeight,
        double originX,
        double originY,
        double resolution)
    {
        var limits = tsdfGrid.Limits;
        tsdfGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);

        if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
            return;

        for (int y = 0; y < croppedLimits.NumYCells; y++)
        {
            for (int x = 0; x < croppedLimits.NumXCells; x++)
            {
                var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);

                if (!tsdfGrid.IsKnown(cellIndex))
                    continue;

                // cellCenter is in LOCAL MAP FRAME (because limits.Max is set from LocalPose.Translation)
                var cellCenter = limits.GetCellCenter(cellIndex);
                // Transform from local map frame to map frame
                var mapPoint = localToMapTransform.TransformPoint(new Vector3(cellCenter.X, cellCenter.Y, 0.0));

                var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
                var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);

                if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
                {
                    // Get TSD and weight from TSDF grid
                    var (tsd, weight) = tsdfGrid.GetTSDAndWeight(cellIndex);

                    // Skip cells with very low weight
                    if (weight < 0.1)
                        continue;

                    // Convert TSD to probability
                    // tsd > 0: Free space → probability LOW
                    // tsd < 0: Occupied → probability HIGH
                    const double maxTsd = 0.3;  // Typical truncation distance
                    var normalizedTsd = Math.Clamp(tsd / maxTsd, -1.0, 1.0);

                    // Convert to probability: tsd=1 → prob=0, tsd=-1 → prob=1
                    var probability = 0.5 * (1.0 - normalizedTsd);

                    // Scale by weight for confidence
                    var normalizedWeight = Math.Min(weight / 10.0, 1.0);
                    probability = 0.5 + (probability - 0.5) * normalizedWeight;

                    probability = Math.Clamp(probability, 0.0, 1.0);

                    var idx = gridY * gridWidth + gridX;

                    // Take maximum probability (most pessimistic)
                    if (maxProbability[idx] < 0 || probability > maxProbability[idx])
                    {
                        maxProbability[idx] = probability;
                    }
                }
            }
        }
    }

    private static void ConvertMaxProbabilityToOccupancy(
        double[] maxProbability,
        OccupancyGrid occupancyGrid,
        OccupancyGridConfiguration? config)
    {
        var useBinary = config?.UseBinaryOutput ?? true;
        var ambiguousLower = config?.AmbiguousRangeLower ?? 0.35;
        var ambiguousUpper = config?.AmbiguousRangeUpper ?? 0.65;
        var ambiguousValue = config?.AmbiguousCellValue ?? (sbyte)-1;

        for (int i = 0; i < maxProbability.Length; i++)
        {
            if (maxProbability[i] < 0)
            {
                occupancyGrid.Data[i] = -1;
                continue;
            }

            var probability = maxProbability[i];
            var occupancy = (int)Math.Round(probability * 100.0);

            if (useBinary)
            {
                if (probability < ambiguousLower)
                    occupancyGrid.Data[i] = 0;
                else if (probability > ambiguousUpper)
                    occupancyGrid.Data[i] = 100;
                else
                    occupancyGrid.Data[i] = ambiguousValue;
            }
            else
            {
                occupancyGrid.Data[i] = (sbyte)Math.Clamp(occupancy, 0, 100);
            }
        }
    }

    #endregion

    #region Strategy Dispatcher

    /// <summary>
    /// Generate occupancy grid using the configured merge strategy.
    /// This is the main entry point that dispatches to the appropriate implementation.
    /// </summary>
    public static OccupancyGrid? Generate(
        IMapBuilder mapBuilder,
        double resolution,
        double padding,
        ILogger? logger = null,
        OccupancyGridConfiguration? config = null)
        => Generate(mapBuilder, resolution, padding, -1, out _, logger, config);

    /// <summary>
    /// Generate occupancy grid with version-based change detection.
    /// If <paramref name="lastGeneratedVersion"/> >= 0 and no new nodes have been
    /// inserted since that version, returns null immediately (skips regeneration).
    /// <paramref name="snapshotVersion"/> receives the PoseGraph version at the time
    /// of the snapshot so the caller can cache it for the next call.
    /// </summary>
    public static OccupancyGrid? Generate(
        IMapBuilder mapBuilder,
        double resolution,
        double padding,
        int lastGeneratedVersion,
        out int snapshotVersion,
        ILogger? logger = null,
        OccupancyGridConfiguration? config = null)
    {
        snapshotVersion = 0;
        var strategy = config?.MergeStrategy ?? SubmapMergeStrategy.LogOddsSum;

        // Non-blocking snapshot: try to get submap data without blocking AddSensorData.
        // TryGetSubmapSnapshot uses Monitor.TryEnter(0) on PoseGraph._dataLock.
        // If the lock is busy (e.g., during optimization or AddNode), we skip this generation
        // and the next 2-second cycle will retry.
        var poseGraph = mapBuilder.PoseGraph;
        MapById<SubmapId, IPoseGraph.SubmapData>? allSubmapData;
        Rigid3d transformToMap;

        if (poseGraph is CartographerSharp.Mapping.Internal.D2D.PoseGraph2D pg2d)
        {
            if (!pg2d.TryGetSubmapSnapshot(out allSubmapData, out transformToMap, out snapshotVersion))
            {
                logger?.LogDebug("OccupancyGridGenerator.Generate: Skipped - PoseGraph lock busy");
                return null;
            }

            // Skip regeneration if no new nodes have been inserted since the last generation.
            if (lastGeneratedVersion >= 0 && snapshotVersion == lastGeneratedVersion)
            {
                logger?.LogDebug(
                    "OccupancyGridGenerator.Generate: Skipped - no new data (version={Version})",
                    snapshotVersion);
                return null;
            }
        }
        else
        {
            allSubmapData = poseGraph.GetAllSubmapData();
            transformToMap = poseGraph.GetTransformToMap();
        }

        if (allSubmapData == null)
            return null;

        int totalSubmaps = 0;
        int submap2DCount = 0;
        foreach (var idDataRef in allSubmapData)
        {
            totalSubmaps++;
            if (idDataRef.Data.Submap is CartographerSharp.Mapping.D2D.Submap2D)
                submap2DCount++;
        }

        if (submap2DCount == 0)
        {
            logger?.LogWarning("OccupancyGridGenerator.Generate: No Submap2D found, returning null");
            return null;
        }

        // Pass pre-fetched submap data to strategy methods to avoid double GetAllSubmapData() call
        return strategy switch
        {
            SubmapMergeStrategy.LogOddsSum => GenerateFromMapBuilderUsingLogOdds(
                mapBuilder, resolution, padding, logger, config, allSubmapData, transformToMap),

            SubmapMergeStrategy.MaxProbability => GenerateFromMapBuilderUsingMaxProbability(
                mapBuilder, resolution, padding, logger, config),

            SubmapMergeStrategy.PorterDuff => GenerateFromMapBuilderUsingTextures(
                mapBuilder, resolution, padding, logger, config),

            _ => GenerateFromMapBuilderUsingLogOdds(
                mapBuilder, resolution, padding, logger, config, allSubmapData, transformToMap)
        };
    }

    #endregion
}