/*
 * Copyright 2017 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.Models.Mapping;
using CartographerSharp.Transform;

namespace CartographerSharp.Mapping;

/// <summary>
/// This interface is used for both library and RPC implementations.
/// Implementations wire up the complete SLAM stack.
/// </summary>
public interface IMapBuilder : IDisposable
{
    /// <summary>
    /// Creates a new trajectory builder and returns its index.
    /// </summary>
    int AddTrajectoryBuilder(
        HashSet<ITrajectoryBuilder.SensorId> expectedSensorIds,
        TrajectoryBuilderOptions trajectoryOptions);

    /// <summary>
    /// Creates a new trajectory and returns its index. Querying the trajectory
    /// builder for it will return 'null'.
    /// </summary>
    int AddTrajectoryForDeserialization(
        TrajectoryBuilderOptionsWithSensorIds optionsWithSensorIdsProto);

    /// <summary>
    /// Returns the 'ITrajectoryBuilder' corresponding to the specified
    /// 'trajectory_id' or 'null' if the trajectory has no corresponding builder.
    /// </summary>
    ITrajectoryBuilder? GetTrajectoryBuilder(int trajectoryId);

    /// <summary>
    /// Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished,
    /// i.e. no further sensor data is expected.
    /// </summary>
    void FinishTrajectory(int trajectoryId);

    /// <summary>
    /// Fills the SubmapQuery::Response corresponding to 'submap_id'. Returns an
    /// error string on failure, or an empty string on success.
    /// </summary>
    string SubmapToProto(SubmapId submapId, out Models.Mapping.SubmapQuery.Response response);

    /// <summary>
    /// Serializes the current state to a proto stream. If
    /// 'include_unfinished_submaps' is set to true, unfinished submaps, i.e.
    /// submaps that have not yet received all rangefinder data insertions, will
    /// be included in the serialized state.
    /// </summary>
    void SerializeState(bool includeUnfinishedSubmaps, IO.IProtoStreamWriter writer);

    /// <summary>
    /// Serializes the current state to a proto stream file on the host system.
    /// Returns true if the file was successfully written.
    /// </summary>
    bool SerializeStateToFile(bool includeUnfinishedSubmaps, string filename);

    /// <summary>
    /// Loads the SLAM state from a proto stream. Returns the remapping of new trajectory_ids.
    /// </summary>
    Dictionary<int, int> LoadState(IO.IProtoStreamReader reader, bool loadFrozenState);

    /// <summary>
    /// Loads the SLAM state from a pbstream file. Returns the remapping of new trajectory_ids.
    /// </summary>
    Dictionary<int, int> LoadStateFromFile(string filename, bool loadFrozenState);

    /// <summary>
    /// Starts relocalization (match C++ MapBuilder::StartRelocalization).
    /// Sets pose graph localization callback and initial poses so constraint builder
    /// uses them when finding constraints (MaybeAddLocalizationConstraint).
    /// Call after AddTrajectoryBuilder when starting localization (e.g. xloc calls
    /// StartRelocalization(initial_poses, callback) after AddTrajectory).
    /// </summary>
    /// <param name="initialPoses">Initial poses in global frame (e.g. from MCL).</param>
    /// <param name="callback">Invoked when relocalization succeeds or when max nodes searched without success (trajectoryId=-1).</param>
    void StartRelocalization(IReadOnlyList<Rigid3d> initialPoses, Action<int, long, Rigid3d>? callback);

    int NumTrajectoryBuilders { get; }

    IPoseGraph PoseGraph { get; }

    List<TrajectoryBuilderOptionsWithSensorIds> GetAllTrajectoryBuilderOptions();
}