/*
 * 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.Collections.Generic;
using System.Text.Json.Serialization;
using CartographerSharp.Mapping.D3D;
using CartographerSharp.Models.Sensor;
using CartographerSharp.Models.Transform;

namespace CartographerSharp.Models.Mapping;

/// <summary>
/// Protocol buffer representation of local trajectory builder options for 3D.
/// </summary>
public struct LocalTrajectoryBuilderOptions3D
{
    /// <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; }

    /// <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>
    /// Voxel filter used to compute a sparser point cloud for matching.
    /// </summary>
    [JsonPropertyName("high_resolution_adaptive_voxel_filter_options")]
    public AdaptiveVoxelFilterOptions? HighResolutionAdaptiveVoxelFilterOptions { get; set; }

    [JsonPropertyName("low_resolution_adaptive_voxel_filter_options")]
    public AdaptiveVoxelFilterOptions? LowResolutionAdaptiveVoxelFilterOptions { 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 CeresScanMatcherOptions3D? CeresScanMatcherOptions { get; set; }

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

    /// <summary>
    /// Time constant in seconds for the orientation moving average based on
    /// observed gravity via the IMU.
    /// </summary>
    [JsonPropertyName("imu_gravity_time_constant")]
    public double ImuGravityTimeConstant { get; set; }

    /// <summary>
    /// Number of histogram buckets for the rotational scan matcher.
    /// </summary>
    [JsonPropertyName("rotational_histogram_size")]
    public int RotationalHistogramSize { get; set; }

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

    [JsonPropertyName("initial_poses")]
    public List<TimestampedTransform>? InitialPoses { get; set; }

    [JsonPropertyName("initial_imu_data")]
    public List<Sensor.ImuData>? InitialImuData { get; set; }

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

    /// <summary>
    /// Whether to use Lidar intensities in Ceres Scan Matcher.
    /// </summary>
    [JsonPropertyName("use_intensities")]
    public bool UseIntensities { get; set; }

    public LocalTrajectoryBuilderOptions3D(
        double minRange = 0.0,
        double maxRange = 60.0,
        int numAccumulatedRangeData = 1,
        double voxelFilterSize = 0.15f,
        AdaptiveVoxelFilterOptions? highResolutionAdaptiveVoxelFilterOptions = null,
        AdaptiveVoxelFilterOptions? lowResolutionAdaptiveVoxelFilterOptions = null,
        bool useOnlineCorrelativeScanMatching = false,
        RealTimeCorrelativeScanMatcherOptions? realTimeCorrelativeScanMatcherOptions = null,
        CeresScanMatcherOptions3D? ceresScanMatcherOptions = null,
        MotionFilterOptions? motionFilterOptions = null,
        double imuGravityTimeConstant = 10.0,
        int rotationalHistogramSize = 120,
        PoseExtrapolatorOptions? poseExtrapolatorOptions = null,
        List<TimestampedTransform>? initialPoses = null,
        List<Sensor.ImuData>? initialImuData = null,
        SubmapsOptions3D? submapsOptions = null,
        bool useIntensities = false)
    {
        MinRange = minRange;
        MaxRange = maxRange;
        NumAccumulatedRangeData = numAccumulatedRangeData;
        VoxelFilterSize = voxelFilterSize;
        HighResolutionAdaptiveVoxelFilterOptions = highResolutionAdaptiveVoxelFilterOptions;
        LowResolutionAdaptiveVoxelFilterOptions = lowResolutionAdaptiveVoxelFilterOptions;
        UseOnlineCorrelativeScanMatching = useOnlineCorrelativeScanMatching;
        RealTimeCorrelativeScanMatcherOptions = realTimeCorrelativeScanMatcherOptions;
        CeresScanMatcherOptions = ceresScanMatcherOptions;
        MotionFilterOptions = motionFilterOptions ?? new MotionFilterOptions();
        ImuGravityTimeConstant = imuGravityTimeConstant;
        RotationalHistogramSize = rotationalHistogramSize;
        PoseExtrapolatorOptions = poseExtrapolatorOptions ?? new PoseExtrapolatorOptions();
        InitialPoses = initialPoses;
        InitialImuData = initialImuData;
        SubmapsOptions = submapsOptions ?? new SubmapsOptions3D();
        UseIntensities = useIntensities;
    }
}