namespace RobotNet10.RobotApp.SLAM.Cartographer;

/// <summary>
/// Map builder configuration
/// </summary>
public class MapBuilderConfiguration
{
    public bool? UseTrajectoryBuilder2D { get; set; }
    public bool? UseTrajectoryBuilder3D { get; set; }
    public int NumBackgroundThreads { get; set; } = 4;
    public int OptimizeEveryNNodes { get; set; } = 90;
    public double MatcherTranslationWeight { get; set; } = 5e2;
    public double MatcherRotationWeight { get; set; } = 1.6e3;
    public int? MaxNumFinalIterations { get; set; }
    public double? GlobalSamplingRatio { get; set; }
    public bool? LogResidualHistograms { get; set; }
    public double? GlobalConstraintSearchAfterNSeconds { get; set; }
    public bool EnableSingleTrajectoryLoopClosure { get; set; } = true;
    public double SingleTrajectoryLoopClosureDistanceThreshold { get; set; } = 3.0;
    public OptimizationProblemOptions PoseGraphOptimizationProblemOptions { get; set; } = new();
    public ConstraintBuilderOptionsConfiguration? ConstraintBuilderOptions { get; set; }
    public OverlappingSubmapsTrimmerOptions2DConfiguration? OverlappingSubmapsTrimmer2D { get; set; }
    public bool CollateByTrajectory { get; set; } = false;
}

/// <summary>
/// Optimization problem options
/// </summary>
public class OptimizationProblemOptions
{
    public double HuberScale { get; set; } = 1e1;
    public double OdometryHuberScale { get; set; } = 1e1;
    public double LocalSlamPoseHuberScale { get; set; } = 1e1;
    public double AccelerationWeight { get; set; } = 1.1e2;
    public double RotationWeight { get; set; } = 1.6e4;
    public double LocalSlamPoseTranslationWeight { get; set; } = 1e5;
    public double LocalSlamPoseRotationWeight { get; set; } = 1e5;
    public double OdometryTranslationWeight { get; set; } = 1e5;
    public double OdometryRotationWeight { get; set; } = 1e5;
    public double FixedFramePoseTranslationWeight { get; set; } = 1e1;
    public double FixedFramePoseRotationWeight { get; set; } = 1e2;
    public bool FixedFramePoseUseTolerantLoss { get; set; } = false;
    public double FixedFramePoseTolerantLossParamA { get; set; } = 1.0;
    public double FixedFramePoseTolerantLossParamB { get; set; } = 1.0;
    public bool LogSolverSummary { get; set; } = false;
    public int MaxNumIterations { get; set; } = 50;
    public CeresSolverOptionsConfiguration? CeresSolverOptions { get; set; }
}

/// <summary>
/// Map storage configuration
/// </summary>
public class MapStorageConfiguration
{
    public string Directory { get; set; } = "./maps";
    public double OccupancyGridResolution { get; set; } = 0.05;
    public double MapPadding { get; set; } = 1.0;
    public bool IncludeUnfinishedSubmaps { get; set; } = false;
    public string LocalizingStateDirectory { get; set; } = "./localizing_state";
    public double PoseSyncIntervalSeconds { get; set; } = 5.0;
    public PngVisualizationOptions PngVisualizationOptions { get; set; } = new();
}

/// <summary>
/// PNG visualization options
/// </summary>
public class PngVisualizationOptions
{
    public bool ShowGrid { get; set; } = true;
    public bool ShowTrajectory { get; set; } = true;
    public string GridColor { get; set; } = "#CCCCCC";
    public string TrajectoryColor { get; set; } = "#FF0000";
}

/// <summary>
/// Constraint builder options configuration
/// </summary>
public class ConstraintBuilderOptionsConfiguration
{
    public double SamplingRatio { get; set; } = 0.3;
    public double MaxConstraintDistance { get; set; } = 15.0;
    public double MinScore { get; set; } = 0.55;
    public double GlobalLocalizationMinScore { get; set; } = 0.6;
    public double LoopClosureTranslationWeight { get; set; } = 1.1e4;
    public double LoopClosureRotationWeight { get; set; } = 1.0;
    public bool LogMatches { get; set; } = true;
    public FastCorrelativeScanMatcherOptions2DConfiguration? FastCorrelativeScanMatcherOptions { get; set; }
    public CeresScanMatcherOptions2DConfiguration? CeresScanMatcherOptions { get; set; }
}