BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/IO/ProtoStreamDeserializer.cs

/*
 * Copyright 2018 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.Mapping;

namespace CartographerSharp.IO;

/// <summary>
/// Helper for deserializing a previously serialized mapping state from a proto stream,
/// abstracting away the format parsing logic.
/// </summary>
public class ProtoStreamDeserializer
{
    private const uint FormatVersion = 2;
    private const uint FormatVersionWithoutSubmapHistograms = 1;

    private readonly IProtoStreamReader _reader;
    private readonly SerializationHeader _header;
    private readonly SerializedData _poseGraph;
    private readonly SerializedData _allTrajectoryBuilderOptions;

    public ProtoStreamDeserializer(IProtoStreamReader reader)
    {
        ArgumentNullException.ThrowIfNull(reader);

        _reader = reader;

        // Read header - use same pattern as ReadNextSerializedData
        // ReadProto returns T? which for structs becomes Nullable<T>
        // Using out var should work, but compiler may not infer nullable correctly
        // So we use a helper method pattern
        if (!ReadNextSerializedData(out var headerDataNullable) || !headerDataNullable.HasValue)
        {
            throw new InvalidOperationException("Failed to read SerializationHeader.");
        }
        
        var headerData = headerDataNullable.Value;
        if (!headerData.SerializationHeader.HasValue)
        {
            throw new InvalidOperationException("SerializedData does not contain SerializationHeader.");
        }

        _header = headerData.SerializationHeader.Value;

        // Validate format version
        if (!IsVersionSupported(_header))
        {
            throw new NotSupportedException(
                $"Unsupported serialization format version: {_header.FormatVersion}. " +
                $"Supported versions: {FormatVersionWithoutSubmapHistograms}, {FormatVersion}");
        }

        // Read pose graph
        if (!ReadNextSerializedData(out SerializedData? poseGraphDataNullable) || !poseGraphDataNullable.HasValue)
        {
            throw new InvalidOperationException(
                "Serialized stream misses PoseGraph. Expecting `PoseGraph` after `SerializationHeader`.");
        }

        var poseGraphData = poseGraphDataNullable.Value;
        if (!poseGraphData.PoseGraph.HasValue)
        {
            throw new InvalidOperationException(
                "SerializedData does not contain PoseGraph. Expecting `PoseGraph` after `SerializationHeader`.");
        }

        _poseGraph = poseGraphData;

        // Read trajectory builder options
        if (!ReadNextSerializedData(out var optionsDataNullable) || !optionsDataNullable.HasValue)
        {
            throw new InvalidOperationException(
                "Serialized stream misses `AllTrajectoryBuilderOptions`. " +
                "Expecting `AllTrajectoryBuilderOptions` after PoseGraph.");
        }

        var optionsData = optionsDataNullable.Value;
        if (!optionsData.AllTrajectoryBuilderOptions.HasValue)
        {
            throw new InvalidOperationException(
                "SerializedData does not contain AllTrajectoryBuilderOptions. " +
                "Expecting `AllTrajectoryBuilderOptions` after PoseGraph.");
        }

        _allTrajectoryBuilderOptions = optionsData;

        // Validate that trajectory count matches
        if (_poseGraph.PoseGraph.HasValue && _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.HasValue)
        {
            var poseGraphProto = _poseGraph.PoseGraph.Value;
            var optionsProto = _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.Value;

            if (poseGraphProto.Trajectories.Count != optionsProto.OptionsWithSensorIds.Count)
            {
                throw new InvalidOperationException(
                    $"Trajectory count mismatch: PoseGraph has {poseGraphProto.Trajectories.Count} trajectories, " +
                    $"but AllTrajectoryBuilderOptions has {optionsProto.OptionsWithSensorIds.Count} options.");
            }
        }
    }

    /// <summary>
    /// Gets the serialization header.
    /// </summary>
    public SerializationHeader Header => _header;

    /// <summary>
    /// Gets the pose graph proto.
    /// </summary>
    public PoseGraph PoseGraph
    {
        get
        {
            if (!_poseGraph.PoseGraph.HasValue)
                throw new InvalidOperationException("PoseGraph is not available.");
            return _poseGraph.PoseGraph.Value;
        }
    }

    /// <summary>
    /// Gets all trajectory builder options.
    /// </summary>
    public AllTrajectoryBuilderOptions AllTrajectoryBuilderOptions
    {
        get
        {
            if (!_allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.HasValue)
                throw new InvalidOperationException("AllTrajectoryBuilderOptions is not available.");
            return _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.Value;
        }
    }

    /// <summary>
    /// Reads the next SerializedData message from the ProtoStream.
    /// Returns true if the message was successfully read, or false if there are no more messages or an error occurred.
    /// </summary>
    public bool ReadNextSerializedData(out SerializedData? data)
    {
        if (_reader.Eof)
        {
            data = null;
            return false;
        }

        if (!_reader.ReadProto<SerializedData>(out var proto))
        {
            data = null;
            return false;
        }

        data = proto;
        return data.HasValue;
    }

    private static bool IsVersionSupported(SerializationHeader header)
    {
        return header.FormatVersion == FormatVersion ||
               header.FormatVersion == FormatVersionWithoutSubmapHistograms;
    }
}