namespace RobotNet10.RobotApp.Motion;

/// <summary>
/// Configuration for Extended Kalman Filter
/// </summary>
public class EKFConfiguration
{
    /// <summary>
    /// Device ID của IMU sensor
    /// </summary>
    public string ImuDeviceId { get; set; } = string.Empty;

    /// <summary>
    /// Frame ID của odometry frame (thường là "odom")
    /// </summary>
    public string FrameId { get; set; } = "odom";

    /// <summary>
    /// Child frame ID (thường là "base_link")
    /// </summary>
    public string ChildFrameId { get; set; } = "base_link";

    /// <summary>
    /// Tần số cập nhật EKF (Hz)
    /// </summary>
    public double UpdateRateHz { get; set; } = 20.0;

    // Process noise covariance (diagonal values)

    /// <summary>
    /// Process noise cho position (m^2)
    /// </summary>
    public double ProcessNoisePosition { get; set; } = 0.01;

    /// <summary>
    /// Process noise cho orientation (rad^2)
    /// </summary>
    public double ProcessNoiseOrientation { get; set; } = 0.01;

    /// <summary>
    /// Process noise cho velocity (m^2/s^2 hoặc rad^2/s^2)
    /// </summary>
    public double ProcessNoiseVelocity { get; set; } = 0.1;

    // Measurement noise covariance

    /// <summary>
    /// Measurement noise cho position từ odometry (m^2)
    /// </summary>
    public double MeasurementNoisePosition { get; set; } = 0.05;

    /// <summary>
    /// Measurement noise cho orientation từ IMU (rad^2)
    /// </summary>
    public double MeasurementNoiseOrientation { get; set; } = 0.01;

    // Initial state uncertainty

    /// <summary>
    /// Initial uncertainty cho position (m^2)
    /// </summary>
    public double InitialPositionUncertainty { get; set; } = 0.1;

    /// <summary>
    /// Initial uncertainty cho orientation (rad^2)
    /// </summary>
    public double InitialOrientationUncertainty { get; set; } = 0.1;

    /// <summary>
    /// Initial uncertainty cho velocity (m^2/s^2 hoặc rad^2/s^2)
    /// </summary>
    public double InitialVelocityUncertainty { get; set; } = 0.5;

    // Slip Detection Configuration

    /// <summary>
    /// Enable wheel slip detection
    /// </summary>
    public bool EnableSlipDetection { get; set; } = true;

    /// <summary>
    /// Enable wheels-in-air detection (uses Z-axis IMU)
    /// </summary>
    public bool EnableWheelsInAirDetection { get; set; } = true;

    /// <summary>
    /// Enable adaptive noise scaling when slip detected
    /// If false, noise scale always stays at 1.0
    /// </summary>
    public bool EnableAdaptiveNoise { get; set; } = true;

    /// <summary>
    /// Enable slip event logging
    /// </summary>
    public bool EnableSlipLogging { get; set; } = true;

    /// <summary>
    /// Acceleration mismatch threshold for slip detection (m/s²)
    /// </summary>
    public double AccelerationMismatchThreshold { get; set; } = 0.5;

    /// <summary>
    /// Innovation threshold for slip detection (sigma/standard deviations)
    /// </summary>
    public double InnovationThreshold { get; set; } = 3.0;

    /// <summary>
    /// Maximum process noise scale factor when slip detected
    /// </summary>
    public double MaxNoiseScale { get; set; } = 10.0;

    /// <summary>
    /// Z-axis acceleration threshold for freefall detection (m/s²)
    /// Value close to 9.81 indicates wheels in air
    /// </summary>
    public double ZAxisFreefallThreshold { get; set; } = 0.15;

    /// <summary>
    /// Horizontal motion threshold for slip classification (m/s²)
    /// </summary>
    public double HorizontalMotionThreshold { get; set; } = 0.1;

    /// <summary>
    /// Process noise scale factor for wheels in air scenario
    /// Very high value effectively freezes position estimation
    /// </summary>
    public double WheelsInAirNoiseScale { get; set; } = 100.0;
}