BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Configuration/TrajectoryBuilderConfiguration.cs

namespace RobotNet10.RobotApp.SLAM.Cartographer;

/// <summary>
/// Trajectory builder configuration
/// </summary>
public class TrajectoryBuilderConfiguration
{
    /// <summary>
    /// Use 2D SLAM (true) or 3D SLAM (false)
    /// </summary>
    public bool Use2D { get; set; } = true;

    /// <summary>
    /// Minimum range for lidar points (meters)
    /// </summary>
    public double MinRange { get; set; } = 0.1;

    /// <summary>
    /// Maximum range for lidar points (meters)
    /// </summary>
    public double MaxRange { get; set; } = 30.0;

    /// <summary>
    /// Minimum Z coordinate for lidar points (meters). Points below this will be filtered out.
    /// </summary>
    public double? MinZ { get; set; }

    /// <summary>
    /// Maximum Z coordinate for lidar points (meters). Points above this will be filtered out.
    /// </summary>
    public double? MaxZ { get; set; }

    /// <summary>
    /// Points beyond 'max_range' will be inserted with this length as empty space (meters).
    /// </summary>
    public double? MissingDataRayLength { get; set; }

    /// <summary>
    /// Voxel filter size (meters)
    /// </summary>
    public double VoxelFilterSize { get; set; } = 0.025;

    /// <summary>
    /// Number of accumulated range data before processing
    /// </summary>
    public int NumAccumulatedRangeData { get; set; } = 1;

    /// <summary>
    /// Use IMU data if available. If null, will be auto-detected from sensor configuration.
    /// </summary>
    public bool? UseImuData { get; set; }

    /// <summary>
    /// Use online correlative scan matching
    /// </summary>
    public bool UseOnlineCorrelativeScanMatching { get; set; } = false;

    /// <summary>
    /// Real-time correlative scan matcher options
    /// </summary>
    public RealTimeCorrelativeScanMatcherOptionsConfiguration? RealTimeCorrelativeScanMatcherOptions { get; set; }

    /// <summary>
    /// Ceres scan matcher options for 2D
    /// </summary>
    public CeresScanMatcherOptions2DConfiguration? CeresScanMatcherOptions { get; set; }

    /// <summary>
    /// Motion filter options
    /// </summary>
    public MotionFilterOptionsConfiguration? MotionFilterOptions { get; set; }

    /// <summary>
    /// Adaptive voxel filter options (optional, if not provided uses fixed voxel_filter_size)
    /// </summary>
    public AdaptiveVoxelFilterOptionsConfiguration? AdaptiveVoxelFilterOptions { get; set; }

    /// <summary>
    /// Loop closure adaptive voxel filter options (optional)
    /// Used to compute a sparser point cloud for finding loop closures.
    /// If not provided, uses the same filter as AdaptiveVoxelFilterOptions or fixed voxel_filter_size.
    /// </summary>
    public AdaptiveVoxelFilterOptionsConfiguration? LoopClosureAdaptiveVoxelFilterOptions { get; set; }

    /// <summary>
    /// Pose extrapolator options
    /// </summary>
    public PoseExtrapolatorOptionsConfiguration? PoseExtrapolatorOptions { get; set; }

    /// <summary>
    /// Debug options for scan matching convergence
    /// </summary>
    public ScanMatchingDebugOptionsConfiguration? ScanMatchingDebugOptions { get; set; }

    /// <summary>
    /// Submaps options for 2D
    /// </summary>
    public SubmapsOptions2DConfiguration? SubmapsOptions { get; set; }

    /// <summary>
    /// High resolution grid options for 2D (optional)
    /// Used for higher precision scan matching
    /// </summary>
    public GridOptions2DConfiguration? HighResGridOptions { get; set; }

    /// <summary>
    /// High resolution adaptive voxel filter options (optional)
    /// Used to filter point cloud for high resolution grid
    /// </summary>
    public AdaptiveVoxelFilterOptionsConfiguration? HighResAdaptiveVoxelFilterOptions { get; set; }

    /// <summary>
    /// High resolution voxel filter size (meters)
    /// Used when HighResAdaptiveVoxelFilterOptions is not provided
    /// </summary>
    public double? HighResVoxelFilterSize { get; set; }

    /// <summary>
    /// Landmark threshold options (optional)
    /// Used for landmark-based localization
    /// </summary>
    public LandmarkThresholdOptionsConfiguration? LandmarkThreshold { get; set; }

    /// <summary>
    /// Number of duplicate inserts to accurate submap
    /// </summary>
    public int? NumDuplicateInsertToAccurateSubmap { get; set; }

    /// <summary>
    /// True score cell threshold
    /// Threshold for determining if a cell has a true score
    /// </summary>
    public double? TrueScoreCellThreshold { get; set; }

    /// <summary>
    /// If true, provides a confidence score for the pose estimate based on
    /// real-time correlative scan matching.
    /// </summary>
    public bool ProvideConfidenceScore { get; set; } = false;

    /// <summary>
    /// Soft limit for Ceres scan match cost. When exceeded, pose is trusted but
    /// scan is NOT inserted into submap (prevents map corruption).
    /// Set to 0 to disable. Default: 0 (disabled).
    /// </summary>
    public double CeresScoreSoftLimit { get; set; } = 0;

    /// <summary>
    /// Hard limit for Ceres scan match cost. When exceeded, the pose is considered
    /// unreliable and odometry prediction is used instead.
    /// Set to 0 to disable. Default: 0 (disabled).
    /// </summary>
    public double CeresScoreHardLimit { get; set; } = 0;

    /// <summary>
    /// After this many consecutive hard-limit failures, a new submap is forced
    /// to break the map-growth deadlock.
    /// Set to 0 to disable. Default: 5.
    /// </summary>
    public int MaxConsecutiveHighCostBeforeNewSubmap { get; set; } = 5;

    /// <summary>
    /// Real-time correlative scan matcher options for initial pose (SetInitialPose).
    /// Uses wider search window for relocalization scenarios.
    /// </summary>
    public RealTimeCorrelativeScanMatcherOptionsConfiguration? InitialPoseRealTimeCorrelativeScanMatcherOptions { get; set; }

    /// <summary>
    /// Ceres scan matcher options for initial pose (SetInitialPose).
    /// Uses stricter weights for better accuracy in relocalization scenarios.
    /// </summary>
    public CeresScanMatcherOptions2DConfiguration? InitialPoseCeresScanMatcherOptions { get; set; }
}

#region Scan Matching Options

/// <summary>
/// Ceres scan matcher options for 2D
/// </summary>
public class CeresScanMatcherOptions2DConfiguration
{
    public double OccupiedSpaceWeight { get; set; } = 1.0;
    public double TranslationWeight { get; set; } = 10.0;
    public double RotationWeight { get; set; } = 40.0;
    public double? HighResOccupiedSpaceWeight { get; set; }
    public double? HighResTranslationWeight { get; set; }
    public double? HighResRotationWeight { get; set; }
    public double? LandmarkWeight { get; set; }
    public CeresSolverOptionsConfiguration? CeresSolverOptions { get; set; }
}

/// <summary>
/// Real-time correlative scan matcher options
/// </summary>
public class RealTimeCorrelativeScanMatcherOptionsConfiguration
{
    public double LinearSearchWindow { get; set; } = 0.1;
    public double AngularSearchWindow { get; set; } = 0.349066; // ~20 degrees
    public double TranslationDeltaCostWeight { get; set; } = 0.1;
    public double RotationDeltaCostWeight { get; set; } = 0.1;
    public int NumThreads { get; set; } = 1;
}

/// <summary>
/// Fast correlative scan matcher options for 2D
/// </summary>
public class FastCorrelativeScanMatcherOptions2DConfiguration
{
    public double LinearSearchWindow { get; set; } = 7.0;
    public double AngularSearchWindow { get; set; } = 0.523599; // ~30 degrees
    public int BranchAndBoundDepth { get; set; } = 7;
    public double? LocalizationLinearSearchWindow { get; set; }
    public double? LocalizationAngularSearchWindow { get; set; }
}

/// <summary>
/// Ceres solver options
/// </summary>
public class CeresSolverOptionsConfiguration
{
    public bool UseNonmonotonicSteps { get; set; } = false;
    public int MaxNumIterations { get; set; } = 20;
    public int NumThreads { get; set; } = 1;
    public double? FunctionTolerance { get; set; }
    public double? GradientTolerance { get; set; }
    public double? ParameterTolerance { get; set; }
}

#endregion

#region Motion and Filter Options

/// <summary>
/// Motion filter options
/// </summary>
public class MotionFilterOptionsConfiguration
{
    public double MaxTimeSeconds { get; set; } = 5.0;
    public double MaxDistanceMeters { get; set; } = 0.2;
    public double MaxAngleRadians { get; set; } = 0.0174533; // ~1 degree
}

/// <summary>
/// Adaptive voxel filter options
/// </summary>
public class AdaptiveVoxelFilterOptionsConfiguration
{
    public double MaxLength { get; set; } = 0.5;
    public double MinNumPoints { get; set; } = 200;
    public double MaxRange { get; set; } = 50.0;
}

#endregion

#region Submap Options

/// <summary>
/// Submaps options for 2D
/// </summary>
public class SubmapsOptions2DConfiguration
{
    public int NumRangeData { get; set; } = 90;
    public GridOptions2DConfiguration GridOptions { get; set; } = new();
    public RangeDataInserterOptionsConfiguration RangeDataInserterOptions { get; set; } = new();
}

/// <summary>
/// Grid options for 2D
/// </summary>
public class GridOptions2DConfiguration
{
    public int GridType { get; set; } = 1; // ProbabilityGrid
    public double Resolution { get; set; } = 0.05;
}

/// <summary>
/// Range data inserter options
/// </summary>
public class RangeDataInserterOptionsConfiguration
{
    public int RangeDataInserterType { get; set; } = 1; // ProbabilityGridInserter2D
    public ProbabilityGridRangeDataInserterOptions2DConfiguration? ProbabilityGridRangeDataInserterOptions { get; set; }
    public TsdfRangeDataInserterOptions2DConfiguration? TsdfRangeDataInserterOptions { get; set; }
}

/// <summary>
/// TSDF range data inserter options for 2D
/// </summary>
public class TsdfRangeDataInserterOptions2DConfiguration
{
    public double TruncationDistance { get; set; } = 0.3;
    public double MaximumWeight { get; set; } = 10.0;
    public bool UpdateFreeSpace { get; set; } = false;
    public NormalEstimationOptions2DConfiguration NormalEstimationOptions { get; set; } = new();
    public bool ProjectSdfDistanceToScanNormal { get; set; } = true;
    public int UpdateWeightRangeExponent { get; set; } = 0;
    public double UpdateWeightAngleScanNormalToRayKernelBandwidth { get; set; } = 0.5;
    public double UpdateWeightDistanceCellToHitKernelBandwidth { get; set; } = 0.5;
}

/// <summary>
/// Normal estimation options for 2D TSDF
/// </summary>
public class NormalEstimationOptions2DConfiguration
{
    public int NumNormalSamples { get; set; } = 4;
    public double SampleRadius { get; set; } = 0.5;
}

/// <summary>
/// Probability grid range data inserter options for 2D
/// </summary>
public class ProbabilityGridRangeDataInserterOptions2DConfiguration
{
    public double HitProbability { get; set; } = 0.55;
    public double MissProbability { get; set; } = 0.49;
    public bool InsertFreeSpace { get; set; } = true;
}

/// <summary>
/// Overlapping submaps trimmer options for 2D
/// </summary>
public class OverlappingSubmapsTrimmerOptions2DConfiguration
{
    public int FreshSubmapsCount { get; set; } = 2;
    public double MinCoveredArea { get; set; } = 1.0;
    public int MinAddedSubmapsCount { get; set; } = 5;
}

#endregion

#region Pose Extrapolator Options

/// <summary>
/// Pose extrapolator options
/// </summary>
public class PoseExtrapolatorOptionsConfiguration
{
    public bool UseImuBased { get; set; } = false;
    public ConstantVelocityPoseExtrapolatorOptionsConfiguration ConstantVelocity { get; set; } = new();
    public ImuBasedPoseExtrapolatorOptionsConfiguration? ImuBased { get; set; }
    public double? VelocityThreshold { get; set; }
    public double? AccelerationThreshold { get; set; }
    public double? GravityDeviationThreshold { get; set; }
    public bool? UseOdometryDirectly { get; set; }
}

/// <summary>
/// Constant velocity pose extrapolator options
/// </summary>
public class ConstantVelocityPoseExtrapolatorOptionsConfiguration
{
    public double ImuGravityTimeConstant { get; set; } = 10.0;
    public double PoseQueueDuration { get; set; } = 0.001;
}

/// <summary>
/// IMU-based pose extrapolator options
/// </summary>
public class ImuBasedPoseExtrapolatorOptionsConfiguration
{
    public double PoseQueueDuration { get; set; } = 5.0;
    public double GravityConstant { get; set; } = 9.806;
    public double PoseTranslationWeight { get; set; } = 1.0;
    public double PoseRotationWeight { get; set; } = 1.0;
    public double ImuAccelerationWeight { get; set; } = 1.0;
    public double ImuRotationWeight { get; set; } = 1.0;
    public double OdometryTranslationWeight { get; set; } = 1.0;
    public double OdometryRotationWeight { get; set; } = 1.0;
    public CeresSolverOptionsConfiguration? SolverOptions { get; set; }
}

#endregion

#region Debug Options

/// <summary>
/// Debug options for scan matching convergence
/// </summary>
public class ScanMatchingDebugOptionsConfiguration
{
    public bool EnableDebugMode { get; set; } = false;
    public double MinCostReductionPercent { get; set; } = 80.0;
    public double MaxPoseChangeDistanceMeters { get; set; } = 0.5;
    public double MaxPoseChangeRotationDegrees { get; set; } = 5.0;
}

/// <summary>
/// Landmark threshold options
/// </summary>
public class LandmarkThresholdOptionsConfiguration
{
    public double MirrorLandmarkMatchingDistance { get; set; } = 0.5;
    public double NewLandmarkDistance { get; set; } = 2.0;
    public int LocalLandmarksHistory { get; set; } = 80;
}

#endregion