/*
 * 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;

/// <summary>
/// Performs lazy computations of lookup tables for mapping from a uint16 value
/// to a double in ['lower_bound', 'upper_bound']. The first element of the table
/// is set to 'unknown_result'.
/// </summary>
public class ValueConversionTables
{
    private readonly Dictionary<(double unknownResult, double lowerBound, double upperBound), double[]> _boundsToLookupTable;

    public ValueConversionTables()
    {
        _boundsToLookupTable = [];
    }

    /// <summary>
    /// Gets or creates a conversion table for the given bounds.
    /// </summary>
    public double[] GetConversionTable(double unknownResult, double lowerBound, double upperBound)
    {
        var key = (unknownResult, lowerBound, upperBound);
        
        if (!_boundsToLookupTable.TryGetValue(key, out var table))
        {
            table = PrecomputeValueToBoundedFloat(unknownResult, lowerBound, upperBound);
            _boundsToLookupTable[key] = table;
        }
        
        return table;
    }

    /// <summary>
    /// Precomputes value to bounded double lookup table.
    /// Match C++: size_t num_values = std::numeric_limits<uint16>::max() + 1; (65536)
    /// C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
    /// </summary>
    private static double[] PrecomputeValueToBoundedFloat(
        double unknownResult, double lowerBound, double upperBound)
    {
        // Match C++: num_values = std::numeric_limits<uint16>::max() + 1 = 65536
        const int kValueCount = 65536; // All possible ushort values (0-65535)
        const ushort kUpdateMarker = (ushort)(1u << 15); // Highest bit
        var result = new double[kValueCount];
        
        // Match C++: for (size_t value = 0; value != num_values; ++value)
        // C++: SlowValueToBoundedFloat(static_cast<uint16>(value) & ~kUpdateMarker, ...)
        // Mask out update marker before computing conversion
        for (int value = 0; value != kValueCount; value++)
        {
            // Match C++: Mask out update marker: value & ~kUpdateMarker
            var valueWithoutMarker = (ushort)(value & ~kUpdateMarker);
            result[value] = SlowValueToBoundedFloat(
                valueWithoutMarker, unknownResult, lowerBound, upperBound);
        }
        
        return result;
    }

    /// <summary>
    /// Slow conversion from value to bounded double (used for precomputation).
    /// Match C++: CHECK_LE(value, 32767) and kScale calculation.
    /// </summary>
    private static double SlowValueToBoundedFloat(
        ushort value, double unknownResult, double lowerBound, double upperBound)
    {
        // Match C++: CHECK_LE(value, 32767) - value should be <= 32767 after masking
        // (value is already masked in PrecomputeValueToBoundedFloat)
        const ushort kUnknownValue = 0;
        if (value == kUnknownValue)
        {
            return unknownResult;
        }
        
        // Match C++: const double kScale = (upper_bound - lower_bound) / 32766.f;
        var kScale = (upperBound - lowerBound) / 32766.0;
        return value * kScale + (lowerBound - kScale);
    }
}