/*
 * 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.Models.Common;

namespace CartographerSharp.Models.Mapping;

/// <summary>
/// Intensity cost function options for 3D scan matching.
/// </summary>
public struct IntensityCostFunctionOptions3D
{
    [JsonPropertyName("weight")]
    public double Weight { get; set; }

    [JsonPropertyName("huber_scale")]
    public double HuberScale { get; set; }

    [JsonPropertyName("intensity_threshold")]
    public double IntensityThreshold { get; set; }

    public IntensityCostFunctionOptions3D(double weight, double huberScale, double intensityThreshold)
    {
        Weight = weight;
        HuberScale = huberScale;
        IntensityThreshold = intensityThreshold;
    }
}

/// <summary>
/// Protocol buffer representation of Ceres scan matcher options for 3D.
/// </summary>
public struct CeresScanMatcherOptions3D
{
    /// <summary>
    /// Occupied space weights for each point cloud/grid pair.
    /// </summary>
    [JsonPropertyName("occupied_space_weight")]
    public List<double> OccupiedSpaceWeight { get; set; }

    /// <summary>
    /// Intensity cost function options for each point cloud/grid pair.
    /// </summary>
    [JsonPropertyName("intensity_cost_function_options")]
    public List<IntensityCostFunctionOptions3D> IntensityCostFunctionOptions { get; set; }

    /// <summary>
    /// Translation weight.
    /// </summary>
    [JsonPropertyName("translation_weight")]
    public double TranslationWeight { get; set; }

    /// <summary>
    /// Rotation weight.
    /// </summary>
    [JsonPropertyName("rotation_weight")]
    public double RotationWeight { get; set; }

    /// <summary>
    /// Only optimize yaw (ignore roll and pitch).
    /// </summary>
    [JsonPropertyName("only_optimize_yaw")]
    public bool OnlyOptimizeYaw { get; set; }

    /// <summary>
    /// Ceres solver options.
    /// </summary>
    [JsonPropertyName("ceres_solver_options")]
    public CeresSolverOptions? CeresSolverOptions { get; set; }

    public CeresScanMatcherOptions3D(
        List<double>? occupiedSpaceWeight = null,
        List<IntensityCostFunctionOptions3D>? intensityCostFunctionOptions = null,
        double translationWeight = 5.0,
        double rotationWeight = 200.0,
        bool onlyOptimizeYaw = false,
        CeresSolverOptions? ceresSolverOptions = null)
    {
        OccupiedSpaceWeight = occupiedSpaceWeight ?? new List<double> { 5.0 };
        IntensityCostFunctionOptions = intensityCostFunctionOptions ?? new List<IntensityCostFunctionOptions3D>();
        TranslationWeight = translationWeight;
        RotationWeight = rotationWeight;
        OnlyOptimizeYaw = onlyOptimizeYaw;
        CeresSolverOptions = ceresSolverOptions;
    }
}