/*
 * 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 System.Text.Json.Serialization;

namespace CartographerSharp.Models.Mapping;

/// <summary>
/// Protocol buffer representation of pose graph options.
/// </summary>
public struct PoseGraphOptions
{
    /// <summary>
    /// Overlapping submaps trimmer options for 2D.
    /// </summary>
    public struct OverlappingSubmapsTrimmerOptions2D
    {
        [JsonPropertyName("fresh_submaps_count")]
        public int FreshSubmapsCount { get; set; }

        [JsonPropertyName("min_covered_area")]
        public double MinCoveredArea { get; set; }

        [JsonPropertyName("min_added_submaps_count")]
        public int MinAddedSubmapsCount { get; set; }

        public OverlappingSubmapsTrimmerOptions2D(int freshSubmapsCount, double minCoveredArea, int minAddedSubmapsCount)
        {
            FreshSubmapsCount = freshSubmapsCount;
            MinCoveredArea = minCoveredArea;
            MinAddedSubmapsCount = minAddedSubmapsCount;
        }
    }

    /// <summary>
    /// Online loop closure: If positive, will run the loop closure while the map is built.
    /// </summary>
    [JsonPropertyName("optimize_every_n_nodes")]
    public int OptimizeEveryNNodes { get; set; }

    /// <summary>
    /// Weight used in the optimization problem for the translational component of
    /// non-loop-closure scan matcher constraints.
    /// </summary>
    [JsonPropertyName("matcher_translation_weight")]
    public double MatcherTranslationWeight { get; set; }

    /// <summary>
    /// Weight used in the optimization problem for the rotational component of
    /// non-loop-closure scan matcher constraints.
    /// </summary>
    [JsonPropertyName("matcher_rotation_weight")]
    public double MatcherRotationWeight { get; set; }

    /// <summary>
    /// Number of iterations to use in 'optimization_problem_options' for the final optimization.
    /// </summary>
    [JsonPropertyName("max_num_final_iterations")]
    public int MaxNumFinalIterations { get; set; }

    /// <summary>
    /// Rate at which we sample a single trajectory's nodes for global localization.
    /// </summary>
    [JsonPropertyName("global_sampling_ratio")]
    public double GlobalSamplingRatio { get; set; }

    /// <summary>
    /// Whether to output histograms for the pose residuals.
    /// </summary>
    [JsonPropertyName("log_residual_histograms")]
    public bool LogResidualHistograms { get; set; }

    /// <summary>
    /// If for the duration specified by this option no global constraint has been
    /// added between two trajectories, loop closure searches will be performed
    /// globally rather than in a smaller search window.
    /// </summary>
    [JsonPropertyName("global_constraint_search_after_n_seconds")]
    public double GlobalConstraintSearchAfterNSeconds { get; set; }

    /// <summary>
    /// Instantiates the 'OverlappingSubmapsTrimmer2d' which trims submaps from the
    /// pose graph based on the area of overlap.
    /// </summary>
    [JsonPropertyName("overlapping_submaps_trimmer_2d")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public OverlappingSubmapsTrimmerOptions2D? OverlappingSubmapsTrimmer2D { get; set; }

    /// <summary>
    /// Options for the constraint builder used to find loop closures.
    /// </summary>
    [JsonPropertyName("constraint_builder_options")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public ConstraintBuilderOptions? ConstraintBuilderOptions { get; set; }

    /// <summary>
    /// Options for the optimization problem.
    /// </summary>
    [JsonPropertyName("optimization_problem_options")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public OptimizationProblemOptions? OptimizationProblemOptions { get; set; }

    /// <summary>
    /// Max number of nodes to search for relocalization constraint (match C++ max_number_relocalization_nodes).
    /// </summary>
    [JsonPropertyName("max_number_relocalization_nodes")]
    public int MaxNumberRelocalizationNodes { get; set; } = 12;

    /// <summary>
    /// Enable loop closure detection within single trajectory.
    /// When enabled, uses MatchFullSubmap instead of Match for nodes that are
    /// far from their insertion submap but close to older finished submaps.
    /// This is an extension to C++ Cartographer which only supports multi-trajectory loop closure.
    /// </summary>
    [JsonPropertyName("enable_single_trajectory_loop_closure")]
    public bool EnableSingleTrajectoryLoopClosure { get; set; } = true;

    /// <summary>
    /// Distance threshold (in meters) for triggering single-trajectory loop closure.
    /// If a node's initial_relative_pose distance to a finished submap exceeds this threshold,
    /// but the node is within search range, use MatchFullSubmap for global search.
    /// Only used when EnableSingleTrajectoryLoopClosure is true.
    /// </summary>
    [JsonPropertyName("single_trajectory_loop_closure_distance_threshold")]
    public double SingleTrajectoryLoopClosureDistanceThreshold { get; set; } = 3.0;

    public PoseGraphOptions(
        int optimizeEveryNNodes = 90,
        double matcherTranslationWeight = 5e2,
        double matcherRotationWeight = 1.6e3,
        int maxNumFinalIterations = 200,
        double globalSamplingRatio = 0.003,
        bool logResidualHistograms = false,
        double globalConstraintSearchAfterNSeconds = 0.0,
        OverlappingSubmapsTrimmerOptions2D? overlappingSubmapsTrimmer2D = null,
        ConstraintBuilderOptions? constraintBuilderOptions = null,
        OptimizationProblemOptions? optimizationProblemOptions = null)
    {
        OptimizeEveryNNodes = optimizeEveryNNodes;
        MatcherTranslationWeight = matcherTranslationWeight;
        MatcherRotationWeight = matcherRotationWeight;
        MaxNumFinalIterations = maxNumFinalIterations;
        GlobalSamplingRatio = globalSamplingRatio;
        LogResidualHistograms = logResidualHistograms;
        GlobalConstraintSearchAfterNSeconds = globalConstraintSearchAfterNSeconds;
        OverlappingSubmapsTrimmer2D = overlappingSubmapsTrimmer2D;
        ConstraintBuilderOptions = constraintBuilderOptions;
        OptimizationProblemOptions = optimizationProblemOptions;
        MaxNumberRelocalizationNodes = 12;
    }
}