/*
 * 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;
using AdaptiveVoxelFilterOptions = CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions;

namespace CartographerSharp.Models.Mapping;

/// <summary>
/// Protocol buffer representation of local trajectory builder options for 2D.
/// </summary>
public struct LocalTrajectoryBuilderOptions2D
{
    /// <summary>
    /// Rangefinder points outside these ranges will be dropped.
    /// </summary>
    [JsonPropertyName("min_range")]
    public double MinRange { get; set; }

    [JsonPropertyName("max_range")]
    public double MaxRange { get; set; }

    [JsonPropertyName("min_z")]
    public double MinZ { get; set; }

    [JsonPropertyName("max_z")]
    public double MaxZ { get; set; }

    /// <summary>
    /// Points beyond 'max_range' will be inserted with this length as empty space.
    /// </summary>
    [JsonPropertyName("missing_data_ray_length")]
    public double MissingDataRayLength { get; set; }

    /// <summary>
    /// Number of range data to accumulate into one unwarped, combined range data
    /// to use for scan matching.
    /// </summary>
    [JsonPropertyName("num_accumulated_range_data")]
    public int NumAccumulatedRangeData { get; set; }

    /// <summary>
    /// Voxel filter that gets applied to the range data immediately after cropping.
    /// </summary>
    [JsonPropertyName("voxel_filter_size")]
    public double VoxelFilterSize { get; set; }

    /// <summary>
    /// Whether to solve the online scan matching first using the correlative scan
    /// matcher to generate a good starting point for Ceres.
    /// </summary>
    [JsonPropertyName("use_online_correlative_scan_matching")]
    public bool UseOnlineCorrelativeScanMatching { get; set; }

    [JsonPropertyName("real_time_correlative_scan_matcher_options")]
    public RealTimeCorrelativeScanMatcherOptions RealTimeCorrelativeScanMatcherOptions { get; set; }

    [JsonPropertyName("ceres_scan_matcher_options")]
    public CeresScanMatcherOptions2D CeresScanMatcherOptions { get; set; }

    [JsonPropertyName("motion_filter_options")]
    public MotionFilterOptions MotionFilterOptions { get; set; }

    [JsonPropertyName("submaps_options")]
    public SubmapsOptions2D SubmapsOptions { get; set; }

    /// <summary>
    /// True if IMU data should be expected and used.
    /// </summary>
    [JsonPropertyName("use_imu_data")]
    public bool UseImuData { get; set; }

    /// <summary>
    /// Adaptive voxel filter options for 2D.
    /// Used instead of fixed voxel_filter_size when configured.
    /// </summary>
    [JsonPropertyName("adaptive_voxel_filter_options")]
    public Models.Sensor.AdaptiveVoxelFilterOptions AdaptiveVoxelFilterOptions { get; set; }

    /// <summary>
    /// Voxel filter used to compute a sparser point cloud for finding loop closures.
    /// </summary>
    [JsonPropertyName("loop_closure_adaptive_voxel_filter_options")]
    public Models.Sensor.AdaptiveVoxelFilterOptions LoopClosureAdaptiveVoxelFilterOptions { get; set; }

    /// <summary>
    /// Time constant in seconds for the orientation moving average based on
    /// observed gravity via the IMU. It should be chosen so that the error
    /// 1. from acceleration measurements not due to gravity (which gets worse when
    /// the constant is reduced) and
    /// 2. from integration of angular velocities (which gets worse when the
    /// constant is increased) is balanced.
    /// TODO(schwoere,wohe): Remove this constant. This is only used for ROS and
    /// was replaced by pose_extrapolator_options.
    /// </summary>
    [JsonPropertyName("imu_gravity_time_constant")]
    public double ImuGravityTimeConstant { get; set; }

    [JsonPropertyName("pose_extrapolator_options")]
    public PoseExtrapolatorOptions PoseExtrapolatorOptions { get; set; }

    /// <summary>
    /// Scan matcher options used when applying a pending initial pose (SetInitialPose called mid-run).
    /// The next accumulated range data uses this pose as prediction and matches with wider search and stricter Ceres.
    /// Configured with wider search windows for relocalization scenarios.
    /// </summary>
    [JsonPropertyName("initial_pose_real_time_correlative_scan_matcher_options")]
    public RealTimeCorrelativeScanMatcherOptions InitialPoseRealTimeCorrelativeScanMatcherOptions { get; set; }

    [JsonPropertyName("initial_pose_ceres_scan_matcher_options")]
    public CeresScanMatcherOptions2D InitialPoseCeresScanMatcherOptions { get; set; }

    /// <summary>
    /// If true, provides a confidence score for the pose estimate based on
    /// real-time correlative scan matching.
    /// Match C++: provide_confidence_score option in LocalTrajectoryBuilderOptions2D
    /// </summary>
    [JsonPropertyName("provide_confidence_score")]
    public bool ProvideConfidenceScore { get; set; }

    /// <summary>
    /// Soft limit for Ceres scan match cost. When ceresScore exceeds this threshold
    /// but is below HardLimit, the pose is trusted (AddPose is called) but the scan
    /// is NOT inserted into the submap to avoid corrupting the map.
    /// Set to 0 to disable. Default: 0 (disabled).
    /// DEPRECATED: Use ProbabilityGridCeresScoreSoftLimit / TsdfCeresScoreSoftLimit instead.
    /// Kept as fallback when per-grid-type values are not set (both == 0).
    /// </summary>
    [JsonPropertyName("ceres_score_soft_limit")]
    public double CeresScoreSoftLimit { get; set; }

    /// <summary>
    /// Hard limit for Ceres scan match cost. When ceresScore exceeds this threshold,
    /// the scan-matched pose is considered unreliable: neither AddPose nor InsertIntoSubmap
    /// is called. The odometry prediction is used instead.
    /// Set to 0 to disable. Default: 0 (disabled).
    /// Must be >= CeresScoreSoftLimit when both are > 0.
    /// DEPRECATED: Use ProbabilityGridCeresScoreHardLimit / TsdfCeresScoreHardLimit instead.
    /// Kept as fallback when per-grid-type values are not set (both == 0).
    /// </summary>
    [JsonPropertyName("ceres_score_hard_limit")]
    public double CeresScoreHardLimit { get; set; }

    /// <summary>
    /// After this many consecutive hard-limit failures, forces creation of a new submap
    /// at the current odometry-predicted position. This breaks the deadlock when the robot
    /// enters a genuinely new area that cannot match any existing submap.
    /// Set to 0 to disable. Default: 5.
    /// </summary>
    [JsonPropertyName("max_consecutive_high_cost_before_new_submap")]
    public int MaxConsecutiveHighCostBeforeNewSubmap { get; set; }

    public LocalTrajectoryBuilderOptions2D(
        double minRange = 0.0,
        double maxRange = 30.0,
        double minZ = -0.8f,
        double maxZ = 2.0,
        double missingDataRayLength = 5.0,
        int numAccumulatedRangeData = 1,
        double voxelFilterSize = 0.025f,
        bool useOnlineCorrelativeScanMatching = true,
        RealTimeCorrelativeScanMatcherOptions realTimeCorrelativeScanMatcherOptions = default,
        CeresScanMatcherOptions2D ceresScanMatcherOptions = default,
        MotionFilterOptions motionFilterOptions = default,
        SubmapsOptions2D submapsOptions = default,
        bool useImuData = false,
        AdaptiveVoxelFilterOptions adaptiveVoxelFilterOptions = default,
        AdaptiveVoxelFilterOptions loopClosureAdaptiveVoxelFilterOptions = default,
        double imuGravityTimeConstant = 10.0,
        PoseExtrapolatorOptions poseExtrapolatorOptions = default,
        RealTimeCorrelativeScanMatcherOptions initialPoseRealTimeCorrelativeScanMatcherOptions = default,
        CeresScanMatcherOptions2D initialPoseCeresScanMatcherOptions = default,
        bool provideConfidenceScore = false,
        double ceresScoreSoftLimit = 0,
        double ceresScoreHardLimit = 0,
        int maxConsecutiveHighCostBeforeNewSubmap = 5)
    {
        MinRange = minRange;
        MaxRange = maxRange;
        MinZ = minZ;
        MaxZ = maxZ;
        MissingDataRayLength = missingDataRayLength;
        NumAccumulatedRangeData = numAccumulatedRangeData;
        VoxelFilterSize = voxelFilterSize;
        UseOnlineCorrelativeScanMatching = useOnlineCorrelativeScanMatching;
        
        // Set default values if not provided (default struct check)
        RealTimeCorrelativeScanMatcherOptions = realTimeCorrelativeScanMatcherOptions.Equals(default(RealTimeCorrelativeScanMatcherOptions))
            ? new RealTimeCorrelativeScanMatcherOptions(linearSearchWindow: 0.1, angularSearchWindow: 0.2)
            : realTimeCorrelativeScanMatcherOptions;
            
        CeresScanMatcherOptions = ceresScanMatcherOptions.Equals(default(CeresScanMatcherOptions2D))
            ? new CeresScanMatcherOptions2D(occupiedSpaceWeight: 1.0, translationWeight: 10.0, rotationWeight: 1.0)
            : ceresScanMatcherOptions;
            
        MotionFilterOptions = motionFilterOptions.Equals(default(MotionFilterOptions))
            ? new MotionFilterOptions(maxTimeSeconds: 5.0, maxDistanceMeters: 0.2, maxAngleRadians: 0.1)
            : motionFilterOptions;
            
        SubmapsOptions = submapsOptions.Equals(default(SubmapsOptions2D))
            ? new SubmapsOptions2D(
                numRangeData: 90,
                new GridOptions2D(GridOptions2D.GridType.ProbabilityGrid, 0.05f),
                new RangeDataInserterOptions(RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D))
            : submapsOptions;
            
        UseImuData = useImuData;
        
        AdaptiveVoxelFilterOptions = adaptiveVoxelFilterOptions.Equals(default(AdaptiveVoxelFilterOptions))
            ? new AdaptiveVoxelFilterOptions(maxLength: 0.3, minNumPoints: 250, maxRange: 50.0)
            : adaptiveVoxelFilterOptions;
            
        LoopClosureAdaptiveVoxelFilterOptions = loopClosureAdaptiveVoxelFilterOptions.Equals(default(AdaptiveVoxelFilterOptions))
            ? new AdaptiveVoxelFilterOptions(maxLength: 0.9, minNumPoints: 100, maxRange: 50.0)
            : loopClosureAdaptiveVoxelFilterOptions;
            
        ImuGravityTimeConstant = imuGravityTimeConstant;
        
        PoseExtrapolatorOptions = poseExtrapolatorOptions.Equals(default(PoseExtrapolatorOptions))
            ? new PoseExtrapolatorOptions()
            : poseExtrapolatorOptions;
            
        InitialPoseRealTimeCorrelativeScanMatcherOptions = initialPoseRealTimeCorrelativeScanMatcherOptions.Equals(default(RealTimeCorrelativeScanMatcherOptions))
            ? new RealTimeCorrelativeScanMatcherOptions(linearSearchWindow: 0.5, angularSearchWindow: 0.5)
            : initialPoseRealTimeCorrelativeScanMatcherOptions;
            
        InitialPoseCeresScanMatcherOptions = initialPoseCeresScanMatcherOptions.Equals(default(CeresScanMatcherOptions2D))
            ? new CeresScanMatcherOptions2D(occupiedSpaceWeight: 20.0, translationWeight: 50.0, rotationWeight: 10.0)
            : initialPoseCeresScanMatcherOptions;

        ProvideConfidenceScore = provideConfidenceScore;
        CeresScoreSoftLimit = ceresScoreSoftLimit;
        CeresScoreHardLimit = ceresScoreHardLimit;
        MaxConsecutiveHighCostBeforeNewSubmap = maxConsecutiveHighCostBeforeNewSubmap;
    }
}