/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using CartographerSharp.Common;
using CartographerSharp.Mapping.Internal.D2D.ScanMatching;
using CartographerSharp.Models.Mapping;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using Submap2D = CartographerSharp.Mapping.D2D.Submap2D;
using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;

namespace CartographerSharp.Mapping.Internal.Constraints;

/// <summary>
/// Result of constraint building.
/// </summary>
public record struct ConstraintBuilder2DResult(List<IPoseGraph.Constraint> Constraints);

/// <summary>
/// Callback for constraint building completion.
/// </summary>
public delegate void ConstraintBuilder2DCallback(ConstraintBuilder2DResult result);

/// <summary>
/// Builds constraints for the pose graph by matching nodes against submaps.
/// Matches C++ ConstraintBuilder2D (xloc) including localization and manual compute APIs.
/// </summary>
public class ConstraintBuilder2D : IDisposable
{
    private bool _disposed;
    private readonly ConstraintBuilderOptions _options;
    private readonly Lock _mutex = new();

    private readonly CeresScanMatcher2D _ceresScanMatcher;
    private readonly FastCorrelativeScanMatcherOptions2D? _fastCorrelativeScanMatcherOptions;
    private readonly Dictionary<SubmapId, FixedRatioSampler> _perSubmapSampler = [];

    // Match C++: localization_mode_, is_search_for_relocalization_
    private bool _localizationMode;
    private bool _isSearchForRelocalization;

    // Match C++: num_started_nodes_, num_finished_nodes_ (constraint_builder_2d.h line 177-179)
    private int _numStartedNodes;
    private int _numFinishedNodes;

    // Constraint task progress counters (for MapSaveProcessor progress tracking)
    private int _numConstraintTasksDispatched;
    private int _numConstraintTasksFinished;

    // Match C++: thread_pool_ (constraint_builder_2d.cc line 63)
    private readonly Common.Threading.ThreadPoolInterface _threadPool;

    // Match C++: finish_node_task_, when_done_task_ (constraint_builder_2d.h line 181-183)
    private Common.Threading.Task _finishNodeTask;
    private Common.Threading.Task _whenDoneTask;

    // Match C++: SubmapScanMatcher struct (constraint_builder_2d.h line 140-145)
    // Stores the grid, fast correlative scan matcher, and creation task handle
    private class SubmapScanMatcher
    {
        public Grid2D? Grid { get; set; }
        public FastCorrelativeScanMatcher2D? FastCorrelativeScanMatcher { get; set; }
        public WeakReference<Common.Threading.Task>? CreationTaskHandle { get; set; }
    }

    // Match C++: submap_scan_matchers_ (constraint_builder_2d.h line 191-192)
    private readonly Dictionary<SubmapId, SubmapScanMatcher> _submapScanMatchers = [];

    // Match C++: constraints_ deque (constraint_builder_2d.h line 188)
    // We use a list of nullable constraints since computation may fail
    private readonly List<IPoseGraph.Constraint?> _pendingConstraints = [];

    // Match C++: when_done_ callback (constraint_builder_2d.h line 170-171)
    private ConstraintBuilder2DCallback? _whenDoneCallback;

    // === MEMORY OPTIMIZATION: Limit concurrent MatchFullSubmap calls ===
    // Each MatchFullSubmap can allocate 150-250MB, running 10+ concurrently causes 3-4GB spikes
    // Limit to 2 concurrent calls to prevent memory exhaustion while still allowing parallelism
    private readonly SemaphoreSlim _matchFullSubmapSemaphore = new(16, 16);
    private int _activeMatchFullSubmapCount;

    // Match C++: Constructor accepts thread_pool (constraint_builder_2d.cc line 59-66)
    public ConstraintBuilder2D(ConstraintBuilderOptions options, Common.Threading.ThreadPoolInterface threadPool)
    {
        _options = options;
        _threadPool = threadPool;
        _fastCorrelativeScanMatcherOptions = options.FastCorrelativeScanMatcherOptions ??
            new FastCorrelativeScanMatcherOptions2D(linearSearchWindow: 7.0, angularSearchWindow: Math.PI / 6.0, branchAndBoundDepth: 7);
        var ceresOptions = options.CeresScanMatcherOptions ?? new CeresScanMatcherOptions2D(20.0, 0.1, 0.1);
        _ceresScanMatcher = new CeresScanMatcher2D(ceresOptions);

        // Match C++ (constraint_builder_2d.cc line 64-65): Initialize task objects
        _finishNodeTask = new Common.Threading.Task();
        _whenDoneTask = new Common.Threading.Task();
    }

    /// <summary>
    /// Match C++: MaybeAddConstraint - one initial_relative_pose, Match() then Ceres.
    /// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
    /// </summary>
    public void MaybeAddConstraint(
        SubmapId submapId,
        NodeId nodeId,
        Submap2D submap,
        TrajectoryNode node,
        Rigid2d initialRelativePose,
        ConstraintBuilder2DCallback? callback = null)
    {
        ArgumentNullException.ThrowIfNull(submap);
        ArgumentNullException.ThrowIfNull(node);
        if (node.ConstantData == null) return;

        if (initialRelativePose.Translation.Length() > _options.MaxConstraintDistance)
            return;

        if (!GetOrCreateSampler(submapId).Pulse())
            return;

        var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
        if (pointCloud == null || pointCloud.Count == 0) return;

        var grid = submap.Grid;
        if (grid == null) return;

        // Match C++ (constraint_builder_2d.cc line 92-111)
        lock (_mutex)
        {
            if (_whenDoneCallback != null)
            {
                // LOG(WARNING): MaybeAddConstraint was called while WhenDone was scheduled
            }

            // Add placeholder for constraint result
            var constraintIndex = _pendingConstraints.Count;
            _pendingConstraints.Add(null);
            _numConstraintTasksDispatched++;

            // Get or create scan matcher (may schedule async construction)
            var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);

            // Schedule constraint computation task
            var constraintTask = new Common.Threading.Task();
            constraintTask.SetWorkItem(() =>
            {
                ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
                    matchFullSubmap: false, [initialRelativePose], constraintIndex);
                Interlocked.Increment(ref _numConstraintTasksFinished);
            });

            // Add dependency on scan matcher construction (match C++ line 108)
            constraintTask.AddDependency(scanMatcher.CreationTaskHandle);

            var constraintTaskHandle = _threadPool.Schedule(constraintTask);

            // Add dependency to finish_node_task (match C++ line 111)
            _finishNodeTask.AddDependency(constraintTaskHandle);
        }
    }

    /// <summary>
    /// Match C++: MaybeAddLocalizationConstraint - list of initial_relative_poses, LocalizationMatch then Ceres.
    /// Only adds a constraint when IsSearchingForRelocalization is true; then clears the flag on success.
    /// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
    /// </summary>
    public void MaybeAddLocalizationConstraint(
        SubmapId submapId,
        NodeId nodeId,
        Submap2D submap,
        TrajectoryNode node,
        IReadOnlyList<Rigid2d> initialRelativePoses,
        ConstraintBuilder2DCallback? callback = null)
    {
        ArgumentNullException.ThrowIfNull(submap);
        ArgumentNullException.ThrowIfNull(node);
        if (node.ConstantData == null) return;
        if (initialRelativePoses == null || initialRelativePoses.Count == 0) return;

        var filtered = initialRelativePoses
            .Where(p => p.Translation.Length() <= _options.MaxConstraintDistance)
            .ToList();
        if (filtered.Count == 0) return;

        _localizationMode = true;

        var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
        if (pointCloud == null || pointCloud.Count == 0) return;

        var grid = submap.Grid;
        if (grid == null) return;

        // Match C++ (constraint_builder_2d.cc line 138-157)
        lock (_mutex)
        {
            if (_whenDoneCallback != null)
            {
                // LOG(WARNING): MaybeAddConstraint was called while WhenDone was scheduled
            }

            var constraintIndex = _pendingConstraints.Count;
            _pendingConstraints.Add(null);
            _numConstraintTasksDispatched++;

            var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);

            var constraintTask = new Common.Threading.Task();
            constraintTask.SetWorkItem(() =>
            {
                ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
                    matchFullSubmap: true, filtered, constraintIndex);
                Interlocked.Increment(ref _numConstraintTasksFinished);
            });

            constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
            var constraintTaskHandle = _threadPool.Schedule(constraintTask);
            _finishNodeTask.AddDependency(constraintTaskHandle);
        }
    }

    /// <summary>
    /// Match C++: MaybeAddGlobalConstraint - full submap match (MatchFullSubmap then Ceres).
    /// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
    /// </summary>
    public void MaybeAddGlobalConstraint(
        SubmapId submapId,
        NodeId nodeId,
        Submap2D submap,
        TrajectoryNode node,
        ConstraintBuilder2DCallback? callback = null)
    {
        ArgumentNullException.ThrowIfNull(submap);
        ArgumentNullException.ThrowIfNull(node);
        if (node.ConstantData == null) return;

        var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
        if (pointCloud == null || pointCloud.Count == 0) return;

        var grid = submap.Grid;
        if (grid == null) return;

        // Match C++ (constraint_builder_2d.cc line 160-182)
        lock (_mutex)
        {
            if (_whenDoneCallback != null)
            {
                // LOG(WARNING): MaybeAddGlobalConstraint was called while WhenDone was scheduled
            }

            var constraintIndex = _pendingConstraints.Count;
            _pendingConstraints.Add(null);
            _numConstraintTasksDispatched++;

            var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);

            var constraintTask = new Common.Threading.Task();
            constraintTask.SetWorkItem(() =>
            {
                ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
                    matchFullSubmap: true, [Rigid2d.Identity], constraintIndex);
                Interlocked.Increment(ref _numConstraintTasksFinished);
            });

            constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
            var constraintTaskHandle = _threadPool.Schedule(constraintTask);
            _finishNodeTask.AddDependency(constraintTaskHandle);
        }
    }

    /// <summary>
    /// Match C++: manualComputeGlobalConstraint - MatchFullSubmap, Ceres, then MatchWithCustomizeParameters(0.1, 0.1, 0.01) for score.
    /// </summary>
    public (double Score, IPoseGraph.Constraint? Constraint) ManualComputeGlobalConstraint(
        SubmapId submapId,
        Submap2D submap,
        NodeId nodeId,
        TrajectoryNode.Data constantData,
        double minScore)
    {
        ArgumentNullException.ThrowIfNull(submap);
        ArgumentNullException.ThrowIfNull(constantData);
        var pointCloud = constantData.FilteredGravityAlignedPointCloud;
        if (pointCloud == null || pointCloud.Count == 0)
            return (0, null);

        var grid = submap.Grid;
        if (grid == null) return (0, null);

        var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
        var submapPose = ComputeSubmapPose(submap);
        if (!fastMatcher.MatchFullSubmap(pointCloud, 0, out _, out var poseEstimate))
            return (0, null);

        _ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary1);
        ceresSummary1?.Dispose();
        if (fastMatcher.MatchWithCustomizeParameters(0.1, 0.1, 0.01f, poseEstimate, pointCloud, 0, out double score, out poseEstimate))
        {
            // Re-calculated score
        }

        var constraintTransform = submapPose.Inverse() * poseEstimate;
        // Match C++: include score and state (constraint_builder_2d.cc)
        var constraint = new IPoseGraph.Constraint(
            submapId, nodeId,
            new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
            IPoseGraph.Constraint.Tag.InterSubmap,
            score,
            IPoseGraph.Constraint.State.Enabled);
        return (score, constraint);
    }

    /// <summary>
    /// Match C++: manualComputeRelocalizationConstraint - try LocalizationMatch on each submap/pose, pick best, Ceres, return constraint.
    /// </summary>
    public (double Score, IPoseGraph.Constraint? Constraint) ManualComputeRelocalizationConstraint(
        IReadOnlyList<SubmapId> submapIds,
        IReadOnlyList<Submap2D> submaps,
        IReadOnlyList<Rigid2d> relativePoses,
        NodeId nodeId,
        TrajectoryNode.Data constantData,
        double minScore)
    {
        if (submapIds == null || submaps == null || relativePoses == null || submapIds.Count != submaps.Count || submapIds.Count != relativePoses.Count)
            return (0, null);
        ArgumentNullException.ThrowIfNull(constantData);
        var pointCloud = constantData.FilteredGravityAlignedPointCloud;
        if (pointCloud == null || pointCloud.Count == 0) return (0, null);

        double bestScore = 0;
        Rigid2d bestPoseEstimate = Rigid2d.Identity;
        Submap2D? bestSubmap = null;
        SubmapId bestSubmapId = default;

        for (int i = 0; i < submaps.Count; i++)
        {
            var submap = submaps[i];
            var grid = submap.Grid;
            if (grid == null) continue;

            var fastMatcher = GetOrCreateFastMatcherSync(submapIds[i], grid);
            var localizationInitialPose = ComputeSubmapPose(submap) * relativePoses[i];

            if (!fastMatcher.LocalizationMatch(localizationInitialPose, pointCloud, minScore, out var score, out var poseEstimate))
                continue;

            if (score > bestScore)
            {
                bestScore = score;
                bestPoseEstimate = poseEstimate;
                bestSubmap = submap;
                bestSubmapId = submapIds[i];
            }
        }

        if (bestSubmap == null) return (0, null);

        _ceresScanMatcher.Match(bestPoseEstimate.Translation, bestPoseEstimate, pointCloud, bestSubmap.Grid!, out bestPoseEstimate, out var ceresSummary2);
        ceresSummary2?.Dispose();
        var constraintTransform = ComputeSubmapPose(bestSubmap).Inverse() * bestPoseEstimate;
        // Match C++: include score and state (constraint_builder_2d.cc)
        var constraint = new IPoseGraph.Constraint(
            bestSubmapId, nodeId,
            new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
            IPoseGraph.Constraint.Tag.InterSubmap,
            bestScore,
            IPoseGraph.Constraint.State.Enabled);
        return (bestScore, constraint);
    }

    /// <summary>
    /// Match C++: manualComputeConstraintScore - MatchWithCustomizeParameters(1.5, 1.5, 0.05) from initial_pose.
    /// </summary>
    public double ManualComputeConstraintScore(
        SubmapId submapId,
        Submap2D submap,
        NodeId nodeId,
        TrajectoryNode.Data constantData,
        double minScore,
        Rigid3d initialPose)
    {
        ArgumentNullException.ThrowIfNull(submap);
        ArgumentNullException.ThrowIfNull(constantData);
        var pointCloud = constantData.FilteredGravityAlignedPointCloud;
        if (pointCloud == null || pointCloud.Count == 0) return 0;

        var grid = submap.Grid;
        if (grid == null) return 0;

        var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
        var poseEstimate = TransformOperations.Project2D(initialPose);
        fastMatcher.MatchWithCustomizeParameters(1.5, 1.5, 0.05f, poseEstimate, pointCloud, 0, out var constraintScore, out _);
        return constraintScore;
    }

    /// <summary>
    /// Match C++: manualComputeScanMatcher - Ceres then MatchWithCustomizeParameters(0.2, 0.2, 0.01), output pose_manual_estimate.
    /// </summary>
    public double ManualComputeScanMatcher(
        SubmapId submapId,
        Submap2D submap,
        NodeId nodeId,
        TrajectoryNode.Data constantData,
        double minScore,
        Rigid3d initialPose,
        out Rigid3d poseManualEstimate)
    {
        poseManualEstimate = default;
        ArgumentNullException.ThrowIfNull(submap);
        ArgumentNullException.ThrowIfNull(constantData);
        var pointCloud = constantData.FilteredGravityAlignedPointCloud;
        if (pointCloud == null || pointCloud.Count == 0) return 0;

        var grid = submap.Grid;
        if (grid == null) return 0;

        var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
        var poseEstimate = TransformOperations.Project2D(initialPose);

        _ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary3);
        ceresSummary3?.Dispose();

        if (fastMatcher.MatchWithCustomizeParameters(0.2, 0.2, 0.01f, poseEstimate, pointCloud, 0, out double score, out poseEstimate))
        {
            poseManualEstimate = TransformOperations.Embed3D(poseEstimate);
            return score;
        }
        poseManualEstimate = TransformOperations.Embed3D(poseEstimate);
        return score;
    }

    /// <summary>
    /// Match C++: NotifyEndOfNode - must be called after all computations for one node have been added.
    /// Match C++ (constraint_builder_2d.cc line 403-415)
    /// </summary>
    public void NotifyEndOfNode()
    {
        lock (_mutex)
        {
            // Set work item for finish_node_task to increment num_finished_nodes
            _finishNodeTask.SetWorkItem(() =>
            {
                lock (_mutex)
                {
                    _numFinishedNodes++;
                }
            });

            // Schedule finish_node_task
            var finishNodeTaskHandle = _threadPool.Schedule(_finishNodeTask);

            // Create new finish_node_task for next node
            _finishNodeTask = new Common.Threading.Task();

            // Add dependency to when_done_task
            _whenDoneTask.AddDependency(finishNodeTaskHandle);

            _numStartedNodes++;
        }
    }

    /// <summary>
    /// Match C++ WhenDone: Registers callback to be called after all computations finish.
    /// Match C++ (constraint_builder_2d.cc line 417-427)
    /// </summary>
    public void WhenDone(ConstraintBuilder2DCallback callback)
    {
        lock (_mutex)
        {
            if (_whenDoneCallback != null)
            {
                throw new InvalidOperationException("WhenDone() called while another WhenDone() was pending");
            }

            _whenDoneCallback = callback;

            // Set work item for when_done_task to run callback
            _whenDoneTask.SetWorkItem(RunWhenDoneCallback);

            // Schedule when_done_task (it will wait for all dependencies)
            _threadPool.Schedule(_whenDoneTask);

            // Create new when_done_task for next cycle
            _whenDoneTask = new Common.Threading.Task();
        }
    }

    /// <summary>
    /// Match C++ RunWhenDoneCallback (constraint_builder_2d.cc line 596-617)
    /// </summary>
    private void RunWhenDoneCallback()
    {
        List<IPoseGraph.Constraint> result = [];
        ConstraintBuilder2DCallback? callback;

        lock (_mutex)
        {
            if (_whenDoneCallback == null)
            {
                throw new InvalidOperationException("RunWhenDoneCallback called without callback set");
            }

            // Collect all non-null constraints
            foreach (var constraint in _pendingConstraints)
            {
                if (constraint != null)
                {
                    result.Add(constraint.Value);
                }
            }

            // Clear pending constraints
            _pendingConstraints.Clear();

            // Take callback and clear
            callback = _whenDoneCallback;
            _whenDoneCallback = null;
        }

        // Invoke callback outside lock
        callback(new ConstraintBuilder2DResult(result));
    }

    public List<IPoseGraph.Constraint> GetConstraints()
    {
        lock (_mutex)
        {
            return [.. _pendingConstraints.Where(c => c != null).Select(c => c!.Value)];
        }
    }

    /// <summary>
    /// Match C++: GetNumFinishedNodes().
    /// </summary>
    public int GetNumFinishedNodes()
    {
        lock (_mutex) return _numFinishedNodes;
    }

    /// <summary>
    /// Match C++: DeleteScanMatcher(submap_id).
    /// </summary>
    public void DeleteScanMatcher(SubmapId submapId)
    {
        lock (_mutex)
        {
            _submapScanMatchers.Remove(submapId);
            _perSubmapSampler.Remove(submapId);
        }
    }

    /// <summary>
    /// Match C++: GetNumStartedNodes().
    /// </summary>
    public int GetNumStartedNodes()
    {
        lock (_mutex) return _numStartedNodes;
    }

    /// <summary>
    /// Gets the total number of constraint tasks dispatched for computation.
    /// </summary>
    public int GetNumConstraintTasksDispatched()
    {
        lock (_mutex) return _numConstraintTasksDispatched;
    }

    /// <summary>
    /// Gets the number of constraint tasks that have finished computation.
    /// </summary>
    public int GetNumConstraintTasksFinished()
    {
        return Interlocked.CompareExchange(ref _numConstraintTasksFinished, 0, 0);
    }

    /// <summary>
    /// Match C++: IsSearchingForRelocalization().
    /// </summary>
    public bool IsSearchingForRelocalization => _isSearchForRelocalization;

    /// <summary>
    /// Match C++: ToggleSearchingForRelocalization(enable).
    /// </summary>
    public void ToggleSearchingForRelocalization(bool enable)
    {
        _isSearchForRelocalization = enable;
    }

    public void Clear()
    {
        lock (_mutex)
        {
            _pendingConstraints.Clear();
        }
    }

    private FixedRatioSampler GetOrCreateSampler(SubmapId submapId)
    {
        lock (_mutex)
        {
            if (!_perSubmapSampler.TryGetValue(submapId, out var sampler))
            {
                sampler = new FixedRatioSampler(_options.SamplingRatio);
                _perSubmapSampler[submapId] = sampler;
            }
            return sampler;
        }
    }

    /// <summary>
    /// Match C++ DispatchScanMatcherConstruction (constraint_builder_2d.cc line 429-450)
    /// Creates or returns existing SubmapScanMatcher, scheduling async construction if needed.
    /// MUST be called with _mutex held.
    /// </summary>
    private SubmapScanMatcher DispatchScanMatcherConstruction(SubmapId submapId, Grid2D grid)
    {
        // Check if scan matcher already exists
        if (_submapScanMatchers.TryGetValue(submapId, out var existingMatcher))
        {
            return existingMatcher;
        }

        // Create new scan matcher entry
        var submapScanMatcher = new SubmapScanMatcher
        {
            Grid = grid
        };
        _submapScanMatchers[submapId] = submapScanMatcher;

        var scanMatcherOptions = _fastCorrelativeScanMatcherOptions ??
            new FastCorrelativeScanMatcherOptions2D(7.0, Math.PI / 6.0, 7);

        // Schedule async construction of FastCorrelativeScanMatcher2D
        var scanMatcherTask = new Common.Threading.Task();
        scanMatcherTask.SetWorkItem(() =>
        {
            // Create the scan matcher (this may be expensive)
            var gridLimits = grid.Limits.CellLimits;
            var matcher = new FastCorrelativeScanMatcher2D(grid, scanMatcherOptions);
            lock (_mutex)
            {
                submapScanMatcher.FastCorrelativeScanMatcher = matcher;
            }
        });

        submapScanMatcher.CreationTaskHandle = _threadPool.Schedule(scanMatcherTask);

        return submapScanMatcher;
    }

    /// <summary>
    /// Gets the FastCorrelativeScanMatcher for a submap (for manual compute methods).
    /// This blocks until the scan matcher is ready.
    /// </summary>
    private FastCorrelativeScanMatcher2D GetOrCreateFastMatcherSync(SubmapId submapId, Grid2D grid)
    {
        SubmapScanMatcher? scanMatcher;
        lock (_mutex)
        {
            if (!_submapScanMatchers.TryGetValue(submapId, out scanMatcher))
            {
                // Create synchronously for manual methods
                var options = _fastCorrelativeScanMatcherOptions ??
                    new FastCorrelativeScanMatcherOptions2D(7.0, Math.PI / 6.0, 7);
                var matcher = new FastCorrelativeScanMatcher2D(grid, options);
                scanMatcher = new SubmapScanMatcher
                {
                    Grid = grid,
                    FastCorrelativeScanMatcher = matcher
                };
                _submapScanMatchers[submapId] = scanMatcher;
                return matcher;
            }
        }

        // Wait for async construction if needed
        if (scanMatcher.FastCorrelativeScanMatcher == null &&
            scanMatcher.CreationTaskHandle != null &&
            scanMatcher.CreationTaskHandle.TryGetTarget(out var task))
        {
            while (task.GetState() != Common.Threading.TaskState.Completed)
            {
                System.Threading.Thread.Sleep(1);
            }
        }

        lock (_mutex)
        {
            return scanMatcher.FastCorrelativeScanMatcher!;
        }
    }

    /// <summary>
    /// Single internal compute: handles MaybeAddConstraint (matchFullSubmap=false, single pose),
    /// MaybeAddLocalizationConstraint (matchFullSubmap=true, localizationMode, many poses),
    /// MaybeAddGlobalConstraint (matchFullSubmap=true, single Identity pose).
    /// Match C++ ComputeConstraint (constraint_builder_2d.cc line 452-594)
    /// </summary>
    private void ComputeConstraint(
        SubmapId submapId,
        NodeId nodeId,
        Submap2D submap,
        Grid2D grid,
        PointCloud pointCloud,
        bool matchFullSubmap,
        IReadOnlyList<Rigid2d> initialRelativePoses,
        int constraintIndex)
    {
        // Get the scan matcher (should be ready by now due to task dependency)
        FastCorrelativeScanMatcher2D? fastMatcher;
        lock (_mutex)
        {
            if (!_submapScanMatchers.TryGetValue(submapId, out var scanMatcher) ||
                scanMatcher.FastCorrelativeScanMatcher == null)
            {
                return; // Scan matcher not ready (shouldn't happen with proper dependencies)
            }
            fastMatcher = scanMatcher.FastCorrelativeScanMatcher;
        }

        var submapPose = ComputeSubmapPose(submap);
        double score = 0;
        Rigid2d poseEstimate = Rigid2d.Identity;

        if (matchFullSubmap)
        {
            if (_localizationMode)
            {
                lock (_mutex)
                {
                    if (!_isSearchForRelocalization)
                        return;
                }

                double bestScore = 0;
                Rigid2d bestPoseEstimate = Rigid2d.Identity;
                foreach (var rel in initialRelativePoses)
                {
                    var localizationInitialPose = submapPose * rel;
                    if (fastMatcher.LocalizationMatch(localizationInitialPose, pointCloud, _options.GlobalLocalizationMinScore, out score, out poseEstimate))
                    {
                        if (score > bestScore)
                        {
                            bestScore = score;
                            bestPoseEstimate = poseEstimate;
                        }
                    }
                }

                if (bestScore < _options.GlobalLocalizationMinScore)
                    return;

                _isSearchForRelocalization = false;
                score = bestScore;
                poseEstimate = bestPoseEstimate;
            }
            else
            {
                // === MEMORY OPTIMIZATION: Limit concurrent MatchFullSubmap calls ===
                // Each call allocates 150-250MB, running many concurrently causes GB-level spikes
                _matchFullSubmapSemaphore.Wait();
                _ = Interlocked.Increment(ref _activeMatchFullSubmapCount);
                try
                {
                    // === DEBUG: Track memory before/after MatchFullSubmap ===
                    var ramBeforeMatch = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);

                    if (!fastMatcher.MatchFullSubmap(pointCloud, _options.GlobalLocalizationMinScore, out score, out poseEstimate))
                    {
                        var ramAfterFail = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);
                        return;
                    }

                    var ramAfterMatch = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);

                    if (score <= _options.GlobalLocalizationMinScore)
                        return;
                }
                finally
                {
                    Interlocked.Decrement(ref _activeMatchFullSubmapCount);
                    _matchFullSubmapSemaphore.Release();
                }
            }
        }
        else
        {
            var initialPose = submapPose * initialRelativePoses[0];
            if (!fastMatcher.Match(initialPose, pointCloud, _options.MinScore, out score, out poseEstimate))
                return;
            if (score <= _options.MinScore)
                return;
        }

        _ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary4);
        ceresSummary4?.Dispose();

        var constraintTransform = submapPose.Inverse() * poseEstimate;
        // Match C++: include score and state (constraint_builder_2d.cc lines 567-574)
        var constraint = new IPoseGraph.Constraint(
            submapId, nodeId,
            new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
            IPoseGraph.Constraint.Tag.InterSubmap,
            score,  // CRITICAL FIX: include score from scan matching
            IPoseGraph.Constraint.State.Enabled);  // Match C++: Constraint::ENABLED

        // Store constraint at the pre-allocated index
        lock (_mutex)
        {
            _pendingConstraints[constraintIndex] = constraint;
        }
    }

    private static Rigid2d ComputeSubmapPose(Submap2D submap)
    {
        return TransformOperations.Project2D(submap.LocalPose);
    }

    public void Dispose()
    {
        if (!_disposed)
        {
            _ceresScanMatcher?.Dispose();
            _matchFullSubmapSemaphore?.Dispose();
            _disposed = true;
        }
    }
}