/*
 * Copyright 2018 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 extrapolator options.
/// </summary>
public struct PoseExtrapolatorOptions
{
    [JsonPropertyName("use_imu_based")]
    public bool UseImuBased { get; set; }

    [JsonPropertyName("constant_velocity")]
    public ConstantVelocityPoseExtrapolatorOptions ConstantVelocity { get; set; }

    [JsonPropertyName("imu_based")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public ImuBasedPoseExtrapolatorOptions? ImuBased { get; set; }

    /// <summary>
    /// Velocity threshold for detecting robot movement (m/s).
    /// Robot is considered "moving" if horizontal velocity exceeds this threshold.
    /// Default: 0.003 m/s (3 mm/s)
    /// </summary>
    [JsonPropertyName("velocity_threshold")]
    public double? VelocityThreshold { get; set; }

    /// <summary>
    /// Acceleration threshold for detecting IMU acceleration (m/s²).
    /// IMU is considered "accelerating" if horizontal acceleration exceeds this threshold.
    /// Default: 0.4 m/s²
    /// </summary>
    [JsonPropertyName("acceleration_threshold")]
    public double? AccelerationThreshold { get; set; }

    /// <summary>
    /// Gravity deviation threshold for detecting vertical acceleration changes (m/s²).
    /// Used to detect changes in vertical acceleration (gravity deviation).
    /// Default: 0.1 m/s²
    /// </summary>
    [JsonPropertyName("gravity_deviation_threshold")]
    public double? GravityDeviationThreshold { get; set; }

    /// <summary>
    /// If true, use odometry pose directly at the time of range data instead of calculating velocity.
    /// This is useful when odometry data is accurate and reliable.
    /// When enabled, ExtrapolatePose will use the latest odometry data at or before the requested time,
    /// instead of extrapolating from the last pose using calculated velocity.
    /// Default: false (use velocity-based extrapolation)
    /// </summary>
    [JsonPropertyName("use_odometry_directly")]
    public bool UseOdometryDirectly { get; set; }

    public PoseExtrapolatorOptions(
        bool useImuBased = false,
        ConstantVelocityPoseExtrapolatorOptions? constantVelocity = null,
        ImuBasedPoseExtrapolatorOptions? imuBased = null,
        double? velocityThreshold = null,
        double? accelerationThreshold = null,
        double? gravityDeviationThreshold = null,
        bool useOdometryDirectly = false)
    {
        UseImuBased = useImuBased;
        ConstantVelocity = constantVelocity ?? new ConstantVelocityPoseExtrapolatorOptions();
        ImuBased = imuBased;
        VelocityThreshold = velocityThreshold;
        AccelerationThreshold = accelerationThreshold;
        GravityDeviationThreshold = gravityDeviationThreshold;
        UseOdometryDirectly = useOdometryDirectly;
    }
}

/// <summary>
/// Constant velocity pose extrapolator options.
/// </summary>
public struct ConstantVelocityPoseExtrapolatorOptions
{
    /// <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; }

    [JsonPropertyName("pose_queue_duration")]
    public double PoseQueueDuration { get; set; }

    public ConstantVelocityPoseExtrapolatorOptions(
        double imuGravityTimeConstant = 10.0,
        double poseQueueDuration = 0.001)
    {
        ImuGravityTimeConstant = imuGravityTimeConstant;
        PoseQueueDuration = poseQueueDuration;
    }
}