Denso/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Configuration/OccupancyGridConfiguration.cs

namespace RobotNet10.RobotApp.SLAM.Cartographer;

/// <summary>
/// Strategy for merging multiple submaps into a single occupancy grid.
/// </summary>
public enum SubmapMergeStrategy
{
    /// <summary>
    /// Porter-Duff Source-Over compositing (Cairo-style).
    /// Matches original Cartographer C++ behavior.
    /// </summary>
    PorterDuff,

    /// <summary>
    /// Sum log-odds from all submaps (Bayesian approach).
    /// Provides clearer free/occupied distinction with multiple observations.
    /// LogOdds = Σ log(p_i / (1 - p_i)), then convert back to probability.
    /// </summary>
    LogOddsSum,

    /// <summary>
    /// Take maximum probability (most pessimistic/conservative).
    /// Good for navigation safety - any occupied observation dominates.
    /// </summary>
    MaxProbability
}

/// <summary>
/// Configuration for occupancy grid generation and filtering.
/// Controls how probability values are converted to occupancy values (0=free, 100=occupied, -1=unknown).
/// </summary>
public class OccupancyGridConfiguration
{
    #region Merge Strategy

    /// <summary>
    /// Strategy for merging overlapping cells from multiple submaps.
    /// Default: LogOddsSum (clearer free/occupied distinction)
    /// </summary>
    public SubmapMergeStrategy MergeStrategy { get; set; } = SubmapMergeStrategy.LogOddsSum;

    /// <summary>
    /// Clamp log-odds to prevent extreme values from dominating.
    /// Range: [1, 20], Default: 10 (corresponds to probability ~0.00005 to ~0.99995)
    /// </summary>
    public double LogOddsClamp { get; set; } = 10.0;

    /// <summary>
    /// When true, use average log-odds instead of sum.
    /// This prevents amplification when a cell is observed by many submaps.
    /// Default: true (average is more stable for visualization)
    /// </summary>
    public bool UseLogOddsAverage { get; set; } = true;

    #endregion

    #region Threshold Configuration

    /// <summary>
    /// Threshold for classifying a cell as FREE (white, occupancy=0).
    /// In texture-based conversion: textureValue >= FreeSpaceThreshold → FREE
    /// Higher value = stricter (fewer free cells), Lower value = more permissive (more free cells).
    /// Range: [0, 255], Default: 100
    ///
    /// Technical detail:
    /// - textureValue = max(0, 128 - logOddsInteger)
    /// - logOddsInteger maps probability [0.1, 0.9] to [1, 255]
    /// - FreeSpaceThreshold=100 requires probability ≤ ~0.15 (very confident free)
    /// - FreeSpaceThreshold=50 requires probability ≤ ~0.30 (moderately confident free)
    /// </summary>
    public int FreeSpaceThreshold { get; set; } = 100;

    /// <summary>
    /// Threshold for classifying a cell as OCCUPIED (black, occupancy=100).
    /// In texture-based conversion: textureAlpha > OccupiedSpaceThreshold → OCCUPIED
    /// Higher value = stricter (fewer occupied cells, thinner walls), Lower value = more permissive.
    /// Range: [0, 255], Default: 0
    ///
    /// Technical detail:
    /// - textureAlpha = max(0, logOddsInteger - 128)
    /// - OccupiedSpaceThreshold=0 requires probability > 0.5 (any occupied tendency)
    /// - OccupiedSpaceThreshold=50 requires probability > ~0.70 (confident occupied)
    /// - OccupiedSpaceThreshold=100 requires probability > ~0.85 (very confident occupied)
    /// </summary>
    public int OccupiedSpaceThreshold { get; set; } = 0;

    #endregion

    #region Output Mode

    /// <summary>
    /// When true, output only binary values (0=free, 100=occupied, -1=unknown).
    /// When false, output gradient values (0-100) based on probability.
    /// Default: true (binary output for compatibility with most navigation systems).
    /// </summary>
    public bool UseBinaryOutput { get; set; } = true;

    #endregion

    #region Wall Thinning (Post-processing)

    /// <summary>
    /// Enable morphological erosion to thin walls in the occupancy grid.
    /// Useful for reducing wall thickness caused by sensor noise or multiple observations.
    /// Default: false
    /// </summary>
    public bool EnableWallThinning { get; set; } = false;

    /// <summary>
    /// Number of erosion iterations for wall thinning.
    /// Each iteration removes one pixel layer from occupied regions.
    /// Higher value = thinner walls, but may disconnect thin walls.
    /// Range: [1, 5], Default: 1
    /// </summary>
    public int WallThinningIterations { get; set; } = 1;

    /// <summary>
    /// Minimum wall thickness to preserve (in pixels) during wall thinning.
    /// Walls thinner than this will not be eroded further.
    /// Range: [1, 10], Default: 1
    /// </summary>
    public int MinWallThicknessPixels { get; set; } = 1;

    #endregion

    #region Ambiguous Cell Handling

    /// <summary>
    /// How to handle ambiguous cells (probability ~0.5, neither clearly free nor occupied).
    /// Values: -1 = Unknown, 0 = Free, 100 = Occupied
    /// Default: -1 (mark as unknown)
    ///
    /// Note: This applies when UseBinaryOutput=true and the cell doesn't meet
    /// either FreeSpaceThreshold or OccupiedSpaceThreshold.
    /// </summary>
    public sbyte AmbiguousCellValue { get; set; } = -1;

    /// <summary>
    /// Lower bound of the ambiguous range (probability).
    /// Cells with probability between AmbiguousRangeLower and AmbiguousRangeUpper
    /// are considered ambiguous and handled according to AmbiguousCellValue.
    /// Default: 0.35 (corresponding to ~neither free nor occupied)
    /// </summary>
    public double AmbiguousRangeLower { get; set; } = 0.35;

    /// <summary>
    /// Upper bound of the ambiguous range (probability).
    /// Default: 0.65
    /// </summary>
    public double AmbiguousRangeUpper { get; set; } = 0.65;

    #endregion

    #region Advanced Options

    /// <summary>
    /// Apply median filter to reduce noise in the occupancy grid.
    /// Useful for removing salt-and-pepper noise.
    /// Default: false
    /// </summary>
    public bool EnableMedianFilter { get; set; } = false;

    /// <summary>
    /// Kernel size for median filter (must be odd number).
    /// Range: [3, 7], Default: 3
    /// </summary>
    public int MedianFilterKernelSize { get; set; } = 3;

    #endregion

    #region TSDF-Specific Options

    /// <summary>
    /// TSD threshold for classifying a cell as FREE space (meters).
    /// Cells with TSD > TsdfFreeThreshold are considered free.
    /// Lower value = more aggressive free space detection (closer to walls).
    /// Range: [0.01, 0.3], Default: 0.05 (5cm from surface)
    /// </summary>
    public double TsdfFreeThreshold { get; set; } = 0.05;

    /// <summary>
    /// TSD threshold for classifying a cell as OCCUPIED (meters, negative value).
    /// Cells with TSD < TsdfOccupiedThreshold are considered occupied.
    /// Higher value (closer to 0) = more aggressive obstacle detection.
    /// Range: [-0.3, 0], Default: -0.02 (2cm inside surface)
    /// </summary>
    public double TsdfOccupiedThreshold { get; set; } = -0.02;

    /// <summary>
    /// Minimum weight required for a TSDF cell to be considered valid.
    /// Cells with weight < TsdfMinWeight are skipped (treated as unknown).
    /// Lower value = include more cells but with less confidence.
    /// Range: [0.01, 5.0], Default: 0.1
    /// </summary>
    public double TsdfMinWeight { get; set; } = 0.1;

    /// <summary>
    /// Maximum TSD value for normalization (meters).
    /// Should match the TruncationDistance in TsdfRangeDataInserterOptions.
    /// Used to convert TSD to probability: probability = 0.5 * (1 - tsd/TsdfMaxTsd)
    /// Range: [0.1, 1.0], Default: 0.3
    /// </summary>
    public double TsdfMaxTsd { get; set; } = 0.3;

    #endregion
}