BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/ProbabilityGridRangeDataInserter2D.cs

/*
 * Copyright 2016 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.Common.Math;
using CartographerSharp.Mapping.Internal.D2D;
using CartographerSharp.Models.Mapping;
using CartographerSharp.Sensor;
using System.Runtime.InteropServices;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping.D2D;

/// <summary>
/// Range data inserter for probability grids in 2D.
/// </summary>
public class ProbabilityGridRangeDataInserter2D : IRangeDataInserter
{
    private const int kSubpixelScale = 1000;
    private const double kPadding = 1e-6;

    private readonly ProbabilityGridRangeDataInserterOptions2D _options;
    private readonly List<ushort> _hitTable;
    private readonly List<ushort> _missTable;

    public ProbabilityGridRangeDataInserter2D(ProbabilityGridRangeDataInserterOptions2D options)
    {
        if (options.HitProbability <= 0.5)
        {
            throw new ArgumentException("hit_probability must be greater than 0.5", nameof(options));
        }
        if (options.MissProbability >= 0.5)
        {
            throw new ArgumentException("miss_probability must be less than 0.5", nameof(options));
        }

        _options = options;
        _hitTable = ProbabilityValues.ComputeLookupTableToApplyCorrespondenceCostOdds(
            ProbabilityValues.Odds(options.HitProbability));
        _missTable = ProbabilityValues.ComputeLookupTableToApplyCorrespondenceCostOdds(
            ProbabilityValues.Odds(options.MissProbability));
    }

    /// <summary>
    /// Inserts 'range_data' into 'grid'.
    /// </summary>
    public void Insert(RangeData rangeData, IGrid grid)
    {
        if (grid is not ProbabilityGrid probabilityGrid)
        {
            throw new ArgumentException("Grid must be a ProbabilityGrid", nameof(grid));
        }

        // Match C++: By not finishing the update after hits are inserted, we give hits priority
        // (i.e. no hits will be ignored because of a miss in the same cell).
        CastRays(rangeData, _hitTable, _missTable, _options.InsertFreeSpace, probabilityGrid);
        probabilityGrid.FinishUpdate();
    }

    private static void GrowAsNeeded(RangeData rangeData, ProbabilityGrid probabilityGrid)
    {
        var origin2D = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
        var minX = origin2D.X - kPadding;
        var minY = origin2D.Y - kPadding;
        var maxX = origin2D.X + kPadding;
        var maxY = origin2D.Y + kPadding;

        foreach (var hit in rangeData.Returns.Points)
        {
            minX = Math.Min(minX, hit.Position.X - kPadding);
            minY = Math.Min(minY, hit.Position.Y - kPadding);
            maxX = Math.Max(maxX, hit.Position.X + kPadding);
            maxY = Math.Max(maxY, hit.Position.Y + kPadding);
        }

        foreach (var miss in rangeData.Misses.Points)
        {
            minX = Math.Min(minX, miss.Position.X - kPadding);
            minY = Math.Min(minY, miss.Position.Y - kPadding);
            maxX = Math.Max(maxX, miss.Position.X + kPadding);
            maxY = Math.Max(maxY, miss.Position.Y + kPadding);
        }

        probabilityGrid.GrowLimits(new Vector2(minX, minY));
        probabilityGrid.GrowLimits(new Vector2(maxX, maxY));
    }

    private static void CastRays(
        RangeData rangeData,
        List<ushort> hitTable,
        List<ushort> missTable,
        bool insertFreeSpace,
        ProbabilityGrid probabilityGrid)
    {
        GrowAsNeeded(rangeData, probabilityGrid);

        var limits = probabilityGrid.Limits;
        var superscaledResolution = limits.Resolution / kSubpixelScale;
        var superscaledLimits = new MapLimits(
            superscaledResolution,
            limits.Max,
            new CellLimits(
                limits.CellLimits.NumXCells * kSubpixelScale,
                limits.CellLimits.NumYCells * kSubpixelScale));

        var origin2D = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);

        var begin = superscaledLimits.GetCellIndex(origin2D);

        // Phase 1: Hit processing (serial - grid writes not thread-safe)
        var returnCount = rangeData.Returns.Points.Count;
        var ends = new Array2i[returnCount];
        for (int i = 0; i < returnCount; i++)
        {
            var hit = rangeData.Returns.Points[i];
            var hit2D = new Vector2(hit.Position.X, hit.Position.Y);
            ends[i] = superscaledLimits.GetCellIndex(hit2D);
            probabilityGrid.ApplyLookupTable(ends[i] / kSubpixelScale, hitTable);
        }

        if (!insertFreeSpace)
        {
            return;
        }

        // Reusable buffer for ray computation - avoids per-ray List<Array2i> allocation.
        // Capacity 512 covers most rays without reallocation (typical ray length at
        // 0.05m resolution with 10m max range ≈ 200 cells).
        var rayBuffer = new List<Array2i>(512);

        // Phase 2: Process miss rays from returns using reusable buffer
        for (int i = 0; i < returnCount; i++)
        {
            rayBuffer.Clear();
            RayToPixelMask.ComputeInto(begin, ends[i], kSubpixelScale, rayBuffer);
            var raySpan = CollectionsMarshal.AsSpan(rayBuffer);
            for (int j = 0; j < raySpan.Length; j++)
            {
                probabilityGrid.ApplyLookupTable(raySpan[j], missTable);
            }
        }

        // Phase 3: Process miss rays from explicit misses using reusable buffer
        var missCount = rangeData.Misses.Points.Count;
        for (int i = 0; i < missCount; i++)
        {
            var miss = rangeData.Misses.Points[i];
            var end = superscaledLimits.GetCellIndex(new Vector2(miss.Position.X, miss.Position.Y));
            rayBuffer.Clear();
            RayToPixelMask.ComputeInto(begin, end, kSubpixelScale, rayBuffer);
            var raySpan = CollectionsMarshal.AsSpan(rayBuffer);
            for (int j = 0; j < raySpan.Length; j++)
            {
                probabilityGrid.ApplyLookupTable(raySpan[j], missTable);
            }
        }
    }
}