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

namespace CartographerSharp.Mapping;

/// <summary>
/// An individual submap, which has a 'local_pose' in the local map frame, keeps
/// track of how many range data were inserted into it, and sets
/// 'insertion_finished' when the map no longer changes and is ready for loop
/// closing.
/// </summary>
public abstract class Submap(Rigid3d localSubmapPose)
{
    private int _numRangeData = 0;
    private bool _insertionFinished = false;

    /// <summary>
    /// Pose of this submap in the local map frame.
    /// </summary>
    public Rigid3d LocalPose => localSubmapPose;

    /// <summary>
    /// Number of RangeData inserted.
    /// </summary>
    public int NumRangeData
    {
        get => Volatile.Read(ref _numRangeData);
        set => Volatile.Write(ref _numRangeData, value);
    }

    /// <summary>
    /// Whether insertion is finished.
    /// </summary>
    public bool InsertionFinished
    {
        get => _insertionFinished;
        set => _insertionFinished = value;
    }

    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public abstract Models.Mapping.Submap ToProto(bool includeGridData);

    /// <summary>
    /// Updates from proto representation.
    /// </summary>
    public abstract void UpdateFromProto(Models.Mapping.Submap proto);

    /// <summary>
    /// Fills data into the response proto.
    /// Note: SubmapQueryResponse proto will be implemented later.
    /// </summary>
    // public abstract void ToResponseProto(Rigid3d globalSubmapPose, SubmapQueryResponse response);
}

/// <summary>
/// Probability value conversion utilities for submaps.
/// </summary>
public static class SubmapProbabilityUtils
{
    /// <summary>
    /// Converts the given probability to log odds.
    /// </summary>
    public static double Logit(double probability)
    {
        return Math.Log(probability / (1.0 - probability));
    }

    public static readonly double kMaxLogOdds = Logit(ProbabilityValues.kMaxProbability);
    public static readonly double kMinLogOdds = Logit(ProbabilityValues.kMinProbability);

    /// <summary>
    /// Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
    /// kMaxLogOdds] is mapped to [1, 255].
    /// </summary>
    public static byte ProbabilityToLogOddsInteger(double probability)
    {
        // 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((Logit(probability) - kMinLogOdds) *
                                    254.0 / (kMaxLogOdds - kMinLogOdds),
                                    MidpointRounding.AwayFromZero) + 1;
        value = Math.Max(1, Math.Min(255, value));
        return (byte)value;
    }
}