BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/TSDValueConverter.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.
 */

namespace CartographerSharp.Mapping.Internal.D2D;

/// <summary>
/// Provides conversions between double and uint16 representations for
/// truncated signed distance values and weights.
/// </summary>
public class TSDValueConverter
{
    private const double kMinWeight = 0.0;
    private const ushort kUnknownTSDValue = 0;
    private const ushort kUnknownWeightValue = 0;
    private const ushort kUpdateMarker = (ushort)(1u << 15); // Highest bit

    private readonly double _maxTSD;
    private readonly double _minTSD;
    private readonly double _maxWeight;
    private readonly double _tsdResolution;
    private readonly double _weightResolution;
    private readonly double[] _valueToTSD;
    private readonly double[] _valueToWeight;

    public TSDValueConverter(double maxTSD, double maxWeight, ValueConversionTables conversionTables)
    {
        _maxTSD = maxTSD;
        _minTSD = -maxTSD;
        _maxWeight = maxWeight;
        _tsdResolution = 32766.0 / (maxTSD - _minTSD);
        _weightResolution = 32766.0 / (maxWeight - kMinWeight);

        // Get conversion tables from ValueConversionTables
        _valueToTSD = conversionTables.GetConversionTable(_minTSD, _minTSD, _maxTSD);
        _valueToWeight = conversionTables.GetConversionTable(kMinWeight, kMinWeight, maxWeight);
    }

    /// <summary>
    /// Converts a TSD to a ushort in the [1, 32767] range.
    /// </summary>
    public ushort TSDToValue(double tsd)
    {
        var clamped = ClampTSD(tsd);
        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
        // (away from zero for .5), equivalent to MidpointRounding.AwayFromZero
        var value = (int)Math.Round((clamped - _minTSD) * _tsdResolution, MidpointRounding.AwayFromZero) + 1;
        return (ushort)Math.Clamp(value, 1, 32767);
    }

    /// <summary>
    /// Converts a weight to a ushort in the [1, 32767] range.
    /// </summary>
    public ushort WeightToValue(double weight)
    {
        var clamped = ClampWeight(weight);
        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
        // (away from zero for .5), equivalent to MidpointRounding.AwayFromZero
        var value = (int)Math.Round((clamped - kMinWeight) * _weightResolution, MidpointRounding.AwayFromZero) + 1;
        return (ushort)Math.Clamp(value, 1, 32767);
    }

    /// <summary>
    /// Converts a ushort (which may or may not have the update marker set) to a
    /// value in the range [min_tsd_, max_tsd_].
    /// Match C++: return (*value_to_tsd_)[value];
    /// Note: C++ conversion table has size 65536 (all possible ushort values),
    /// and handles update marker by masking it out in PrecomputeValueToBoundedFloat.
    /// </summary>
    public double ValueToTSD(ushort value)
    {
        // Match C++: value_to_tsd_ has size 65536 (all possible ushort values)
        // C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
        // So we can safely access _valueToTSD[value] even if value has update marker set
        if (value >= _valueToTSD.Length)
        {
            // Defensive check: should never happen if conversion table is correctly sized
            return _minTSD;
        }
        return _valueToTSD[value];
    }

    /// <summary>
    /// Converts a ushort (which may or may not have the update marker set) to a
    /// value in the range [min_weight_, max_weight_].
    /// Match C++: return (*value_to_weight_)[value];
    /// Note: C++ conversion table has size 65536 (all possible ushort values),
    /// and handles update marker by masking it out in PrecomputeValueToBoundedFloat.
    /// </summary>
    public double ValueToWeight(ushort value)
    {
        // Match C++: value_to_weight_ has size 65536 (all possible ushort values)
        // C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
        // So we can safely access _valueToWeight[value] even if value has update marker set
        if (value >= _valueToWeight.Length)
        {
            // Defensive check: should never happen if conversion table is correctly sized
            return kMinWeight;
        }
        return _valueToWeight[value];
    }

    public static ushort GetUnknownTSDValue() => kUnknownTSDValue;
    public static ushort GetUnknownWeightValue() => kUnknownWeightValue;
    public static ushort GetUpdateMarker() => kUpdateMarker;

    public double GetMaxTSD() => _maxTSD;
    public double GetMinTSD() => _minTSD;
    public double GetMaxWeight() => _maxWeight;
    public double GetMinWeight() => kMinWeight;

    /// <summary>
    /// Clamps TSD to be in the range [min_tsd_, max_tsd_].
    /// </summary>
    private double ClampTSD(double tsd)
    {
        return Math.Clamp(tsd, _minTSD, _maxTSD);
    }

    /// <summary>
    /// Clamps weight to be in the range [min_weight_, max_weight_].
    /// </summary>
    private double ClampWeight(double weight)
    {
        return Math.Clamp(weight, kMinWeight, _maxWeight);
    }
}