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

namespace CartographerSharp.Mapping;

/// <summary>
/// Probability value conversion utilities.
/// </summary>
public static class ProbabilityValues
{
    public const double kMinProbability = 0.1;
    public const double kMaxProbability = 1.0 - kMinProbability;
    public const double kMinCorrespondenceCost = 1.0 - kMaxProbability;
    public const double kMaxCorrespondenceCost = 1.0 - kMinProbability;

    private const ushort kUnknownProbabilityValue = 0;
    private const ushort kUnknownCorrespondenceValue = kUnknownProbabilityValue;
    private const ushort kUpdateMarker = (ushort)(1u << 15);

    private const int kValueCount = 32768;

    // Precomputed lookup tables
    private static readonly double[] kValueToProbability;
    private static readonly double[] kValueToCorrespondenceCost;

    static ProbabilityValues()
    {
        kValueToProbability = PrecomputeValueToProbability();
        kValueToCorrespondenceCost = PrecomputeValueToCorrespondenceCost();
    }

    /// <summary>
    /// Converts probability to odds.
    /// </summary>
    public static double Odds(double probability)
    {
        return probability / (1.0 - probability);
    }

    /// <summary>
    /// Converts odds to probability.
    /// </summary>
    public static double ProbabilityFromOdds(double odds)
    {
        return odds / (odds + 1.0);
    }

    /// <summary>
    /// Converts probability to correspondence cost.
    /// </summary>
    public static double ProbabilityToCorrespondenceCost(double probability)
    {
        return 1.0 - probability;
    }

    /// <summary>
    /// Converts correspondence cost to probability.
    /// </summary>
    public static double CorrespondenceCostToProbability(double correspondenceCost)
    {
        return 1.0 - correspondenceCost;
    }

    /// <summary>
    /// Clamps probability to be in the range [kMinProbability, kMaxProbability].
    /// </summary>
    public static double ClampProbability(double probability)
    {
        return MathUtils.Clamp(probability, kMinProbability, kMaxProbability);
    }

    /// <summary>
    /// Clamps correspondence cost to be in the range [kMinCorrespondenceCost, kMaxCorrespondenceCost].
    /// </summary>
    public static double ClampCorrespondenceCost(double correspondenceCost)
    {
        return MathUtils.Clamp(correspondenceCost, kMinCorrespondenceCost, kMaxCorrespondenceCost);
    }

    /// <summary>
    /// Converts a correspondence_cost to a uint16 in the [1, 32767] range.
    /// </summary>
    public static ushort CorrespondenceCostToValue(double correspondenceCost)
    {
        return BoundedFloatToValue(correspondenceCost, kMinCorrespondenceCost, kMaxCorrespondenceCost);
    }

    /// <summary>
    /// Converts a probability to a uint16 in the [1, 32767] range.
    /// </summary>
    public static ushort ProbabilityToValue(double probability)
    {
        return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
    }

    /// <summary>
    /// Converts a uint16 (which may or may not have the update marker set) to a
    /// probability in the range [kMinProbability, kMaxProbability].
    /// </summary>
    public static double ValueToProbability(ushort value)
    {
        if (value >= kValueToProbability.Length)
        {
            return kMinProbability;
        }
        return kValueToProbability[value];
    }

    /// <summary>
    /// Converts a uint16 (which may or may not have the update marker set) to a
    /// correspondence cost in the range [kMinCorrespondenceCost, kMaxCorrespondenceCost].
    /// </summary>
    public static double ValueToCorrespondenceCost(ushort value)
    {
        if (value >= kValueToCorrespondenceCost.Length)
        {
            return kMaxCorrespondenceCost;
        }
        return kValueToCorrespondenceCost[value];
    }

    /// <summary>
    /// Converts probability value to correspondence cost value.
    /// </summary>
    public static ushort ProbabilityValueToCorrespondenceCostValue(ushort probabilityValue)
    {
        if (probabilityValue == kUnknownProbabilityValue)
        {
            return kUnknownCorrespondenceValue;
        }

        bool updateCarry = false;
        if (probabilityValue > kUpdateMarker)
        {
            probabilityValue -= kUpdateMarker;
            updateCarry = true;
        }

        ushort result = CorrespondenceCostToValue(
            ProbabilityToCorrespondenceCost(ValueToProbability(probabilityValue)));
        if (updateCarry)
        {
            result += kUpdateMarker;
        }
        return result;
    }

    /// <summary>
    /// Converts correspondence cost value to probability value.
    /// </summary>
    public static ushort CorrespondenceCostValueToProbabilityValue(ushort correspondenceCostValue)
    {
        if (correspondenceCostValue == kUnknownCorrespondenceValue)
        {
            return kUnknownProbabilityValue;
        }

        bool updateCarry = false;
        if (correspondenceCostValue > kUpdateMarker)
        {
            correspondenceCostValue -= kUpdateMarker;
            updateCarry = true;
        }

        ushort result = ProbabilityToValue(CorrespondenceCostToProbability(
            ValueToCorrespondenceCost(correspondenceCostValue)));
        if (updateCarry)
        {
            result += kUpdateMarker;
        }
        return result;
    }

    /// <summary>
    /// Computes lookup table to apply odds.
    /// </summary>
    public static List<ushort> ComputeLookupTableToApplyOdds(double odds)
    {
        var result = new List<ushort>(kValueCount)
        {
            (ushort)(ProbabilityToValue(ProbabilityFromOdds(odds)) + kUpdateMarker)
        };
        
        for (int cell = 1; cell != kValueCount; cell++)
        {
            result.Add((ushort)(ProbabilityToValue(ProbabilityFromOdds(
                odds * Odds(ValueToProbability((ushort)cell)))) + kUpdateMarker));
        }
        
        return result;
    }

    /// <summary>
    /// Computes lookup table to apply correspondence cost odds.
    /// </summary>
    public static List<ushort> ComputeLookupTableToApplyCorrespondenceCostOdds(double odds)
    {
        var result = new List<ushort>(kValueCount)
        {
            (ushort)(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
            ProbabilityFromOdds(odds))) + kUpdateMarker)
        };
        
        for (int cell = 1; cell != kValueCount; cell++)
        {
            result.Add((ushort)(CorrespondenceCostToValue(
                ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
                    odds * Odds(CorrespondenceCostToProbability(
                        ValueToCorrespondenceCost((ushort)cell)))))) + kUpdateMarker));
        }
        
        return result;
    }

    /// <summary>
    /// Converts a bounded double value to a uint16 value.
    /// </summary>
    private static ushort BoundedFloatToValue(double floatValue, double lowerBound, double upperBound)
    {
        var clamped = MathUtils.Clamp(floatValue, lowerBound, upperBound);
        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
        // with rounding half away from zero (not banker's rounding)
        var value = (int)Math.Round(
            (clamped - lowerBound) * (32766.0 / (upperBound - lowerBound)),
            MidpointRounding.AwayFromZero) + 1;
        
        // Clamp to valid range
        value = Math.Max(1, Math.Min(32767, value));
        return (ushort)value;
    }

    /// <summary>
    /// Precomputes value to probability lookup table.
    /// </summary>
    private static double[] PrecomputeValueToProbability()
    {
        return PrecomputeValueToBoundedFloat(
            kUnknownProbabilityValue, kMinProbability, kMinProbability, kMaxProbability);
    }

    /// <summary>
    /// Precomputes value to correspondence cost lookup table.
    /// </summary>
    private static double[] PrecomputeValueToCorrespondenceCost()
    {
        return PrecomputeValueToBoundedFloat(
            kUnknownCorrespondenceValue, kMaxCorrespondenceCost,
            kMinCorrespondenceCost, kMaxCorrespondenceCost);
    }

    /// <summary>
    /// Precomputes value to bounded double lookup table.
    /// </summary>
    private static double[] PrecomputeValueToBoundedFloat(
        ushort unknownValue, double unknownResult, double lowerBound, double upperBound)
    {
        // Repeat two times, so that both values with and without the update marker
        // can be converted to a probability.
        const int kRepetitionCount = 2;
        var result = new double[kRepetitionCount * kValueCount];
        
        for (int repeat = 0; repeat != kRepetitionCount; repeat++)
        {
            for (int value = 0; value != kValueCount; value++)
            {
                result[repeat * kValueCount + value] = SlowValueToBoundedFloat(
                    (ushort)value, unknownValue, unknownResult, lowerBound, upperBound);
            }
        }
        
        return result;
    }

    /// <summary>
    /// Slow conversion from value to bounded double (used for precomputation).
    /// </summary>
    private static double SlowValueToBoundedFloat(
        ushort value, ushort unknownValue, double unknownResult,
        double lowerBound, double upperBound)
    {
        if (value == unknownValue)
        {
            return unknownResult;
        }
        
        // Match C++: const double kScale = (upper_bound - lower_bound) / 32766.f;
        // kValueCount = 32768, so kValueCount - 2.0 = 32766.0
        // Use explicit 32766.0 for consistency with C++ and ValueConversionTables
        var kScale = (upperBound - lowerBound) / 32766.0;
        return value * kScale + (lowerBound - kScale);
    }
}