BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/Submap2D.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.Models.Transform;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using System;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping.D2D;

/// <summary>
/// 2D Submap implementation.
/// </summary>
public class Submap2D : Submap
{
    private Grid2D? _grid;
    private Grid2D? _highResGrid;
    private readonly ValueConversionTables _conversionTables;

    public Submap2D(Rigid3d localSubmapPose, Grid2D grid, ValueConversionTables conversionTables, Grid2D? highResGrid = null)
        : base(localSubmapPose) // Pass the full Rigid3d pose including rotation
    {
        _grid = grid;
        _highResGrid = highResGrid;
        _conversionTables = conversionTables;
    }
    
    // Keep old constructor for backward compatibility (if needed)
    public Submap2D(Vector2 origin, Grid2D grid, ValueConversionTables conversionTables)
        : this(new Rigid3d(new Vector3(origin.X, origin.Y, 0.0), Quaternion.Identity), grid, conversionTables)
    {
    }

    public Submap2D(Models.Mapping.Submap2D proto, ValueConversionTables conversionTables)
        : base((Rigid3d)proto.LocalPose)
    {
        _conversionTables = conversionTables;
        NumRangeData = proto.NumRangeData;
        InsertionFinished = proto.Finished;

        // Match C++: if (proto.has_grid()) - no resolution check
        if (proto.Grid.HasValue)
        {
            var gridProto = proto.Grid.Value;
            if (gridProto.ProbabilityGrid2D.HasValue)
            {
                _grid = new ProbabilityGrid(gridProto, conversionTables);
            }
            else if (gridProto.Tsdf2D.HasValue)
            {
                _grid = new TSDF2D(gridProto, conversionTables);
            }
            else
            {
                throw new ArgumentException("proto::Submap2D has grid with unknown type.", nameof(proto));
            }
        }
    }

    /// <summary>
    /// Gets the grid.
    /// </summary>
    public Grid2D? Grid => _grid;

    /// <summary>
    /// Gets the optional high resolution grid for fine-grained Ceres scan matching.
    /// </summary>
    public Grid2D? HighResGrid => _highResGrid;

    /// <summary>
    /// Insert 'range_data' into this submap using 'range_data_inserter'. The
    /// submap must not be finished yet.
    /// </summary>
    public void InsertRangeData(RangeData rangeData, IRangeDataInserter rangeDataInserter)
    {
        if (InsertionFinished)
        {
            throw new InvalidOperationException("Cannot insert range data into finished submap");
        }
        if (_grid == null)
        {
            throw new InvalidOperationException("Grid is not initialized");
        }
        // Insert range data directly (already transformed to submap frame in ActiveSubmaps2D)
        if (_highResGrid != null)
        {
            // Main grid and high-res grid are independent - insert in parallel.
            var highResGrid = _highResGrid;
            var highResTask = Task.Run(() => rangeDataInserter.Insert(rangeData, (IGrid)highResGrid));
            rangeDataInserter.Insert(rangeData, (IGrid)_grid);
            highResTask.GetAwaiter().GetResult();
        }
        else
        {
            rangeDataInserter.Insert(rangeData, (IGrid)_grid);
        }
        NumRangeData++;
    }

    /// <summary>
    /// Finishes the submap.
    /// Match C++: grid_ = grid_->ComputeCroppedGrid(); to reduce memory usage
    /// </summary>
    public void Finish()
    {
        if (_grid == null)
        {
            throw new InvalidOperationException("Grid is not initialized");
        }
        if (InsertionFinished)
        {
            throw new InvalidOperationException("Submap is already finished");
        }
        // Match C++: Crop grid to reduce memory usage when submap is finished
        _grid = _grid.ComputeCroppedGrid();
        _highResGrid = _highResGrid?.ComputeCroppedGrid();
        InsertionFinished = true;
    }

    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public override Models.Mapping.Submap ToProto(bool includeGridData)
    {
        var submap2D = new Models.Mapping.Submap2D(
            (Rigid3dProto)LocalPose,
            NumRangeData,
            InsertionFinished,
            includeGridData && _grid != null ? _grid.ToProto() : null
        );

        // Note: SubmapId will be set by caller
        // Note: SubmapId will be set by caller when creating full Submap
        return new Models.Mapping.Submap(new Models.Mapping.PoseGraph.SubmapId(0, 0), submap2D, null);
    }

    /// <summary>
    /// Updates from proto representation.
    /// Match C++: CHECK(proto.has_submap_2d())
    /// </summary>
    public override void UpdateFromProto(Models.Mapping.Submap proto)
    {
        if (!proto.Submap2D.HasValue)
        {
            throw new ArgumentException("Proto must contain Submap2D", nameof(proto));
        }

        var submap2D = proto.Submap2D.Value;
        NumRangeData = submap2D.NumRangeData;
        InsertionFinished = submap2D.Finished;
        
        // Match C++: if (proto.submap_2d().has_grid()) - no resolution check
        if (submap2D.Grid.HasValue)
        {
            var gridProto = submap2D.Grid.Value;
            if (gridProto.ProbabilityGrid2D.HasValue)
            {
                _grid = new ProbabilityGrid(gridProto, _conversionTables);
            }
            else if (gridProto.Tsdf2D.HasValue)
            {
                _grid = new TSDF2D(gridProto, _conversionTables);
            }
            else
            {
                throw new ArgumentException("proto::Submap2D has grid with unknown type.", nameof(proto));
            }
        }
    }
}