using RobotNet10.RobotApp.Shared;

namespace RobotNet10.RobotApp.SLAM.Cartographer;


public class MapCartographerInfo : MapInfo
{

    /// <summary>
    /// Cartographer configuration snapshot khi map được save
    /// Dùng để validate khi load map
    /// </summary>
    public MapCartographerConfigSnapshot? MapConfig { get; init; }

}

/// <summary>
/// Snapshot của CartographerConfiguration khi save map
/// Chỉ lưu các phần cần thiết để recreate MapBuilder
/// </summary>
public class MapCartographerConfigSnapshot
{
    /// <summary>
    /// Use 2D or 3D trajectory builder
    /// </summary>
    public bool Use2D { get; init; }

    /// <summary>
    /// MapBuilder configuration snapshot
    /// </summary>
    public MapCarographerBuilderConfigSnapshot MapBuilder { get; init; } = new();

    /// <summary>
    /// TrajectoryBuilder configuration snapshot (chỉ các phần quan trọng)
    /// </summary>
    public TrajectoryBuilderConfigSnapshot TrajectoryBuilder { get; init; } = new();
}

/// <summary>
/// Snapshot của MapBuilderConfiguration
/// </summary>
public class MapCarographerBuilderConfigSnapshot
{
    public bool? UseTrajectoryBuilder2D { get; init; }
    public bool? UseTrajectoryBuilder3D { get; init; }
    public int NumBackgroundThreads { get; init; }
    public int OptimizeEveryNNodes { get; init; }
    public double MatcherTranslationWeight { get; init; }
    public double MatcherRotationWeight { get; init; }
    public int? MaxNumFinalIterations { get; init; }
    public double? GlobalSamplingRatio { get; init; }
    public bool? LogResidualHistograms { get; init; }
    public double? GlobalConstraintSearchAfterNSeconds { get; init; }
    public bool EnableSingleTrajectoryLoopClosure { get; init; } = true;
    public double SingleTrajectoryLoopClosureDistanceThreshold { get; init; } = 3.0;
    public OptimizationProblemOptionsSnapshot OptimizationProblemOptions { get; init; } = new();
}

/// <summary>
/// Snapshot của OptimizationProblemOptions
/// </summary>
public class OptimizationProblemOptionsSnapshot
{
    public double HuberScale { get; init; }
    public double AccelerationWeight { get; init; }
    public double RotationWeight { get; init; }
    public double LocalSlamPoseTranslationWeight { get; init; }
    public double LocalSlamPoseRotationWeight { get; init; }
    public double OdometryTranslationWeight { get; init; }
    public double OdometryRotationWeight { get; init; }
    public double FixedFramePoseTranslationWeight { get; init; }
    public double FixedFramePoseRotationWeight { get; init; }
    public bool FixedFramePoseUseTolerantLoss { get; init; }
    public double FixedFramePoseTolerantLossParamA { get; init; }
    public double FixedFramePoseTolerantLossParamB { get; init; }
    public bool LogSolverSummary { get; init; }
    public int MaxNumIterations { get; init; }
}

/// <summary>
/// Snapshot của TrajectoryBuilderConfiguration (chỉ các phần quan trọng)
/// </summary>
public class TrajectoryBuilderConfigSnapshot
{
    public bool Use2D { get; init; }
    public double MinRange { get; init; }
    public double MaxRange { get; init; }
    public double? MinZ { get; init; }
    public double? MaxZ { get; init; }
    public double? MissingDataRayLength { get; init; }
    public double VoxelFilterSize { get; init; }
    public int NumAccumulatedRangeData { get; init; }
    public bool? UseImuData { get; init; }
    public bool UseOnlineCorrelativeScanMatching { get; init; }
}

/// <summary>
/// Result from loading map files (pbstream path and saved config)
/// Used to separate file loading from CartographerSharp MapBuilder creation
/// </summary>
public class MapCartographerLoadResult
{
    /// <summary>
    /// Đường dẫn đến file .pbstream
    /// </summary>
    public string PbstreamPath { get; init; } = string.Empty;

    /// <summary>
    /// Đường dẫn đến map folder
    /// </summary>
    public string MapPath { get; init; } = string.Empty;

    /// <summary>
    /// Cartographer configuration snapshot khi map được save (nếu có)
    /// </summary>
    public MapCartographerConfigSnapshot? SavedConfig { get; init; }
}