using CartographerSharp.Mapping;
using CartographerSharp.Models.Mapping;
using static CartographerSharp.Mapping.PoseGraph;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

/// <summary>
/// Factory for creating TrajectoryBuilderOptions to avoid code duplication
/// </summary>
public static class TrajectoryBuilderOptionsFactory
{
    /// <summary>
    /// Creates 2D trajectory builder options from configuration.
    /// </summary>
    /// <param name="config">Trajectory builder configuration</param>
    /// <param name="useImuData">Whether to use IMU data</param>
    /// <param name="gridTypeOverride">Override grid type (null = use config value). ProbabilityGrid for ScanMapping, Tsdf for Localizing.</param>
    public static TrajectoryBuilderOptions Create2D(
        TrajectoryBuilderConfiguration config,
        bool useImuData,
        GridOptions2D.GridType? gridTypeOverride = null)
    {
        if (!config.Use2D)
        {
            throw new ArgumentException("Configuration must have Use2D = true", nameof(config));
        }

        // Use explicit UseImuData from config if provided, otherwise use parameter
        var finalUseImuData = config.UseImuData ?? useImuData;

        // Create RealTimeCorrelativeScanMatcherOptions
        RealTimeCorrelativeScanMatcherOptions? realTimeCorrelativeScanMatcherOptions = null;
        if (config.RealTimeCorrelativeScanMatcherOptions != null)
        {
            var rtcConfig = config.RealTimeCorrelativeScanMatcherOptions;
            realTimeCorrelativeScanMatcherOptions = new RealTimeCorrelativeScanMatcherOptions(
                linearSearchWindow: rtcConfig.LinearSearchWindow,
                angularSearchWindow: rtcConfig.AngularSearchWindow,
                translationDeltaCostWeight: rtcConfig.TranslationDeltaCostWeight,
                rotationDeltaCostWeight: rtcConfig.RotationDeltaCostWeight,
                numThreads: rtcConfig.NumThreads);
        }

        // Create CeresScanMatcherOptions2D
        CeresScanMatcherOptions2D? ceresScanMatcherOptions = null;
        if (config.CeresScanMatcherOptions != null)
        {
            var csConfig = config.CeresScanMatcherOptions;
            CartographerSharp.Models.Common.CeresSolverOptions? ceresSolverOptions = null;
            if (csConfig.CeresSolverOptions != null)
            {
                var solverConfig = csConfig.CeresSolverOptions;
                ceresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
                    useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
                    maxNumIterations: solverConfig.MaxNumIterations,
                    numThreads: solverConfig.NumThreads,
                    functionTolerance: solverConfig.FunctionTolerance,
                    gradientTolerance: solverConfig.GradientTolerance,
                    parameterTolerance: solverConfig.ParameterTolerance);
            }

            ceresScanMatcherOptions = new CeresScanMatcherOptions2D(
                occupiedSpaceWeight: csConfig.OccupiedSpaceWeight,
                translationWeight: csConfig.TranslationWeight,
                rotationWeight: csConfig.RotationWeight,
                ceresSolverOptions: ceresSolverOptions,
                highResOccupiedSpaceWeight: csConfig.HighResOccupiedSpaceWeight,
                highResTranslationWeight: csConfig.HighResTranslationWeight,
                highResRotationWeight: csConfig.HighResRotationWeight,
                landmarkWeight: csConfig.LandmarkWeight);
        }

        // Create MotionFilterOptions
        MotionFilterOptions? motionFilterOptions = null;
        if (config.MotionFilterOptions != null)
        {
            var mfConfig = config.MotionFilterOptions;
            motionFilterOptions = new MotionFilterOptions(
                maxTimeSeconds: mfConfig.MaxTimeSeconds,
                maxDistanceMeters: mfConfig.MaxDistanceMeters,
                maxAngleRadians: mfConfig.MaxAngleRadians);
        }

        // Create SubmapsOptions2D
        SubmapsOptions2D? submapsOptions = null;
        if (config.SubmapsOptions != null)
        {
            var subConfig = config.SubmapsOptions;

            // Determine effective grid type (override takes priority over config)
            var configGridType = (GridOptions2D.GridType)subConfig.GridOptions.GridType;
            var effectiveGridType = gridTypeOverride ?? configGridType;

            var gridOptions = new GridOptions2D(
                gridType: effectiveGridType,
                resolution: subConfig.GridOptions.Resolution);

            // Create RangeDataInserterOptions
            RangeDataInserterOptions? rangeDataInserterOptions = null;
            if (subConfig.RangeDataInserterOptions != null)
            {
                var rdiConfig = subConfig.RangeDataInserterOptions;

                // Determine inserter type to match effective grid type
                var effectiveInserterType = effectiveGridType switch
                {
                    GridOptions2D.GridType.ProbabilityGrid => RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D,
                    GridOptions2D.GridType.Tsdf => RangeDataInserterOptions.RangeDataInserterType.TsdfInserter2D,
                    _ => (RangeDataInserterOptions.RangeDataInserterType)rdiConfig.RangeDataInserterType
                };

                // Read ProbabilityGrid options (use configured or C++ defaults)
                ProbabilityGridRangeDataInserterOptions2D? probGridOptions = null;
                if (rdiConfig.ProbabilityGridRangeDataInserterOptions != null)
                {
                    var pgConfig = rdiConfig.ProbabilityGridRangeDataInserterOptions;
                    probGridOptions = new ProbabilityGridRangeDataInserterOptions2D(
                        hitProbability: pgConfig.HitProbability,
                        missProbability: pgConfig.MissProbability,
                        insertFreeSpace: pgConfig.InsertFreeSpace);
                }
                else if (effectiveGridType == GridOptions2D.GridType.ProbabilityGrid)
                {
                    // C++ Cartographer defaults
                    probGridOptions = new ProbabilityGridRangeDataInserterOptions2D(0.55, 0.49, true);
                }

                // Read TSDF options (use configured or C++ defaults)
                TSDFRangeDataInserterOptions2D? tsdfOptions = null;
                if (rdiConfig.TsdfRangeDataInserterOptions != null)
                {
                    var tsdfConfig = rdiConfig.TsdfRangeDataInserterOptions;
                    var normalEstOptions = new NormalEstimationOptions2D(
                        tsdfConfig.NormalEstimationOptions.NumNormalSamples,
                        tsdfConfig.NormalEstimationOptions.SampleRadius);
                    tsdfOptions = new TSDFRangeDataInserterOptions2D(
                        truncationDistance: tsdfConfig.TruncationDistance,
                        maximumWeight: tsdfConfig.MaximumWeight,
                        updateFreeSpace: tsdfConfig.UpdateFreeSpace,
                        normalEstimationOptions: normalEstOptions,
                        projectSdfDistanceToScanNormal: tsdfConfig.ProjectSdfDistanceToScanNormal,
                        updateWeightRangeExponent: tsdfConfig.UpdateWeightRangeExponent,
                        updateWeightAngleScanNormalToRayKernelBandwidth: tsdfConfig.UpdateWeightAngleScanNormalToRayKernelBandwidth,
                        updateWeightDistanceCellToHitKernelBandwidth: tsdfConfig.UpdateWeightDistanceCellToHitKernelBandwidth);
                }
                else if (effectiveGridType == GridOptions2D.GridType.Tsdf)
                {
                    // C++ Cartographer defaults for TSDF
                    tsdfOptions = new TSDFRangeDataInserterOptions2D(
                        truncationDistance: 0.3,
                        maximumWeight: 10.0);
                }

                rangeDataInserterOptions = new RangeDataInserterOptions(
                    rangeDataInserterType: effectiveInserterType,
                    probabilityGridRangeDataInserterOptions2D: probGridOptions,
                    tsdfRangeDataInserterOptions2D: tsdfOptions);
            }

            // Create HighResGridOptions2D if configured
            // Apply same gridTypeOverride so HighResGridOptions matches the main grid type
            GridOptions2D? highResGridOptions = null;
            if (config.HighResGridOptions != null)
            {
                highResGridOptions = new GridOptions2D(
                    gridType: gridTypeOverride ?? (GridOptions2D.GridType)config.HighResGridOptions.GridType,
                    resolution: config.HighResGridOptions.Resolution);
            }

            submapsOptions = new SubmapsOptions2D(
                numRangeData: subConfig.NumRangeData,
                gridOptions2D: gridOptions,
                rangeDataInserterOptions: rangeDataInserterOptions ?? new RangeDataInserterOptions(
                    RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D),
                highResGridOptions2D: highResGridOptions);
        }

        // Create AdaptiveVoxelFilterOptions
        CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions? adaptiveVoxelFilterOptions = null;
        if (config.AdaptiveVoxelFilterOptions != null)
        {
            var avfConfig = config.AdaptiveVoxelFilterOptions;
            adaptiveVoxelFilterOptions = new CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions(
                maxLength: avfConfig.MaxLength,
                minNumPoints: avfConfig.MinNumPoints,
                maxRange: avfConfig.MaxRange);
        }

        // Create LoopClosureAdaptiveVoxelFilterOptions
        CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions? loopClosureAdaptiveVoxelFilterOptions = null;
        if (config.LoopClosureAdaptiveVoxelFilterOptions != null)
        {
            var lcavfConfig = config.LoopClosureAdaptiveVoxelFilterOptions;
            loopClosureAdaptiveVoxelFilterOptions = new CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions(
                maxLength: lcavfConfig.MaxLength,
                minNumPoints: lcavfConfig.MinNumPoints,
                maxRange: lcavfConfig.MaxRange);
        }

        // Create PoseExtrapolatorOptions
        PoseExtrapolatorOptions? poseExtrapolatorOptions = null;
        if (config.PoseExtrapolatorOptions != null)
        {
            var peConfig = config.PoseExtrapolatorOptions;
            
            // Create ConstantVelocityPoseExtrapolatorOptions
            var constantVelocityOptions = new ConstantVelocityPoseExtrapolatorOptions(
                imuGravityTimeConstant: peConfig.ConstantVelocity.ImuGravityTimeConstant,
                poseQueueDuration: peConfig.ConstantVelocity.PoseQueueDuration);

            // Create ImuBasedPoseExtrapolatorOptions
            ImuBasedPoseExtrapolatorOptions? imuBasedOptions = null;
            if (peConfig.ImuBased != null)
            {
                var imuConfig = peConfig.ImuBased;
                CartographerSharp.Models.Common.CeresSolverOptions? solverOptions = null;
                if (imuConfig.SolverOptions != null)
                {
                    var solverConfig = imuConfig.SolverOptions;
                    solverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
                        useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
                        maxNumIterations: solverConfig.MaxNumIterations,
                        numThreads: solverConfig.NumThreads,
                        functionTolerance: solverConfig.FunctionTolerance,
                        gradientTolerance: solverConfig.GradientTolerance,
                        parameterTolerance: solverConfig.ParameterTolerance);
                }

                imuBasedOptions = new ImuBasedPoseExtrapolatorOptions(
                    poseQueueDuration: imuConfig.PoseQueueDuration,
                    gravityConstant: imuConfig.GravityConstant,
                    poseTranslationWeight: imuConfig.PoseTranslationWeight,
                    poseRotationWeight: imuConfig.PoseRotationWeight,
                    imuAccelerationWeight: imuConfig.ImuAccelerationWeight,
                    imuRotationWeight: imuConfig.ImuRotationWeight,
                    solverOptions: solverOptions,
                    odometryTranslationWeight: imuConfig.OdometryTranslationWeight,
                    odometryRotationWeight: imuConfig.OdometryRotationWeight);
            }

            poseExtrapolatorOptions = new PoseExtrapolatorOptions(
                useImuBased: peConfig.UseImuBased,
                constantVelocity: constantVelocityOptions,
                imuBased: imuBasedOptions,
                velocityThreshold: peConfig.VelocityThreshold,
                accelerationThreshold: peConfig.AccelerationThreshold,
                gravityDeviationThreshold: peConfig.GravityDeviationThreshold,
                useOdometryDirectly: peConfig.UseOdometryDirectly ?? false);
        }

        // Create InitialPoseRealTimeCorrelativeScanMatcherOptions
        RealTimeCorrelativeScanMatcherOptions? initialPoseRealTimeCorrelativeScanMatcherOptions = null;
        if (config.InitialPoseRealTimeCorrelativeScanMatcherOptions != null)
        {
            var iprtcConfig = config.InitialPoseRealTimeCorrelativeScanMatcherOptions;
            initialPoseRealTimeCorrelativeScanMatcherOptions = new RealTimeCorrelativeScanMatcherOptions(
                linearSearchWindow: iprtcConfig.LinearSearchWindow,
                angularSearchWindow: iprtcConfig.AngularSearchWindow,
                translationDeltaCostWeight: iprtcConfig.TranslationDeltaCostWeight,
                rotationDeltaCostWeight: iprtcConfig.RotationDeltaCostWeight,
                numThreads: iprtcConfig.NumThreads);
        }

        // Create InitialPoseCeresScanMatcherOptions
        CeresScanMatcherOptions2D? initialPoseCeresScanMatcherOptions = null;
        if (config.InitialPoseCeresScanMatcherOptions != null)
        {
            var ipcsConfig = config.InitialPoseCeresScanMatcherOptions;
            CartographerSharp.Models.Common.CeresSolverOptions? ipCeresSolverOptions = null;
            if (ipcsConfig.CeresSolverOptions != null)
            {
                var solverConfig = ipcsConfig.CeresSolverOptions;
                ipCeresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
                    useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
                    maxNumIterations: solverConfig.MaxNumIterations,
                    numThreads: solverConfig.NumThreads,
                    functionTolerance: solverConfig.FunctionTolerance,
                    gradientTolerance: solverConfig.GradientTolerance,
                    parameterTolerance: solverConfig.ParameterTolerance);
            }

            initialPoseCeresScanMatcherOptions = new CeresScanMatcherOptions2D(
                occupiedSpaceWeight: ipcsConfig.OccupiedSpaceWeight,
                translationWeight: ipcsConfig.TranslationWeight,
                rotationWeight: ipcsConfig.RotationWeight,
                ceresSolverOptions: ipCeresSolverOptions,
                highResOccupiedSpaceWeight: ipcsConfig.HighResOccupiedSpaceWeight,
                highResTranslationWeight: ipcsConfig.HighResTranslationWeight,
                highResRotationWeight: ipcsConfig.HighResRotationWeight,
                landmarkWeight: ipcsConfig.LandmarkWeight);
        }

        var options2D = new LocalTrajectoryBuilderOptions2D(
            minRange: config.MinRange,
            maxRange: config.MaxRange,
            minZ: (config.MinZ ?? -0.8f), // Default from Cartographer C++
            maxZ: (config.MaxZ ?? 2.0), // Default from Cartographer C++
            missingDataRayLength: (config.MissingDataRayLength ?? 5.0), // Default from Cartographer C++
            numAccumulatedRangeData: config.NumAccumulatedRangeData,
            voxelFilterSize: config.VoxelFilterSize,
            useOnlineCorrelativeScanMatching: config.UseOnlineCorrelativeScanMatching,
            realTimeCorrelativeScanMatcherOptions: realTimeCorrelativeScanMatcherOptions ?? default,
            ceresScanMatcherOptions: ceresScanMatcherOptions ?? default,
            motionFilterOptions: motionFilterOptions ?? default,
            submapsOptions: submapsOptions ?? default,
            useImuData: finalUseImuData,
            adaptiveVoxelFilterOptions: adaptiveVoxelFilterOptions ?? default,
            loopClosureAdaptiveVoxelFilterOptions: loopClosureAdaptiveVoxelFilterOptions ?? default,
            poseExtrapolatorOptions: poseExtrapolatorOptions ?? default,
            initialPoseRealTimeCorrelativeScanMatcherOptions: initialPoseRealTimeCorrelativeScanMatcherOptions ?? default,
            initialPoseCeresScanMatcherOptions: initialPoseCeresScanMatcherOptions ?? default,
            provideConfidenceScore: config.ProvideConfidenceScore,
            ceresScoreSoftLimit: config.CeresScoreSoftLimit,
            ceresScoreHardLimit: config.CeresScoreHardLimit,
            maxConsecutiveHighCostBeforeNewSubmap: config.MaxConsecutiveHighCostBeforeNewSubmap);

        var jsonOptions = System.Text.Json.JsonSerializer.Serialize(options2D);
        var jsonElement = System.Text.Json.JsonDocument.Parse(jsonOptions).RootElement;

        return new TrajectoryBuilderOptions(trajectoryBuilder2DOptions: jsonElement);
    }

    /// <summary>
    /// Creates 3D trajectory builder options from configuration
    /// </summary>
    public static TrajectoryBuilderOptions Create3D(
        TrajectoryBuilderConfiguration config,
        bool useImuData)
    {
        if (config.Use2D)
        {
            throw new ArgumentException("Configuration must have Use2D = false", nameof(config));
        }

        // TODO: Implement 3D options when needed
        throw new NotSupportedException("3D trajectory builder not yet implemented");
    }

    /// <summary>
    /// Creates PoseGraphOptions from configuration
    /// </summary>
    public static PoseGraphOptions CreatePoseGraphOptions(MapBuilderConfiguration config)
    {
        // Create OptimizationProblemOptions from configuration
        // Note: OptimizationProblemOptions proto only supports MaxNumIterations directly,
        // not the full CeresSolverOptions (UseNonmonotonicSteps, NumThreads).
        // If CeresSolverOptions.MaxNumIterations is provided, it takes precedence over MaxNumIterations.
        var maxNumIterations = config.PoseGraphOptimizationProblemOptions.MaxNumIterations;
        if (config.PoseGraphOptimizationProblemOptions.CeresSolverOptions != null)
        {
            // Use MaxNumIterations from CeresSolverOptions if provided, otherwise use direct MaxNumIterations
            maxNumIterations = config.PoseGraphOptimizationProblemOptions.CeresSolverOptions.MaxNumIterations;
        }

        var optimizationOptions = new CartographerSharp.Models.Mapping.OptimizationProblemOptions
        {
            HuberScale = config.PoseGraphOptimizationProblemOptions.HuberScale,
            OdometryHuberScale = config.PoseGraphOptimizationProblemOptions.OdometryHuberScale,
            LocalSlamPoseHuberScale = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseHuberScale,
            AccelerationWeight = config.PoseGraphOptimizationProblemOptions.AccelerationWeight,
            RotationWeight = config.PoseGraphOptimizationProblemOptions.RotationWeight,
            LocalSlamPoseTranslationWeight = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseTranslationWeight,
            LocalSlamPoseRotationWeight = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseRotationWeight,
            OdometryTranslationWeight = config.PoseGraphOptimizationProblemOptions.OdometryTranslationWeight,
            OdometryRotationWeight = config.PoseGraphOptimizationProblemOptions.OdometryRotationWeight,
            FixedFramePoseTranslationWeight = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTranslationWeight,
            FixedFramePoseRotationWeight = config.PoseGraphOptimizationProblemOptions.FixedFramePoseRotationWeight,
            FixedFramePoseUseTolerantLoss = config.PoseGraphOptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
            FixedFramePoseTolerantLossParamA = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
            FixedFramePoseTolerantLossParamB = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
            LogSolverSummary = config.PoseGraphOptimizationProblemOptions.LogSolverSummary,
            MaxNumIterations = maxNumIterations
        };
        
        // Log mapped values to verify correct assignment

        // Note: Other CeresSolverOptions properties (UseNonmonotonicSteps, NumThreads) are not supported
        // by OptimizationProblemOptions proto. These options are hardcoded in OptimizationProblem2D/3D classes.

        // Create ConstraintBuilderOptions from configuration
        CartographerSharp.Models.Mapping.ConstraintBuilderOptions? constraintBuilderOptions = null;
        if (config.ConstraintBuilderOptions != null)
        {
            var cbConfig = config.ConstraintBuilderOptions;
            
            // Create FastCorrelativeScanMatcherOptions2D
            FastCorrelativeScanMatcherOptions2D? fastCorrelativeScanMatcherOptions = null;
            if (cbConfig.FastCorrelativeScanMatcherOptions != null)
            {
                var fcsConfig = cbConfig.FastCorrelativeScanMatcherOptions;
                fastCorrelativeScanMatcherOptions = new FastCorrelativeScanMatcherOptions2D(
                    linearSearchWindow: fcsConfig.LinearSearchWindow,
                    angularSearchWindow: fcsConfig.AngularSearchWindow,
                    branchAndBoundDepth: fcsConfig.BranchAndBoundDepth,
                    localizationLinearSearchWindow: fcsConfig.LocalizationLinearSearchWindow,
                    localizationAngularSearchWindow: fcsConfig.LocalizationAngularSearchWindow);
            }

            // Create CeresScanMatcherOptions2D for constraint builder
            CeresScanMatcherOptions2D? ceresScanMatcherOptions = null;
            if (cbConfig.CeresScanMatcherOptions != null)
            {
                var csConfig = cbConfig.CeresScanMatcherOptions;
                CartographerSharp.Models.Common.CeresSolverOptions? ceresSolverOptions = null;
                if (csConfig.CeresSolverOptions != null)
                {
                    var solverConfig = csConfig.CeresSolverOptions;
                    ceresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
                        useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
                        maxNumIterations: solverConfig.MaxNumIterations,
                        numThreads: solverConfig.NumThreads,
                        functionTolerance: solverConfig.FunctionTolerance,
                        gradientTolerance: solverConfig.GradientTolerance,
                        parameterTolerance: solverConfig.ParameterTolerance);
                }

                ceresScanMatcherOptions = new CeresScanMatcherOptions2D(
                    occupiedSpaceWeight: csConfig.OccupiedSpaceWeight,
                    translationWeight: csConfig.TranslationWeight,
                    rotationWeight: csConfig.RotationWeight,
                    ceresSolverOptions: ceresSolverOptions,
                    highResOccupiedSpaceWeight: csConfig.HighResOccupiedSpaceWeight,
                    highResTranslationWeight: csConfig.HighResTranslationWeight,
                    highResRotationWeight: csConfig.HighResRotationWeight,
                    landmarkWeight: csConfig.LandmarkWeight);
            }

            constraintBuilderOptions = new CartographerSharp.Models.Mapping.ConstraintBuilderOptions
            {
                SamplingRatio = cbConfig.SamplingRatio,
                MaxConstraintDistance = cbConfig.MaxConstraintDistance,
                MinScore = cbConfig.MinScore,
                GlobalLocalizationMinScore = cbConfig.GlobalLocalizationMinScore,
                LoopClosureTranslationWeight = cbConfig.LoopClosureTranslationWeight,
                LoopClosureRotationWeight = cbConfig.LoopClosureRotationWeight,
                LogMatches = cbConfig.LogMatches,
                FastCorrelativeScanMatcherOptions = fastCorrelativeScanMatcherOptions,
                CeresScanMatcherOptions = ceresScanMatcherOptions
            };
        }

        // Create OverlappingSubmapsTrimmerOptions2D from configuration
        PoseGraphOptions.OverlappingSubmapsTrimmerOptions2D? overlappingSubmapsTrimmer2D = null;
        if (config.OverlappingSubmapsTrimmer2D != null)
        {
            var trimmerConfig = config.OverlappingSubmapsTrimmer2D;
            overlappingSubmapsTrimmer2D = new PoseGraphOptions.OverlappingSubmapsTrimmerOptions2D(
                freshSubmapsCount: trimmerConfig.FreshSubmapsCount,
                minCoveredArea: trimmerConfig.MinCoveredArea,
                minAddedSubmapsCount: trimmerConfig.MinAddedSubmapsCount
            );
        }

        // Create PoseGraphOptions with optimization options from configuration
        return new PoseGraphOptions(
            optimizeEveryNNodes: config.OptimizeEveryNNodes,
            matcherTranslationWeight: config.MatcherTranslationWeight,
            matcherRotationWeight: config.MatcherRotationWeight,
            maxNumFinalIterations: config.MaxNumFinalIterations ?? 200, // Default from Cartographer C++
            globalSamplingRatio: config.GlobalSamplingRatio ?? 0.003, // Default from Cartographer C++
            logResidualHistograms: config.LogResidualHistograms ?? false, // Default
            globalConstraintSearchAfterNSeconds: config.GlobalConstraintSearchAfterNSeconds ?? 0.0, // Default (disabled)
            overlappingSubmapsTrimmer2D: overlappingSubmapsTrimmer2D,
            constraintBuilderOptions: constraintBuilderOptions,
            optimizationProblemOptions: optimizationOptions
        )
        {
            // Single-trajectory loop closure extension (not in C++ Cartographer)
            EnableSingleTrajectoryLoopClosure = config.EnableSingleTrajectoryLoopClosure,
            SingleTrajectoryLoopClosureDistanceThreshold = config.SingleTrajectoryLoopClosureDistanceThreshold
        };
    }
}