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

namespace CartographerSharp.Mapping;

/// <summary>
/// Match C++ PureLocalizationTrimmer: keeps only the last 'num_submaps_to_keep'
/// submaps of a localization trajectory, trimming older ones.
/// Used when trajectory_options has pure_localization_trimmer (map_builder.cc MaybeAddPureLocalizationTrimmer).
/// </summary>
public class PureLocalizationTrimmer : PoseGraphTrimmer
{
    private readonly int _trajectoryId;
    private int _numSubmapsToKeep;
    private bool _finished;

    /// <summary>
    /// Match C++ PureLocalizationTrimmer(trajectory_id, num_submaps_to_keep).
    /// CHECK_GE(num_submaps_to_keep, 2) in C++.
    /// </summary>
    public PureLocalizationTrimmer(int trajectoryId, int numSubmapsToKeep)
    {
        if (numSubmapsToKeep < 2)
            throw new ArgumentException("Cannot trim with less than 2 submaps", nameof(numSubmapsToKeep));
        _trajectoryId = trajectoryId;
        _numSubmapsToKeep = numSubmapsToKeep;
    }

    /// <summary>
    /// Match C++ Trim(Trimmable*).
    /// </summary>
    public override void Trim(ITrimmable trimmable)
    {
        if (trimmable.IsFinished(_trajectoryId))
        {
            _numSubmapsToKeep = 0;
        }

        var submapIds = trimmable.GetSubmapIds(_trajectoryId);
        // C++: for (i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) TrimSubmap(submap_ids.at(i));
        for (int i = 0; i + _numSubmapsToKeep < submapIds.Count; ++i)
        {
            trimmable.TrimSubmap(submapIds[i]);
        }

        if (_numSubmapsToKeep == 0)
        {
            _finished = true;
            trimmable.SetTrajectoryState(_trajectoryId, IPoseGraph.TrajectoryState.Deleted);
        }
    }

    /// <summary>
    /// Match C++ IsFinished().
    /// </summary>
    public bool IsFinished() => _finished;
}