/*
 * 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.Constraints;
using CartographerSharp.Mapping.Internal.Optimization;
using CartographerSharp.Models.Mapping;
using CartographerSharp.Models.Transform;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using System.Diagnostics;
using RobotNet10.Shared.Numbers;
using PoseGraphOptions = CartographerSharp.Models.Mapping.PoseGraphOptions;

namespace CartographerSharp.Mapping.Internal.D2D;

/// <summary>
/// Callback invoked when relocalization search reaches max nodes without success.
/// </summary>
public delegate void LocalizationSearchCallback(int trajectoryId, long time, Rigid3d pose);

/// <summary>
/// Memory estimation for Copy-on-Write optimization overhead.
/// </summary>
public struct CopyOnWriteMemoryEstimate
{
    /// <summary>Number of trajectory nodes.</summary>
    public int NodeCount { get; init; }

    /// <summary>Number of submaps.</summary>
    public int SubmapCount { get; init; }

    /// <summary>Number of constraints.</summary>
    public int ConstraintCount { get; init; }

    /// <summary>Memory used by trajectory nodes in bytes.</summary>
    public long TrajectoryNodesBytes { get; init; }

    /// <summary>Memory used by trajectory node poses in bytes.</summary>
    public long TrajectoryNodePosesBytes { get; init; }

    /// <summary>Memory used by submap poses in bytes.</summary>
    public long SubmapPosesBytes { get; init; }

    /// <summary>Memory used by constraints in bytes.</summary>
    public long ConstraintsBytes { get; init; }

    /// <summary>Peak additional memory overhead during COW optimization in bytes.</summary>
    public long PeakOverheadBytes { get; init; }

    /// <summary>Peak additional memory overhead during COW optimization in MB.</summary>
    public double PeakOverheadMB { get; init; }

    /// <summary>
    /// Returns a formatted string with memory breakdown.
    /// </summary>
    public override string ToString()
    {
        return $"COW Memory Estimate:\n" +
               $"  Nodes: {NodeCount}, Submaps: {SubmapCount}, Constraints: {ConstraintCount}\n" +
               $"  TrajectoryNodes: {TrajectoryNodesBytes / 1024.0:F1} KB\n" +
               $"  TrajectoryNodePoses: {TrajectoryNodePosesBytes / 1024.0:F1} KB\n" +
               $"  SubmapPoses: {SubmapPosesBytes / 1024.0:F1} KB\n" +
               $"  Constraints: {ConstraintsBytes / 1024.0:F1} KB\n" +
               $"  Peak Overhead: {PeakOverheadMB:F2} MB";
    }
}

/// <summary>
/// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
/// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
/// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
/// on (pp. 22--29). IEEE, 2010.
///
/// It is extended for submapping:
/// Each node has been matched against one or more submaps (adding a constraint
/// for each match), both poses of nodes and of submaps are to be optimized.
/// All constraints are between a submap i and a node j.
/// </summary>
public class PoseGraph2D : PoseGraph, IDisposable
{
    private readonly PoseGraphOptions _options;
    private GlobalSlamOptimizationCallback? _globalSlamOptimizationCallback;

    // Data structures
    private readonly MapById<SubmapId, IPoseGraph.SubmapData> _submapData = new();
    private readonly MapById<NodeId, TrajectoryNode> _trajectoryNodes = new();
    private readonly MapById<NodeId, TrajectoryNodePose> _trajectoryNodePoses = new();
    private readonly List<IPoseGraph.Constraint> _constraints = [];
    private readonly Dictionary<int, IPoseGraph.TrajectoryState> _trajectoryStates = [];
    private readonly Dictionary<int, IPoseGraph.TrajectoryData> _trajectoryData = [];
    private readonly Dictionary<string, Rigid3d> _landmarkPoses = [];
    private readonly Dictionary<string, IPoseGraph.LandmarkNode> _landmarkNodes = [];
    private readonly Dictionary<int, Rigid3d> _localToGlobalTransforms = [];

    // CRITICAL FIX: Match C++ data_.global_submap_poses_2d
    // This is a copy of optimization_problem_->submap_data() that gets updated after optimization.
    // GetLocalToGlobalTransform reads from this instead of optimization_problem directly to avoid
    // potential blocking/slowness when optimization_problem is being modified.
    private readonly MapById<SubmapId, Optimization.SubmapSpec2D> _globalSubmapPoses2D = new();

    // Sensor data storage (simplified - full implementation would use MapByTime)
    private readonly Dictionary<int, List<ImuData>> _imuData = [];
    private readonly Dictionary<int, List<OdometryData>> _odometryData = [];
    private readonly Dictionary<int, List<FixedFramePoseData>> _fixedFramePoseData = [];

    // Get OptimizationProblemOptions from PoseGraphOptions
    private readonly OptimizationProblem2D _optimizationProblem;

    // ConstraintBuilder2D for inter-submap constraint computation
    private readonly ConstraintBuilder2D? _constraintBuilder;

    // Match C++: Thread pool for parallel constraint computation (pose_graph_2d.cc line 70)
    private readonly Common.Threading.ThreadPool _threadPool;

    // Work queue for serializing operations (similar to Cartographer C++)
    private readonly WorkQueue _workQueue;

    // Optimization tracking counters (thread-safe with Interlocked)
    // Track how many optimization requests were made vs completed
    private long _optimizationRequested = 0;
    private long _optimizationCompleted = 0;

    // Monotonically increasing version counter, incremented each time a node is appended.
    // Used by OccupancyGridManager to skip regeneration when no new data has been added.
    private int _nodeInsertionVersion;

    // Mutex for data structures (only accessed from work queue thread)
    private readonly object _dataLock = new();

    // Periodic optimization tracking
    // FIXED: Match C++ - use GLOBAL counter instead of per-trajectory
    private int _numNodesSinceLastLoopClosure = 0;
    private readonly Lock _optimizationLock = new();
    private Thread? _optimizationThread;
    private bool _optimizationInProgress = false;

    // Track pending optimization requests (when optimization is already running)
    private long _pendingOptimizationCount = 0;

    // Track node-to-submap insertions for constraint builder.
    // SortedSet for O(1) access to Max (last node), matching C++ std::set with rbegin().
    private readonly Dictionary<SubmapId, SortedSet<NodeId>> _submapNodeInsertions = [];

    // Cache of finished submap IDs to avoid scanning all submaps every node.
    // Updated when a submap transitions to InsertionFinished = true.
    private readonly HashSet<SubmapId> _finishedSubmapIds = [];

    // Track nodes for active submaps (before SubmapId is created) by submap reference
    // Key: submap reference (object), Value: set of NodeIds that were inserted into this submap
    private readonly Dictionary<Submap, HashSet<NodeId>> _activeSubmapNodeInsertions = [];

    // Transform from local map frame to global map frame (map origin). Set when loading from pbstream (C++ transform_to_map).
    private Rigid3d? _transformToMap;

    // Match C++: trajectory_connectivity_state, localization, relocalization
    private readonly TrajectoryConnectivityState _trajectoryConnectivityState = new();
    private bool _localizationMode;
    private List<Rigid3d> _localizationInitialPoses = [];
    // Match C++ data_.initial_trajectory_poses: from_trajectory_id -> (to_trajectory_id, pose, time). Used in InitializeGlobalSubmapPoses to Connect when adding first submap.
    private readonly Dictionary<int, (int ToTrajectoryId, Rigid3d Pose, long Time)> _initialTrajectoryPoses = [];
    private int _numRelocalizationConstraintSearch;
    private readonly Dictionary<int, FixedRatioSampler> _globalLocalizationSamplers = [];
    private bool _isRelocalized;
    // Localization result callback - for manual relocalization (LocalizationResultCallback) and search timeout (LocalizationSearchCallback)
    private LocalizationResultCallback? _localizationResultCallback;
    private LocalizationSearchCallback? _localizationSearchCallback;

    // Match C++: current_trajectory_id - tracks the current trajectory being processed
    private int _currentTrajectoryId;

    // Match C++: matching_score_enabled - enables pose confidence scoring
    private bool _matchingScoreEnabled;

    // Match C++: node_scores_ - tracks pose confidence scores over time (max 30 entries)
    private readonly LinkedList<(long Time, double Score)> _nodeScores = new();
    private const int MaxNodeScoresCount = 30;

    // Constructor: Initialize with PoseGraphOptions and ThreadPool
    // Match C++ (pose_graph_2d.cc line 63-70): Accept thread_pool parameter
    // According to Cartographer C++ implementation, PoseGraphOptions is used to:
    // 1. Initialize OptimizationProblem with optimization_problem_options
    // 2. Setup OverlappingSubmapsTrimmer2D if configured
    // 3. Store matcher weights (matcher_translation_weight, matcher_rotation_weight) for constraint building
    // 4. Configure optimization frequency (optimize_every_n_nodes)
    // 5. Setup ConstraintBuilder2D for inter-submap constraint computation
    public PoseGraph2D(PoseGraphOptions options, Common.Threading.ThreadPool threadPool) : base()
    {
        _options = options;
        _threadPool = threadPool;

        // Initialize work queue for thread-safe operations (similar to Cartographer C++)
        _workQueue = new WorkQueue();
        _workQueue.OptimizationNeeded += OnOptimizationNeeded;

        // Initialize optimization problem with options from PoseGraphOptions
        var optimizationProblemOptions = _options.OptimizationProblemOptions ?? new OptimizationProblemOptions();
        _optimizationProblem = new OptimizationProblem2D(optimizationProblemOptions);

        // Setup trimmer if configured
        // This follows the C++ implementation where OverlappingSubmapsTrimmer2D is created from options
        if (_options.OverlappingSubmapsTrimmer2D.HasValue)
        {
            var trimmerOptions = _options.OverlappingSubmapsTrimmer2D.Value;
            var trimmer = new OverlappingSubmapsTrimmer2D(
                trimmerOptions.FreshSubmapsCount,
                trimmerOptions.MinCoveredArea,
                trimmerOptions.MinAddedSubmapsCount);
            AddTrimmer(trimmer);
        }

        // Setup ConstraintBuilder2D if options are provided
        // Match C++ (pose_graph_2d.cc line 69): Pass thread_pool to ConstraintBuilder2D
        if (_options.ConstraintBuilderOptions.HasValue)
        {
            var constraintBuilderOptions = _options.ConstraintBuilderOptions.Value;
            _constraintBuilder = new ConstraintBuilder2D(constraintBuilderOptions, _threadPool);
        }

        // Note: _options.MatcherTranslationWeight and _options.MatcherRotationWeight are used
        // for intra-submap constraints (non-loop-closure)
        // _options.OptimizeEveryNNodes controls when to run periodic optimization
    }

    /// <summary>
    /// Handler for when work queue signals optimization is needed.
    /// Match C++ HandleWorkQueue: Run optimization SYNCHRONOUSLY and then notify work queue to continue.
    /// </summary>
    private void OnOptimizationNeeded(object? sender, EventArgs e)
    {
        try
        {
            // Run optimization SYNCHRONOUSLY (matches C++ HandleWorkQueue behavior)
            // This blocks the work queue thread until optimization completes,
            // preventing lock contention between optimization and work items.
            RunOptimizationSync();
        }
        finally
        {
            // CRITICAL: Always notify work queue that optimization is done
            // This allows work queue to continue processing items
            _workQueue.NotifyOptimizationDone();
        }
    }

    /// <summary>
    /// Adds a work item to the queue. Non-blocking.
    /// </summary>
    private void AddWorkItem(string name, Func<WorkItemResult> workItem)
    {
        // Wrap the work item for logging
        WorkItemResult wrappedWorkItem()
        {
            // Execute the actual work item
            var result = workItem();
            return result;
        }

        _workQueue.AddWorkItem(new WorkItem(wrappedWorkItem));
    }

    // PoseGraphInterface implementation
    // Match C++ implementation (xloc): RunFinalOptimization() sets max_num_iterations via work queue and waits
    // The actual optimization (Solve) happens in RunOptimization() which is triggered by work queue
    public override void RunFinalOptimization()
    {
        // Match C++: Add work items to set max_num_iterations and trigger optimization
        AddWorkItem("RunFinalOptimization Add work items to set max_num_iterations and trigger optimization", () =>
        {
            lock (_dataLock)
            {
                // Set max_num_iterations to max_num_final_iterations for final optimization
                var maxNumIterations = _options.MaxNumFinalIterations > 0
                    ? _options.MaxNumFinalIterations
                    : (_options.OptimizationProblemOptions?.MaxNumIterations ?? 50);
                _optimizationProblem.SetMaxNumIterations(maxNumIterations);
            }
            return WorkItemResult.RunOptimization; // Trigger RunOptimization()
        });

        AddWorkItem("RunFinalOptimization Reset max_num_iterations back to default after optimization", () =>
        {
            lock (_dataLock)
            {
                // Reset max_num_iterations back to default after optimization
                var defaultMaxIterations = _options.OptimizationProblemOptions?.MaxNumIterations ?? 50;
                _optimizationProblem.SetMaxNumIterations(defaultMaxIterations);
            }
            return WorkItemResult.DoNotRunOptimization;
        });

        // Match C++: Wait for all computations to complete before returning
        WaitForAllComputations();
    }

    /// <summary>
    /// Waits for all computations to complete (work queue, constraint builder, and optimization thread).
    /// Match C++ WaitForAllComputations() implementation (line 875-936).
    /// </summary>
    private void WaitForAllComputations()
    {
        // Timeout for waiting: 60 minutes (sufficient for large maps with many constraints)
        var waitTimeout = TimeSpan.FromMinutes(60);

        // Match C++ (line 876-880): Get number of trajectory nodes
        int numTrajectoryNodes;
        lock (_dataLock)
        {
            numTrajectoryNodes = _trajectoryNodes.Count;
        }

        // Match C++ (line 882-898): Progress reporting function with detailed logging
        void ReportProgress(string phase)
        {
            var numFinishedNodes = _constraintBuilder?.GetNumFinishedNodes() ?? 0;
            var remainingNodes = numTrajectoryNodes - numFinishedNodes;
            //Console.WriteLine($"[WaitForAllComputations] {phase}: FinishedNodes={numFinishedNodes}/{numTrajectoryNodes}, Remaining={remainingNodes}");
        }

        // Match C++ (line 900-913): First wait for the work queue to drain
        // so that it's safe to schedule a WhenDone() callback
        var workQueueStartTime = DateTime.UtcNow;
        while (!_workQueue.IsEmpty && (DateTime.UtcNow - workQueueStartTime) < waitTimeout)
        {
            ReportProgress("WorkQueue draining");
            Thread.Sleep(1000); // Check every second (match C++ 1s timeout)
        }

        // Match C++ (line 915-933): Now wait for any pending constraint computations to finish
        if (_constraintBuilder != null)
        {
            // Match C++: Use WhenDone callback to wait for constraint builder to finish
            var constraintBuilderFinished = false;
            var constraintBuilderLock = new object();
            var constraintsFromBuilder = new List<IPoseGraph.Constraint>();

            _constraintBuilder.WhenDone((result) =>
            {
                lock (constraintBuilderLock)
                {
                    if (result.Constraints != null)
                    {
                        constraintsFromBuilder.AddRange(result.Constraints);
                    }
                    constraintBuilderFinished = true;
                    Monitor.PulseAll(constraintBuilderLock);
                }
            });

            // Match C++ (line 930-933): Wait with progress reporting (60 minutes timeout)
            var constraintStartTime = DateTime.UtcNow;
            var lastLogTime = DateTime.UtcNow;
            lock (constraintBuilderLock)
            {
                while (!constraintBuilderFinished && (DateTime.UtcNow - constraintStartTime) < waitTimeout)
                {
                    // Log progress every 5 seconds to avoid log spam
                    if ((DateTime.UtcNow - lastLogTime) >= TimeSpan.FromSeconds(5))
                    {
                        var elapsed = DateTime.UtcNow - constraintStartTime;
                        ReportProgress($"Waiting for ConstraintBuilder (elapsed: {elapsed.TotalSeconds:F1}s)");
                        lastLogTime = DateTime.UtcNow;
                    }
                    Monitor.Wait(constraintBuilderLock, TimeSpan.FromSeconds(1));
                }
            }

            var totalElapsed = DateTime.UtcNow - constraintStartTime;
            if (!constraintBuilderFinished)
            {
                ReportProgress("Timeout state");
            }

            // Match C++ (line 923-924): Add constraints from builder to main constraints list
            if (constraintsFromBuilder.Count > 0)
            {
                lock (_dataLock)
                {
                    _constraints.AddRange(constraintsFromBuilder);
                }
            }

            // Match C++ (line 934): CHECK_EQ constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes
            var numFinishedNodes = _constraintBuilder.GetNumFinishedNodes();
        }

        // CRITICAL FIX: Wait for optimization thread to complete
        // This ensures RunFinalOptimization() doesn't return until optimization is fully done
        // No timeout - wait indefinitely until optimization thread completes
        while (true)
        {
            bool isOptimizationInProgress;
            Thread? currentOptimizationThread;

            lock (_optimizationLock)
            {
                isOptimizationInProgress = _optimizationInProgress;
                currentOptimizationThread = _optimizationThread;
            }

            if (!isOptimizationInProgress)
            {
                // Optimization is not in progress, check if thread is still alive
                if (currentOptimizationThread != null && currentOptimizationThread.IsAlive)
                {
                    // Thread is still alive but optimization flag is false - thread might be finishing
                    // Wait a bit more
                    Thread.Sleep(100);
                    continue;
                }
                else
                {
                    // Optimization is complete
                    break;
                }
            }

            // Wait a bit before checking again
            Thread.Sleep(100);
        }

        // Final check: Wait for optimization thread to finish if it's still alive
        // No timeout - wait indefinitely until thread completes
        if (_optimizationThread != null && _optimizationThread.IsAlive)
        {
            _optimizationThread.Join(); // Wait indefinitely
        }
    }

    /// <summary>
    /// Runs optimization synchronously in the calling thread.
    /// Match C++ RunOptimization() implementation.
    /// Thread-safe: only one optimization runs at a time.
    /// If optimization is already running, increments pending count and returns.
    /// The running optimization will check pending count and run again if needed.
    /// </summary>
    private void RunOptimizationSync()
    {
        // Increment optimization requested counter (always, even if skipped)
        Interlocked.Increment(ref _optimizationRequested);

        lock (_optimizationLock)
        {
            // If optimization is already in progress, increment pending count and return
            if (_optimizationInProgress)
            {
                Interlocked.Increment(ref _pendingOptimizationCount);
                return;
            }

            _optimizationInProgress = true;
        }

        // Run optimization loop until no pending requests
        while (true)
        {
            try
            {
                // Use Copy-on-Write implementation if enabled, otherwise use original
                if (_useCopyOnWriteOptimization)
                {
                    RunOptimizationSyncInternal_CopyOnWrite();
                }
                else
                {
                    RunOptimizationSyncInternal();
                }

                // Increment completed counter
                Interlocked.Increment(ref _optimizationCompleted);
            }
            catch
            {
                throw;
            }

            // Check if there are pending optimization requests
            lock (_optimizationLock)
            {
                var pending = Interlocked.Read(ref _pendingOptimizationCount);
                if (pending > 0)
                {
                    // Reset pending count and continue loop
                    Interlocked.Exchange(ref _pendingOptimizationCount, 0);
                }
                else
                {
                    // No pending requests, exit loop
                    _optimizationInProgress = false;
                    break;
                }
            }
        }
    }

    /// <summary>
    /// Internal implementation of optimization (extracted from RunOptimizationSync).
    /// Assumes _optimizationInProgress is true.
    /// </summary>
    private void RunOptimizationSyncInternal()
    {
        try
        {
            // Match C++ RunOptimization(): Check if optimization problem is empty
            if (_optimizationProblem.SubmapData().IsEmpty)
            {
                return;
            }

            // Get trajectory states
            var trajectoryStates = GetTrajectoryStates();

            // Get landmark nodes
            var landmarkNodes = GetLandmarkNodes();

            var optimizationSubmapData = _optimizationProblem.SubmapData();
            var optimizationNodeData = _optimizationProblem.NodeData();

            // FIX: Match C++ (lines 1208-1229): Remove invalid constraints before optimization
            // CRITICAL: Must be inside lock to prevent race condition with OnConstraintBuilderResult()
            // which adds constraints inside a lock. Without lock, RemoveAll can run concurrently
            // with constraint additions, causing data corruption or lost constraints.
            List<IPoseGraph.Constraint> constraints;
            lock (_dataLock)
            {
                var removedCount = _constraints.RemoveAll(c =>
                    !optimizationNodeData.Contains(c.NodeId) ||
                    !optimizationSubmapData.Contains(c.SubmapId));

                // Get constraints snapshot inside lock to ensure consistency
                constraints = Constraints();
            }

            // Solve optimization problem (will use max_num_iterations set via SetMaxNumIterations)
            _optimizationProblem.Solve(constraints, trajectoryStates, landmarkNodes, maxNumIterationsOverride: null);

            // Invoke global SLAM optimization callback if set
            InvokeGlobalSlamOptimizationCallback();

            // Prepare callback data outside lock scope, but populate inside lock
            LocalizationResult? localizationResultToInvoke = null;
            (int TrajectoryId, long Time, Rigid3d Pose)? localizationSearchToInvoke = null;

            lock (_dataLock)  // ===== SINGLE LOCK START =====
            {
                // Store old submap poses before updating (for extrapolation)
                var oldSubmapPoses = new MapById<SubmapId, Rigid2d>();
                foreach (var kvp in _submapData)
                {
                    oldSubmapPoses.Insert(kvp.Id, TransformOperations.Project2D(kvp.Data.Pose));
                }

                // Get optimized data
                var optimizedNodeData = _optimizationProblem.NodeData();
                var optimizedSubmapData = _optimizationProblem.SubmapData();

                // Update ALL nodes (no lock per iteration)
                foreach (var kvp in optimizedNodeData)
                {
                    var nodeId = kvp.Id;
                    var nodeSpec = kvp.Data;

                    if (_trajectoryNodes.Contains(nodeId))
                    {
                        var node = _trajectoryNodes[nodeId];
                        // Match C++: global_pose = Embed3D(global_pose_2d) * Rotation(gravity_alignment)
                        var optimizedPose3D = TransformOperations.Embed3D(nodeSpec.GlobalPose2D) *
                            Rigid3d.FromRotation(node.ConstantData?.GravityAlignment ?? Quaternion.Identity);

                        // Update trajectory node pose
                        if (_trajectoryNodePoses.Contains(nodeId))
                        {
                            var nodePose = _trajectoryNodePoses[nodeId];
                            nodePose.GlobalPose = optimizedPose3D;
                            _trajectoryNodePoses[nodeId] = nodePose;
                        }

                        // Update trajectory node
                        node.GlobalPose = optimizedPose3D;
                        _trajectoryNodes[nodeId] = node;
                    }
                }

                // Extrapolate nodes not included in optimization
                foreach (var trajectoryId in optimizedNodeData.TrajectoryIds)
                {
                    if (optimizedNodeData.SizeOfTrajectoryOrZero(trajectoryId) == 0)
                        continue;

                    // Compute local_to_new_global from optimized submap data
                    var localToNewGlobal = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);

                    // Compute local_to_old_global from old submap poses
                    var localToOldGlobal = ComputeLocalToGlobalTransformFromSubmapPoses(oldSubmapPoses, trajectoryId);

                    // Compute transform from old global to new global
                    var oldGlobalToNewGlobal = localToNewGlobal * localToOldGlobal.Inverse();

                    // Find last optimized node
                    var trajectoryNodes = optimizedNodeData.BeginOfTrajectory(trajectoryId)
                        .Select(kvp => kvp.Id)
                        .OrderBy(id => id.NodeIndex)
                        .ToList();

                    if (trajectoryNodes.Count > 0)
                    {
                        var lastOptimizedNodeId = trajectoryNodes[^1];

                        // Find all nodes in trajectory after lastOptimizedNodeId
                        var nodesToUpdate = _trajectoryNodes
                            .Where(kvp => kvp.Id.TrajectoryId == trajectoryId &&
                                         kvp.Id.NodeIndex > lastOptimizedNodeId.NodeIndex)
                            .ToList();

                        foreach (var kvp in nodesToUpdate)
                        {
                            var node = kvp.Data;
                            node.GlobalPose = oldGlobalToNewGlobal * node.GlobalPose;
                            _trajectoryNodes[kvp.Id] = node;

                            // Update trajectory node pose if exists
                            if (_trajectoryNodePoses.Contains(kvp.Id))
                            {
                                var nodePose = _trajectoryNodePoses[kvp.Id];
                                nodePose.GlobalPose = node.GlobalPose;
                                _trajectoryNodePoses[kvp.Id] = nodePose;
                            }
                        }
                    }
                }

                // Update _globalSubmapPoses2D (ONLY - match C++)
                // Match C++ line 1280: data_.global_submap_poses_2d = submap_data
                var existingSubmapIds = _globalSubmapPoses2D.Select(kvp => kvp.Id).ToList();
                foreach (var existingId in existingSubmapIds)
                {
                    _globalSubmapPoses2D.Trim(existingId);
                }
                foreach (var kvp in optimizedSubmapData)
                {
                    _globalSubmapPoses2D.Insert(kvp.Id, kvp.Data);
                }

                // === LOOP CLOSURE OPTIMIZATION RESULT LOG ===
                // Count InterSubmap constraints (loop closures)
                var interSubmapCount = _constraints.Count(c => c.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap);
                var intraSubmapCount = _constraints.Count(c => c.ConstraintTag == IPoseGraph.Constraint.Tag.IntraSubmap);

                if (optimizedSubmapData.Count > 0)
                {
                    var submapPosesStr = string.Join(", ", optimizedSubmapData.Select(kvp =>
                        $"[{kvp.Id.TrajectoryId}:{kvp.Id.SubmapIndex}]=({kvp.Data.GlobalPose.Translation.X:F3},{kvp.Data.GlobalPose.Translation.Y:F3},{kvp.Data.GlobalPose.Rotation * 180 / Math.PI:F1}°)"));
                    
                    // Log node poses if available
                    var nodePosesStr = string.Join(", ", _trajectoryNodes.Take(10).Select(kvp =>
                        $"[{kvp.Id.TrajectoryId}:{kvp.Id.NodeIndex}]=({kvp.Data.GlobalPose.Translation.X:F3},{kvp.Data.GlobalPose.Translation.Y:F3})"));
                }

                // REFACTOR: DO NOT update _submapData.Pose (removed dual source of truth)
                // C++ does NOT update data_.submap_data with global pose after optimization.
                // C++ only updates data_.global_submap_poses_2d and calculates pose dynamically in GetSubmapDataUnderLock().
                // GetSubmapData() will calculate pose dynamically from _globalSubmapPoses2D (source of truth).

                // Update landmark poses
                var landmarkData = _optimizationProblem.LandmarkData();
                foreach (var kvp in landmarkData)
                {
                    _landmarkPoses[kvp.Key] = kvp.Value;
                }

                // Update local to global transforms
                // Match C++: Use _globalSubmapPoses2D (which was just updated above)
                foreach (var trajectoryId in _trajectoryStates.Keys)
                {
                    var transform = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
                    _localToGlobalTransforms[trajectoryId] = transform;
                }

                // Match C++ (lines 1282-1301): Check for relocalization when first inter-submap constraint is found
                // Prepare callback data (don't call yet - will call outside lock)
                if (!_isRelocalized && _localizationMode)
                {
                    foreach (var constraint in _constraints)
                    {
                        if (constraint.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap &&
                            constraint.NodeId.TrajectoryId == _currentTrajectoryId)
                        {
                            _isRelocalized = true;

                            // Prepare callback data (invoke outside lock)
                            if (_trajectoryNodes.Contains(constraint.NodeId))
                            {
                                var matchingNode = _trajectoryNodes[constraint.NodeId];

                                localizationSearchToInvoke = (
                                    constraint.NodeId.TrajectoryId,
                                    matchingNode.ConstantData?.Time ?? 0,
                                    matchingNode.GlobalPose
                                );

                                localizationResultToInvoke = new LocalizationResult
                                {
                                    NodeId = constraint.NodeId,
                                    SubmapId = constraint.SubmapId,
                                    GlobalPose = matchingNode.GlobalPose,
                                    Score = constraint.Score
                                };
                            }
                            break;
                        }
                    }
                }
            } // ===== SINGLE LOCK END - match C++ unlock here =====

            // REFACTOR: Call callbacks OUTSIDE lock (matching C++ behavior)
            if (localizationSearchToInvoke.HasValue)
            {
                _localizationSearchCallback?.Invoke(
                    localizationSearchToInvoke.Value.TrajectoryId,
                    localizationSearchToInvoke.Value.Time,
                    localizationSearchToInvoke.Value.Pose);
            }
            if (localizationResultToInvoke != null)
            {
                _localizationResultCallback?.Invoke(localizationResultToInvoke.Value);
            }

            // Run trimmers after optimization
            RunTrimmers();
        }
        catch
        {
            throw;
        }
        // Note: _optimizationInProgress is managed by outer RunOptimizationSync function
    }

    #region Copy-on-Write Optimization

    /// <summary>
    /// Flag to enable Copy-on-Write optimization mode.
    /// When true, RunOptimizationSyncInternal_CopyOnWrite is used instead of RunOptimizationSyncInternal.
    /// Default: false (use original blocking implementation for backward compatibility).
    /// </summary>
    private bool _useCopyOnWriteOptimization = false;

    /// <summary>
    /// Enables or disables Copy-on-Write optimization mode.
    /// COW mode significantly reduces lock contention during optimization by:
    /// 1. Preparing updated data structures outside of lock
    /// 2. Swapping references atomically with minimal lock time
    /// This reduces AddNode blocking time by ~95-97% depending on node count.
    /// </summary>
    public void SetCopyOnWriteOptimization(bool enabled)
    {
        _useCopyOnWriteOptimization = enabled;
    }

    /// <summary>
    /// Gets whether Copy-on-Write optimization is enabled.
    /// </summary>
    public bool IsCopyOnWriteOptimizationEnabled => _useCopyOnWriteOptimization;

    /// <summary>
    /// Copy-on-Write implementation of RunOptimizationSyncInternal.
    /// Minimizes lock contention by preparing updated data outside of lock,
    /// then swapping references atomically.
    ///
    /// Phase 1 (NO LOCK or MINIMAL LOCK):
    /// - Snapshot current constraints
    /// - Solve optimization problem (CPU intensive)
    /// - Build new MapById instances with updated poses
    ///
    /// Phase 2 (MINIMAL LOCK ~1-5ms):
    /// - Merge nodes added during Phase 1
    /// - Swap references atomically
    /// - Update remaining dictionaries
    /// </summary>
    private void RunOptimizationSyncInternal_CopyOnWrite()
    {
        try
        {
            // ═══════════════════════════════════════════════════════════════
            // EARLY EXIT: Check if optimization problem is empty
            // ═══════════════════════════════════════════════════════════════
            if (_optimizationProblem.SubmapData().IsEmpty)
            {
                return;
            }

            // ═══════════════════════════════════════════════════════════════
            // PHASE 1: PREPARE - Minimal lock for snapshots, then NO LOCK
            // ═══════════════════════════════════════════════════════════════

            // 1.1 Get input data (read-only access to optimization problem)
            var trajectoryStates = GetTrajectoryStates();
            var landmarkNodes = GetLandmarkNodes();
            var optimizationSubmapData = _optimizationProblem.SubmapData();
            var optimizationNodeData = _optimizationProblem.NodeData();

            // 1.2 Snapshot constraints and current state with minimal lock
            List<IPoseGraph.Constraint> constraintsSnapshot;
            MapById<SubmapId, Rigid2d> oldSubmapPosesSnapshot;  // Rigid2d for ComputeLocalToGlobalTransformFromSubmapPoses
            MapById<SubmapId, Optimization.SubmapSpec2D> oldGlobalSubmapPoses2DSnapshot;  // SubmapSpec2D for reference
            MapById<NodeId, TrajectoryNode> currentNodesSnapshot;
            MapById<NodeId, TrajectoryNodePose> currentNodePosesSnapshot;
            Dictionary<int, IPoseGraph.TrajectoryState> trajectoryStatesSnapshot;
            int currentTrajectoryIdSnapshot;
            bool localizationModeSnapshot;
            bool isRelocalizedSnapshot;

            lock (_dataLock)  // Lock #1: ~0.5-2ms - snapshot only
            {
                // Remove invalid constraints (must be done under lock)
                _constraints.RemoveAll(c =>
                    !optimizationNodeData.Contains(c.NodeId) ||
                    !optimizationSubmapData.Contains(c.SubmapId));

                // Snapshot current state (shallow clone - fast for structs)
                constraintsSnapshot = new List<IPoseGraph.Constraint>(_constraints);

                // Create oldSubmapPosesSnapshot as MapById<SubmapId, Rigid2d> for ComputeLocalToGlobalTransformFromSubmapPoses
                oldSubmapPosesSnapshot = new MapById<SubmapId, Rigid2d>();
                foreach (var kvp in _globalSubmapPoses2D)
                {
                    oldSubmapPosesSnapshot.Insert(kvp.Id, kvp.Data.GlobalPose);
                }
                oldGlobalSubmapPoses2DSnapshot = _globalSubmapPoses2D.ShallowClone();

                currentNodesSnapshot = _trajectoryNodes.ShallowClone();
                currentNodePosesSnapshot = _trajectoryNodePoses.ShallowClone();
                trajectoryStatesSnapshot = new Dictionary<int, IPoseGraph.TrajectoryState>(_trajectoryStates);
                currentTrajectoryIdSnapshot = _currentTrajectoryId;
                localizationModeSnapshot = _localizationMode;
                isRelocalizedSnapshot = _isRelocalized;
            }

            // 1.3 Solve optimization problem (CPU intensive, NO LOCK)
            // This is the main computational work - can take 50-200ms
            _optimizationProblem.Solve(constraintsSnapshot, trajectoryStates, landmarkNodes, maxNumIterationsOverride: null);

            // Invoke global SLAM optimization callback (outside lock)
            InvokeGlobalSlamOptimizationCallback();

            // 1.4 Get optimized results
            var optimizedNodeData = _optimizationProblem.NodeData();
            var optimizedSubmapData = _optimizationProblem.SubmapData();

            // 1.5 Build NEW data structures (NO LOCK - this is where COW saves time)
            var newTrajectoryNodes = new MapById<NodeId, TrajectoryNode>();
            var newTrajectoryNodePoses = new MapById<NodeId, TrajectoryNodePose>();
            var newGlobalSubmapPoses2D = new MapById<SubmapId, Optimization.SubmapSpec2D>();
            var newLandmarkPoses = new Dictionary<string, Rigid3d>();
            var newLocalToGlobalTransforms = new Dictionary<int, Rigid3d>();

            // 1.5.1 Update optimized nodes
            // CRITICAL: TrajectoryNode is a class (reference type), so we must clone
            // to avoid modifying the original while other threads may be reading it.
            foreach (var kvp in optimizedNodeData)
            {
                var nodeId = kvp.Id;
                var nodeSpec = kvp.Data;

                if (currentNodesSnapshot.Contains(nodeId))
                {
                    var originalNode = currentNodesSnapshot[nodeId];
                    // CRITICAL FIX: Clone the node to avoid data race!
                    // ShallowClone shares ConstantData (immutable) but creates new GlobalPose
                    var node = originalNode.ShallowClone();

                    // Match C++: global_pose = Embed3D(global_pose_2d) * Rotation(gravity_alignment)
                    var optimizedPose3D = TransformOperations.Embed3D(nodeSpec.GlobalPose2D) *
                        Rigid3d.FromRotation(node.ConstantData?.GravityAlignment ?? Quaternion.Identity);

                    // Now safe to modify - this is our own copy
                    node.GlobalPose = optimizedPose3D;
                    newTrajectoryNodes.Insert(nodeId, node);

                    // Create updated node pose
                    var nodePose = currentNodePosesSnapshot.Contains(nodeId)
                        ? currentNodePosesSnapshot[nodeId]
                        : new TrajectoryNodePose();
                    nodePose.GlobalPose = optimizedPose3D;
                    newTrajectoryNodePoses.Insert(nodeId, nodePose);
                }
            }

            // 1.5.2 Extrapolate nodes not included in optimization
            // Pre-build optimized submap poses once (avoid rebuilding in loop)
            var optimizedSubmapPosesMap = new MapById<SubmapId, Optimization.SubmapSpec2D>();
            foreach (var kvp in optimizedSubmapData)
            {
                optimizedSubmapPosesMap.Insert(kvp.Id, kvp.Data);
            }

            foreach (var trajectoryId in optimizedNodeData.TrajectoryIds)
            {
                if (optimizedNodeData.SizeOfTrajectoryOrZero(trajectoryId) == 0)
                    continue;

                // Compute local_to_new_global from optimized submap poses
                var localToNewGlobal = ComputeLocalToGlobalTransform(optimizedSubmapPosesMap, trajectoryId);

                // Compute local_to_old_global from snapshot
                var localToOldGlobal = ComputeLocalToGlobalTransformFromSubmapPoses(oldSubmapPosesSnapshot, trajectoryId);

                // Compute transform from old global to new global
                var oldGlobalToNewGlobal = localToNewGlobal * localToOldGlobal.Inverse();

                // Find last optimized node
                var trajectoryNodes = optimizedNodeData.BeginOfTrajectory(trajectoryId)
                    .Select(kvp => kvp.Id)
                    .OrderBy(id => id.NodeIndex)
                    .ToList();

                if (trajectoryNodes.Count > 0)
                {
                    var lastOptimizedNodeId = trajectoryNodes[^1];

                    // Find all nodes in snapshot after lastOptimizedNodeId
                    var nodesToExtrapolate = currentNodesSnapshot
                        .Where(kvp => kvp.Id.TrajectoryId == trajectoryId &&
                                     kvp.Id.NodeIndex > lastOptimizedNodeId.NodeIndex)
                        .ToList();

                    foreach (var kvp in nodesToExtrapolate)
                    {
                        // CRITICAL FIX: Clone the node to avoid data race!
                        var node = kvp.Data.ShallowClone();
                        node.GlobalPose = oldGlobalToNewGlobal * kvp.Data.GlobalPose;

                        // Only insert if not already in newTrajectoryNodes
                        if (!newTrajectoryNodes.Contains(kvp.Id))
                        {
                            newTrajectoryNodes.Insert(kvp.Id, node);
                        }

                        // Update or insert node pose
                        var nodePose = currentNodePosesSnapshot.Contains(kvp.Id)
                            ? currentNodePosesSnapshot[kvp.Id]
                            : new TrajectoryNodePose();
                        nodePose.GlobalPose = node.GlobalPose;
                        if (!newTrajectoryNodePoses.Contains(kvp.Id))
                        {
                            newTrajectoryNodePoses.Insert(kvp.Id, nodePose);
                        }
                    }
                }
            }

            // 1.5.3 Build new submap poses
            foreach (var kvp in optimizedSubmapData)
            {
                newGlobalSubmapPoses2D.Insert(kvp.Id, kvp.Data);
            }

            // 1.5.4 Update landmark poses
            var landmarkData = _optimizationProblem.LandmarkData();
            foreach (var kvp in landmarkData)
            {
                newLandmarkPoses[kvp.Key] = kvp.Value;
            }

            // 1.5.5 Compute new transforms using new submap poses
            foreach (var trajectoryId in trajectoryStatesSnapshot.Keys)
            {
                var transform = ComputeLocalToGlobalTransform(newGlobalSubmapPoses2D, trajectoryId);
                newLocalToGlobalTransforms[trajectoryId] = transform;
            }

            // ═══════════════════════════════════════════════════════════════
            // PHASE 2: SWAP - Minimal lock for atomic reference swap
            // ═══════════════════════════════════════════════════════════════

            LocalizationResult? localizationResultToInvoke = null;
            (int TrajectoryId, long Time, Rigid3d Pose)? localizationSearchToInvoke = null;

            lock (_dataLock)  // Lock #2: ~1-5ms - merge and swap only
            {
                // 2.1 Merge nodes that were added during Phase 1
                // These are nodes in _trajectoryNodes that are NOT in currentNodesSnapshot
                var nodesAddedDuringPhase1 = _trajectoryNodes.GetIdsDifference(currentNodesSnapshot).ToList();

                if (nodesAddedDuringPhase1.Count > 0)
                {
                    // Compute transform delta to apply to newly added nodes
                    foreach (var nodeId in nodesAddedDuringPhase1)
                    {
                        var node = _trajectoryNodes[nodeId];

                        // Compute transform from old global to new global for this trajectory
                        if (newLocalToGlobalTransforms.TryGetValue(nodeId.TrajectoryId, out var newTransform) &&
                            _localToGlobalTransforms.TryGetValue(nodeId.TrajectoryId, out var oldTransform))
                        {
                            var oldToNew = newTransform * oldTransform.Inverse();
                            node.GlobalPose = oldToNew * node.GlobalPose;
                        }

                        // Add to new structures
                        if (!newTrajectoryNodes.Contains(nodeId))
                        {
                            newTrajectoryNodes.Insert(nodeId, node);
                        }
                        else
                        {
                            newTrajectoryNodes[nodeId] = node;
                        }

                        // Update node pose
                        var nodePose = new TrajectoryNodePose { GlobalPose = node.GlobalPose };
                        if (!newTrajectoryNodePoses.Contains(nodeId))
                        {
                            newTrajectoryNodePoses.Insert(nodeId, nodePose);
                        }
                        else
                        {
                            newTrajectoryNodePoses[nodeId] = nodePose;
                        }
                    }
                }

                // 2.2 Also merge any submaps added during Phase 1
                var submapsAddedDuringPhase1 = _globalSubmapPoses2D.GetIdsDifference(oldGlobalSubmapPoses2DSnapshot).ToList();
                foreach (var submapId in submapsAddedDuringPhase1)
                {
                    if (!newGlobalSubmapPoses2D.Contains(submapId))
                    {
                        var submapPose = _globalSubmapPoses2D[submapId];
                        newGlobalSubmapPoses2D.Insert(submapId, submapPose);
                    }
                }

                // 2.3 Atomic swap - replace old data with new data
                _trajectoryNodes.ReplaceWith(newTrajectoryNodes);
                _trajectoryNodePoses.ReplaceWith(newTrajectoryNodePoses);
                _globalSubmapPoses2D.ReplaceWith(newGlobalSubmapPoses2D);

                // 2.4 Update dictionaries
                foreach (var kvp in newLandmarkPoses)
                {
                    _landmarkPoses[kvp.Key] = kvp.Value;
                }
                foreach (var kvp in newLocalToGlobalTransforms)
                {
                    _localToGlobalTransforms[kvp.Key] = kvp.Value;
                }

                // 2.5 Check for relocalization (same logic as original)
                if (!_isRelocalized && _localizationMode)
                {
                    foreach (var constraint in _constraints)
                    {
                        if (constraint.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap &&
                            constraint.NodeId.TrajectoryId == _currentTrajectoryId)
                        {
                            _isRelocalized = true;

                            if (_trajectoryNodes.Contains(constraint.NodeId))
                            {
                                var matchingNode = _trajectoryNodes[constraint.NodeId];

                                localizationSearchToInvoke = (
                                    constraint.NodeId.TrajectoryId,
                                    matchingNode.ConstantData?.Time ?? 0,
                                    matchingNode.GlobalPose
                                );

                                localizationResultToInvoke = new LocalizationResult
                                {
                                    NodeId = constraint.NodeId,
                                    SubmapId = constraint.SubmapId,
                                    GlobalPose = matchingNode.GlobalPose,
                                    Score = constraint.Score
                                };
                            }
                            break;
                        }
                    }
                }
            } // End of Phase 2 lock

            // Invoke callbacks OUTSIDE lock (matching C++ behavior)
            if (localizationSearchToInvoke.HasValue)
            {
                _localizationSearchCallback?.Invoke(
                    localizationSearchToInvoke.Value.TrajectoryId,
                    localizationSearchToInvoke.Value.Time,
                    localizationSearchToInvoke.Value.Pose);
            }
            if (localizationResultToInvoke != null)
            {
                _localizationResultCallback?.Invoke(localizationResultToInvoke.Value);
            }

            // Run trimmers after optimization
            RunTrimmers();
        }
        catch
        {
            throw;
        }
    }

    /// <summary>
    /// Gets memory usage estimation for Copy-on-Write overhead.
    /// Returns the peak additional memory used during COW optimization in bytes.
    /// </summary>
    public CopyOnWriteMemoryEstimate GetCopyOnWriteMemoryEstimate()
    {
        lock (_dataLock)
        {
            // TrajectoryNode: class with Data (~200 bytes) + GlobalPose (56 bytes) = ~256 bytes
            const int trajectoryNodeSize = 256;

            // TrajectoryNodePose: struct with GlobalPose (56 bytes) + optional ConstantPoseData (64 bytes) = ~120 bytes
            const int trajectoryNodePoseSize = 120;

            // SubmapSpec2D: struct with Rigid2d (24 bytes) = ~24 bytes
            const int submapSpec2DSize = 24;

            // Constraint: struct ~100 bytes
            const int constraintSize = 100;

            var nodeCount = _trajectoryNodes.Count;
            var submapCount = _globalSubmapPoses2D.Count;
            var constraintCount = _constraints.Count;

            // Calculate memory for each structure
            var trajectoryNodesMemory = _trajectoryNodes.EstimateMemoryUsageBytes(trajectoryNodeSize);
            var trajectoryNodePosesMemory = _trajectoryNodePoses.EstimateMemoryUsageBytes(trajectoryNodePoseSize);
            var submapPosesMemory = _globalSubmapPoses2D.EstimateMemoryUsageBytes(submapSpec2DSize);
            var constraintsMemory = constraintCount * constraintSize + 32; // List overhead

            // During COW, we create copies of all these structures
            var peakOverhead = trajectoryNodesMemory + trajectoryNodePosesMemory + submapPosesMemory + constraintsMemory;

            return new CopyOnWriteMemoryEstimate
            {
                NodeCount = nodeCount,
                SubmapCount = submapCount,
                ConstraintCount = constraintCount,
                TrajectoryNodesBytes = trajectoryNodesMemory,
                TrajectoryNodePosesBytes = trajectoryNodePosesMemory,
                SubmapPosesBytes = submapPosesMemory,
                ConstraintsBytes = constraintsMemory,
                PeakOverheadBytes = peakOverhead,
                PeakOverheadMB = peakOverhead / (1024.0 * 1024.0)
            };
        }
    }

    #endregion

    /// <summary>
    /// Runs optimization asynchronously on a separate thread (legacy method).
    /// DEPRECATED: This method is kept for backward compatibility but should not be used
    /// from work queue context. Use RunOptimizationSync() instead when called from work queue.
    /// </summary>
    private void RunOptimization()
    {
        lock (_optimizationLock)
        {
            // If optimization is already in progress, skip
            if (_optimizationInProgress) return;
        }

        // Run optimization on dedicated thread
        _optimizationThread = new Thread(() =>
        {
            Thread.BeginThreadAffinity();
            try
            {
                // Call the synchronous version (which sets _optimizationInProgress itself)
                RunOptimizationSync();
            }
            catch
            {
                throw;
            }
            finally
            {
                Thread.EndThreadAffinity();
            }
        })
        {
            Priority = ThreadPriority.BelowNormal,
            IsBackground = true,
            Name = "CartographerOptimization2D"
        };
        _optimizationThread.Start();
    }

    /// <summary>
    /// Invokes the global SLAM optimization callback with current submap and node IDs.
    /// </summary>
    private void InvokeGlobalSlamOptimizationCallback()
    {
        if (_globalSlamOptimizationCallback == null)
        {
            return;
        }

        // Collect optimized submap IDs per trajectory
        var submapIds = new Dictionary<int, SubmapId>();
        foreach (var kvp in _submapData)
        {
            submapIds[kvp.Id.TrajectoryId] = kvp.Id;
        }

        // Collect optimized node IDs per trajectory (use the most recent node)
        var nodeIds = new Dictionary<int, NodeId>();
        foreach (var kvp in _trajectoryNodes)
        {
            // Keep the latest node for each trajectory
            if (!nodeIds.TryGetValue(kvp.Id.TrajectoryId, out NodeId value) || kvp.Id.NodeIndex > value.NodeIndex)
            {
                value = kvp.Id;
                nodeIds[kvp.Id.TrajectoryId] = value;
            }
        }

        _globalSlamOptimizationCallback(submapIds, nodeIds);
    }

    public override MapById<SubmapId, IPoseGraph.SubmapData> GetAllSubmapData()
    {
        lock (_dataLock)
        {
            // Match C++ GetSubmapDataUnderLock(): Calculate global pose dynamically from _globalSubmapPoses2D
            var result = new MapById<SubmapId, IPoseGraph.SubmapData>();
            foreach (var kvp in _submapData)
            {
                result.Insert(kvp.Id, GetSubmapDataUnsafe(kvp.Id));
            }
            return result;
        }
    }

    /// <summary>
    /// Monotonically increasing version counter. Incremented each time a node is
    /// appended via AddNode (i.e. each successful range data insertion).
    /// Safe to read without locking (volatile read).
    /// </summary>
    public int NodeInsertionVersion => Volatile.Read(ref _nodeInsertionVersion);

    /// <summary>
    /// Non-blocking attempt to get all submap data + transforms for visualization.
    /// Uses Monitor.TryEnter to avoid blocking the sensor data processing path.
    /// Returns false if the lock is currently held (e.g., during optimization or AddNode).
    /// Also returns the current nodeInsertionVersion for change detection.
    /// </summary>
    public bool TryGetSubmapSnapshot(
        out MapById<SubmapId, IPoseGraph.SubmapData>? submapData,
        out Rigid3d transformToMap,
        out int snapshotVersion)
    {
        submapData = null;
        transformToMap = Rigid3d.Identity;
        snapshotVersion = 0;

        if (!Monitor.TryEnter(_dataLock))
            return false;

        try
        {
            var result = new MapById<SubmapId, IPoseGraph.SubmapData>();
            foreach (var kvp in _submapData)
            {
                result.Insert(kvp.Id, GetSubmapDataUnsafe(kvp.Id));
            }
            submapData = result;
            transformToMap = _transformToMap ?? Rigid3d.Identity;
            snapshotVersion = _nodeInsertionVersion;
            return true;
        }
        finally
        {
            Monitor.Exit(_dataLock);
        }
    }

    public override IPoseGraph.SubmapData GetSubmapData(SubmapId submapId)
    {
        lock (_dataLock)
        {
            return GetSubmapDataUnsafe(submapId);
        }
    }

    /// <summary>
    /// Gets submap data with dynamic global pose calculation (matches C++ GetSubmapDataUnderLock).
    /// Assumes lock held.
    /// </summary>
    private IPoseGraph.SubmapData GetSubmapDataUnsafe(SubmapId submapId)
    {
        // Match C++ GetSubmapDataUnderLock() (dòng 1565-1582)
        if (!_submapData.Contains(submapId))
        {
            return new IPoseGraph.SubmapData(null, Rigid3d.Identity);
        }

        var submapData = _submapData[submapId];
        var submap = submapData.Submap;

        // Match C++: if (data_.global_submap_poses_2d.Contains(submap_id))
        if (_globalSubmapPoses2D.Contains(submapId))
        {
            // We already have an optimized pose.
            // Match C++: return {submap, transform::Embed3D(data_.global_submap_poses_2d.at(submap_id).global_pose)}
            var optimizedPose2D = _globalSubmapPoses2D[submapId].GlobalPose;
            var optimizedPose3D = TransformOperations.Embed3D(optimizedPose2D);
            return new IPoseGraph.SubmapData(submap, optimizedPose3D);
        }

        // We have to extrapolate.
        // Match C++: return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, submap_id.trajectory_id) * submap->local_pose()}
        var transform = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, submapId.TrajectoryId);
        if (submap is Mapping.D2D.Submap2D submap2D)
        {
            var extrapolatedPose = transform * submap2D.LocalPose;
            return new IPoseGraph.SubmapData(submap, extrapolatedPose);
        }

        // Fallback for non-2D submaps
        return new IPoseGraph.SubmapData(submap, Rigid3d.Identity);
    }

    public override MapById<SubmapId, IPoseGraph.SubmapPose> GetAllSubmapPoses()
    {
        lock (_dataLock)
        {
            // Match C++ GetAllSubmapPoses() (dòng 1527-1539): Calculate pose dynamically using GetSubmapDataUnderLock
            var result = new MapById<SubmapId, IPoseGraph.SubmapPose>();
            foreach (var kvp in _submapData)
            {
                // Match C++: auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
                var submapData = GetSubmapDataUnsafe(kvp.Id);

                var version = 0;
                if (submapData.Submap is Mapping.D2D.Submap2D submap2D)
                {
                    version = submap2D.NumRangeData;
                }
                // Match C++: submap_poses.Insert(..., SubmapPose{submap_data.submap->num_range_data(), submap_data.pose})
                result.Insert(kvp.Id, new IPoseGraph.SubmapPose(version, submapData.Pose));
            }
            return result;
        }
    }

    public override Rigid3d GetLocalToGlobalTransform(int trajectoryId)
    {
        lock (_dataLock)
        {
            return GetLocalToGlobalTransformUnsafe(trajectoryId);
        }
    }

    public override MapById<NodeId, TrajectoryNode> GetTrajectoryNodes()
    {
        lock (_dataLock)
        {
            // Return a copy to avoid external modification
            var result = new MapById<NodeId, TrajectoryNode>();
            foreach (var kvp in _trajectoryNodes)
            {
                result.Insert(kvp.Id, kvp.Data);
            }
            return result;
        }
    }

    public override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
    {
        lock (_dataLock)
        {
            // CRITICAL FIX: Match C++ implementation - compute from trajectory_nodes on demand
            // instead of using separate _trajectoryNodePoses member variable.
            // This matches C++ GetTrajectoryNodePoses() which creates a new MapById and
            // populates it from data_.trajectory_nodes.
            var result = new MapById<NodeId, TrajectoryNodePose>();
            foreach (var kvp in _trajectoryNodes)
            {
                ConstantPoseData? constantPoseData = null;
                if (kvp.Data.ConstantData != null)
                {
                    constantPoseData = new ConstantPoseData
                    {
                        Time = kvp.Data.ConstantData.Time,
                        LocalPose = kvp.Data.ConstantData.LocalPose
                    };
                }
                result.Insert(kvp.Id, new TrajectoryNodePose
                {
                    GlobalPose = kvp.Data.GlobalPose,
                    ConstantPoseData = constantPoseData
                });
            }
            return result;
        }
    }

    public override Dictionary<int, IPoseGraph.TrajectoryState> GetTrajectoryStates()
    {
        lock (_dataLock)
        {
            return new Dictionary<int, IPoseGraph.TrajectoryState>(_trajectoryStates);
        }
    }

    public override Dictionary<string, Rigid3d> GetLandmarkPoses()
    {
        lock (_dataLock)
        {
            return new Dictionary<string, Rigid3d>(_landmarkPoses);
        }
    }

    public override void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false)
    {
        lock (_dataLock)
        {
            _landmarkPoses[landmarkId] = globalPose;
            if (!_landmarkNodes.TryGetValue(landmarkId, out var node))
            {
                node = new IPoseGraph.LandmarkNode();
            }
            node.GlobalLandmarkPose = globalPose;
            node.Frozen = frozen;
            _landmarkNodes[landmarkId] = node;
        }
    }

    public override void DeleteTrajectory(int trajectoryId)
    {
        lock (_dataLock)
        {
            _trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Deleted;

            // Remove trajectory data
            _trajectoryData.Remove(trajectoryId);
            _imuData.Remove(trajectoryId);
            _odometryData.Remove(trajectoryId);
            _fixedFramePoseData.Remove(trajectoryId);
            _localToGlobalTransforms.Remove(trajectoryId);
        }

        // Remove nodes and submaps for this trajectory
        var nodesToRemove = new List<NodeId>();
        foreach (var kvp in _trajectoryNodes)
        {
            if (kvp.Id.TrajectoryId == trajectoryId)
            {
                nodesToRemove.Add(kvp.Id);
            }
        }
        foreach (var nodeId in nodesToRemove)
        {
            _trajectoryNodes.Trim(nodeId);
            _trajectoryNodePoses.Trim(nodeId);
        }

        var submapsToRemove = new List<SubmapId>();
        foreach (var kvp in _submapData)
        {
            if (kvp.Id.TrajectoryId == trajectoryId)
            {
                submapsToRemove.Add(kvp.Id);
            }
        }
        foreach (var submapId in submapsToRemove)
        {
            _submapData.Trim(submapId);
            _finishedSubmapIds.Remove(submapId);
        }

        // Remove constraints involving this trajectory
        _constraints.RemoveAll(c =>
            c.SubmapId.TrajectoryId == trajectoryId ||
            c.NodeId.TrajectoryId == trajectoryId);
    }

    public override bool IsTrajectoryFinished(int trajectoryId)
    {
        return _trajectoryStates.TryGetValue(trajectoryId, out var state) &&
               state == IPoseGraph.TrajectoryState.Finished;
    }

    public override bool IsTrajectoryFrozen(int trajectoryId)
    {
        return _trajectoryStates.TryGetValue(trajectoryId, out var state) &&
               state == IPoseGraph.TrajectoryState.Frozen;
    }

    public override Dictionary<int, IPoseGraph.TrajectoryData> GetTrajectoryData()
    {
        // FIX: Added lock to match C++ (pose_graph_2d.cc:1429)
        lock (_dataLock)
        {
            return new Dictionary<int, IPoseGraph.TrajectoryData>(_trajectoryData);
        }
    }

    public override List<IPoseGraph.Constraint> Constraints()
    {
        lock (_dataLock)
        {
            return [.. _constraints];
        }
    }

    public override Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps)
    {
        // FIX: Added lock for thread safety - reads multiple member variables
        lock (_dataLock)
        {
            var constraints = new List<Models.Mapping.PoseGraph.Constraint>();
            foreach (var constraint in _constraints)
            {
                constraints.Add(ConstraintOperations.ToProto(constraint));
            }

            var proto = new Models.Mapping.PoseGraph
            {
                Constraints = constraints,
                Trajectories = []
            };

            // Add trajectories
            var trajectoryIds = new HashSet<int>();
            foreach (var kvp in _trajectoryNodes)
            {
                trajectoryIds.Add(kvp.Id.TrajectoryId);
            }
            foreach (var kvp in _submapData)
            {
                trajectoryIds.Add(kvp.Id.TrajectoryId);
            }

            foreach (var trajectoryId in trajectoryIds)
            {
                var trajectory = new Models.Mapping.Trajectory
                {
                    TrajectoryId = trajectoryId,
                    Nodes = [],
                    Submaps = []
                };

                // Add nodes
                foreach (var kvp in _trajectoryNodes)
                {
                    if (kvp.Id.TrajectoryId == trajectoryId)
                    {
                        // CRITICAL FIX: Read GlobalPose from _trajectoryNodes instead of _trajectoryNodePoses
                        // because _trajectoryNodePoses is only populated when loading from proto
                        var nodePose = kvp.Data.GlobalPose;
                        trajectory.Nodes.Add(new Models.Mapping.Trajectory.Node(
                            kvp.Id.NodeIndex,
                            kvp.Data.ConstantData?.Time ?? 0,
                            (Rigid3dProto)nodePose
                        ));
                    }
                }

                // Add submaps
                foreach (var kvp in _submapData)
                {
                    if (kvp.Id.TrajectoryId == trajectoryId)
                    {
                        if (kvp.Data.Submap is Mapping.D2D.Submap2D submap2D)
                        {
                            if (includeUnfinishedSubmaps || submap2D.InsertionFinished)
                            {
                                trajectory.Submaps.Add(new Models.Mapping.Trajectory.Submap(
                                    kvp.Id.SubmapIndex,
                                    (Rigid3dProto)kvp.Data.Pose
                                ));
                            }
                        }
                    }
                }

                proto.Trajectories.Add(trajectory);
            }

            // Add landmark poses
            foreach (var kvp in _landmarkPoses)
            {
                proto.LandmarkPoses.Add(new Models.Mapping.PoseGraph.LandmarkPose
                {
                    LandmarkId = kvp.Key,
                    GlobalPose = (Rigid3dProto)kvp.Value
                });
            }

            // Match C++: serialize transform_to_map when set
            if (_transformToMap.HasValue)
            {
                proto.TransformToMap = (Rigid3dProto)_transformToMap.Value;
            }

            return proto;
        }
    }

    public override void SetTransformToMap(Rigid3d transform)
    {
        lock (_dataLock)
        {
            _transformToMap = transform;
        }
    }

    public override Rigid3d GetTransformToMap()
    {
        lock (_dataLock)
        {
            return _transformToMap ?? Rigid3d.Identity;
        }
    }

    public override void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback)
    {
        _globalSlamOptimizationCallback = callback;
    }

    // PoseGraph implementation
    // Thread-safe: Uses work queue pattern similar to Cartographer C++
    public override void AddImuData(int trajectoryId, ImuData imuData)
    {
        // Capture by value to avoid closure issues
        var capturedTrajectoryId = trajectoryId;
        var capturedImuData = imuData;

        AddWorkItem("", () =>
        {
            lock (_dataLock)
            {
                if (!_imuData.TryGetValue(capturedTrajectoryId, out var data))
                {
                    data = [];
                    _imuData[capturedTrajectoryId] = data;
                }
                data.Add(capturedImuData);
            }
            return WorkItemResult.DoNotRunOptimization;
        });
    }

    public override void AddOdometryData(int trajectoryId, OdometryData odometryData)
    {
        var capturedTrajectoryId = trajectoryId;
        var capturedOdometryData = odometryData;

        AddWorkItem("", () =>
        {
            lock (_dataLock)
            {
                if (!_odometryData.TryGetValue(capturedTrajectoryId, out var data))
                {
                    data = [];
                    _odometryData[capturedTrajectoryId] = data;
                }
                data.Add(capturedOdometryData);
            }
            return WorkItemResult.DoNotRunOptimization;
        });
    }

    public override void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData)
    {
        var capturedTrajectoryId = trajectoryId;
        var capturedFixedFramePoseData = fixedFramePoseData;

        AddWorkItem($"AddFixedFramePoseData trajectoryId={trajectoryId}", () =>
        {
            lock (_dataLock)
            {
                if (!_fixedFramePoseData.TryGetValue(capturedTrajectoryId, out var data))
                {
                    data = [];
                    _fixedFramePoseData[capturedTrajectoryId] = data;
                }
                data.Add(capturedFixedFramePoseData);
            }
            return WorkItemResult.DoNotRunOptimization;
        });
    }

    /// <summary>
    /// Trims sensor data for a finished trajectory, removing entries older than the
    /// oldest remaining node's time. This bounds memory growth for long-running sessions.
    /// Must be called under _dataLock.
    /// </summary>
    private void TrimSensorDataForFinishedTrajectory(int trajectoryId)
    {
        // Find the oldest node time for this trajectory
        long oldestNodeTime = long.MaxValue;
        foreach (var kvp in _trajectoryNodes.BeginOfTrajectory(trajectoryId))
        {
            var time = kvp.Data.ConstantData?.Time ?? long.MaxValue;
            if (time < oldestNodeTime)
                oldestNodeTime = time;
        }

        if (oldestNodeTime == long.MaxValue)
            return; // No nodes found, nothing to trim

        // Trim IMU data: remove entries before oldest node, keeping last one before for interpolation
        if (_imuData.TryGetValue(trajectoryId, out var imuList) && imuList.Count > 1)
        {
            int lastBeforeIdx = -1;
            for (int i = 0; i < imuList.Count; i++)
            {
                if (imuList[i].Time < oldestNodeTime)
                    lastBeforeIdx = i;
                else
                    break; // List is sorted by time (added chronologically)
            }
            // Keep one entry before oldest node for interpolation, remove the rest
            if (lastBeforeIdx > 0)
            {
                imuList.RemoveRange(0, lastBeforeIdx);
            }
        }

        // Trim odometry data
        if (_odometryData.TryGetValue(trajectoryId, out var odomList) && odomList.Count > 1)
        {
            int lastBeforeIdx = -1;
            for (int i = 0; i < odomList.Count; i++)
            {
                if (odomList[i].Time < oldestNodeTime)
                    lastBeforeIdx = i;
                else
                    break;
            }
            if (lastBeforeIdx > 0)
            {
                odomList.RemoveRange(0, lastBeforeIdx);
            }
        }

        // Trim fixed frame pose data
        if (_fixedFramePoseData.TryGetValue(trajectoryId, out var ffpList) && ffpList.Count > 1)
        {
            int lastBeforeIdx = -1;
            for (int i = 0; i < ffpList.Count; i++)
            {
                if (ffpList[i].Time < oldestNodeTime)
                    lastBeforeIdx = i;
                else
                    break;
            }
            if (lastBeforeIdx > 0)
            {
                ffpList.RemoveRange(0, lastBeforeIdx);
            }
        }
    }

    public override void AddLandmarkData(int trajectoryId, LandmarkData landmarkData)
    {
        var capturedTrajectoryId = trajectoryId;
        var capturedLandmarkData = landmarkData;

        AddWorkItem("", () =>
        {
            lock (_dataLock)
            {
                foreach (var observation in capturedLandmarkData.LandmarkObservations)
                {
                    if (!_landmarkNodes.TryGetValue(observation.Id, out var node))
                    {
                        node = new IPoseGraph.LandmarkNode();
                    }
                    node.LandmarkObservations.Add(new IPoseGraph.LandmarkNode.LandmarkObservation(
                        capturedTrajectoryId,
                        capturedLandmarkData.Time,
                        observation.LandmarkToTrackingTransform,
                        observation.TranslationWeight,
                        observation.RotationWeight
                    ));
                    _landmarkNodes[observation.Id] = node;
                }
            }
            return WorkItemResult.DoNotRunOptimization;
        });
    }

    /// <summary>
    /// Manually compute a constraint between a node and submap using global scan matching.
    /// Match C++: manualComputeConstraint (pose_graph_2d.cc:677-696)
    /// Note: C++ does not lock here, but we use a single lock for thread safety in C#.
    /// </summary>
    public override (double Score, IPoseGraph.Constraint? Constraint) ManualComputeConstraint(
        NodeId nodeId, SubmapId submapId)
    {
        // Match C++ (pose_graph_2d.cc:677-696)
        // FIX: Merged two separate locks into one to prevent data changes between locks

        TrajectoryNode.Data? constantData;
        Mapping.D2D.Submap2D? submap;

        lock (_dataLock)
        {
            // 1. Validate and get node data
            if (!_trajectoryNodes.Contains(nodeId))
                return (0, null);

            var node = _trajectoryNodes[nodeId];
            constantData = node.ConstantData;
            if (constantData == null)
                return (0, null);

            // 2. Validate and get submap data
            if (!_submapData.Contains(submapId))
                return (0, null);

            var submapData = _submapData[submapId];
            submap = submapData.Submap as Mapping.D2D.Submap2D;
            if (submap == null)
                return (0, null);
        }

        // 3. Delegate to constraint builder (no lock needed for computation)
        if (_constraintBuilder == null)
            return (0, null);

        return _constraintBuilder.ManualComputeGlobalConstraint(
            submapId, submap, nodeId, constantData, minScore: 0);
    }

    /// <summary>
    /// Manually compute constraint score from an initial pose estimate.
    /// Match C++: manualComputeConstraintScore (pose_graph_2d.cc:698-713)
    /// Note: C++ does not lock here, but we use a single lock for thread safety in C#.
    /// </summary>
    public override double ManualComputeConstraintScore(
        NodeId nodeId, SubmapId submapId, Rigid3d initialPose)
    {
        // Match C++ (pose_graph_2d.cc:698-713)
        // FIX: Merged two separate locks into one to prevent data changes between locks

        TrajectoryNode.Data? constantData;
        Mapping.D2D.Submap2D? submap;

        lock (_dataLock)
        {
            // Get node constant data
            if (!_trajectoryNodes.Contains(nodeId))
                return 0;
            var node = _trajectoryNodes[nodeId];
            constantData = node.ConstantData;
            if (constantData == null)
                return 0;

            // Get submap
            if (!_submapData.Contains(submapId))
                return 0;
            var submapData = _submapData[submapId];
            submap = submapData.Submap as Mapping.D2D.Submap2D;
            if (submap == null)
                return 0;
        }

        // Delegate to constraint builder
        if (_constraintBuilder == null)
            return 0;

        return _constraintBuilder.ManualComputeConstraintScore(
            submapId, submap, nodeId, constantData, minScore: 0, initialPose);
    }

    /// <summary>
    /// Manually compute scan matcher score with refined pose output.
    /// Match C++: manualComputeScanMatcher (pose_graph_2d.cc:715-729)
    /// </summary>
    public override double ManualComputeScanMatcher(
        NodeId nodeId, SubmapId submapId, Rigid3d initialPose,
        out Rigid3d poseManualEstimate)
    {
        // Match C++ (pose_graph_2d.cc:715-729)

        poseManualEstimate = Rigid3d.Identity;

        // Get node and submap (same pattern as above)
        TrajectoryNode.Data? constantData;
        Mapping.D2D.Submap2D? submap;

        lock (_dataLock)
        {
            if (!_trajectoryNodes.Contains(nodeId))
                return 0;
            var node = _trajectoryNodes[nodeId];
            constantData = node.ConstantData;

            if (!_submapData.Contains(submapId))
                return 0;
            var submapData = _submapData[submapId];
            submap = submapData.Submap as Mapping.D2D.Submap2D;
        }

        if (constantData == null || submap == null)
            return 0;

        if (_constraintBuilder == null)
            return 0;

        return _constraintBuilder.ManualComputeScanMatcher(
            submapId, submap, nodeId, constantData, minScore: 0,
            initialPose, out poseManualEstimate);
    }

    /// <summary>
    /// Manually relocalize a trajectory against finished submaps.
    /// Match C++: ManualRelocalization (pose_graph_2d.cc:565-675)
    /// </summary>
    public override bool ManualRelocalization(
        int trajectoryId, out double score, Rigid2d initialPose,
        LocalizationResultCallback? callback)
    {
        // Match C++ (pose_graph_2d.cc:565-675)
        // FIX: Merged 4 separate locks into 1 to prevent data changes between locks

        score = 0.0;

        NodeId lastNodeId;
        TrajectoryNode.Data? constantData;
        var finishedSubmapIds = new List<SubmapId>();
        var submaps = new List<Mapping.D2D.Submap2D>();
        var relativePoses = new List<Rigid2d>();

        // Single lock for all data reading operations
        lock (_dataLock)
        {
            // 1. Store callback
            if (callback != null)
            {
                _localizationResultCallback = callback;
            }

            // 2. Get last node of trajectory (match C++ line 580: std::prev(EndOfTrajectory))
            var nodesInTrajectory = _trajectoryNodes
                .Where(kvp => kvp.Id.TrajectoryId == trajectoryId)
                .OrderBy(kvp => kvp.Data.ConstantData?.Time ?? 0)
                .ToList();

            if (nodesInTrajectory.Count == 0)
                return false;

            var lastNode = nodesInTrajectory[^1];
            lastNodeId = lastNode.Id;
            constantData = lastNode.Data.ConstantData;

            if (constantData == null)
                return false;

            // 3. Get finished submaps (trajectory 0 only, match C++ line 585)
            foreach (var kvp in _submapData)
            {
                var submapId = kvp.Id;
                var submapData = kvp.Data;

                // Match C++: trajectory_id == 0 && state == kFinished
                // Check if submap is finished by checking InsertionFinished property
                if (submapId.TrajectoryId == 0 &&
                    submapData.Submap is Mapping.D2D.Submap2D submap2D &&
                    submap2D.InsertionFinished)
                {
                    finishedSubmapIds.Add(submapId);
                }
            }

            // 4. Compute relative poses for each submap
            foreach (var submapId in finishedSubmapIds)
            {
                if (!_submapData.Contains(submapId))
                    continue;

                var submapData = _submapData[submapId];
                if (submapData.Submap is not Mapping.D2D.Submap2D submap)
                    continue;

                // Get global pose from optimization problem (already 2D)
                if (!_globalSubmapPoses2D.Contains(submapId))
                    continue;

                var globalSubmapPose2D = _globalSubmapPoses2D[submapId].GlobalPose;

                // Compute relative pose: submap.inverse() * initial_pose
                var relativePose = globalSubmapPose2D.Inverse() * initialPose;

                // Filter by max constraint distance (match C++ line 600-606)
                if (_options.ConstraintBuilderOptions.HasValue &&
                    relativePose.Translation.Length() < _options.ConstraintBuilderOptions.Value.MaxConstraintDistance)
                {
                    submaps.Add(submap);
                    relativePoses.Add(relativePose);
                }
            }
        }

        // 5. Check if we have submaps to relocalize against
        if (submaps.Count == 0)
            return false;

        // 6. Call constraint builder for relocalization
        if (_constraintBuilder == null)
            return false;

        var (Score, Constraint) = _constraintBuilder.ManualComputeRelocalizationConstraint(
            finishedSubmapIds, submaps, relativePoses,
            lastNodeId, constantData, minScore: 0);

        score = Score;

        // 7. If relocalization failed, return false
        if (Score == 0 || Constraint == null)
            return false;

        // 8. Add constraint and run optimization
        lock (_dataLock)
        {
            // Verify node and submap still exist (match C++ line 656-663)
            if (!_trajectoryNodes.Contains(Constraint.Value.NodeId))
                return false;
            if (!_submapData.Contains(Constraint.Value.SubmapId))
                return false;

            _constraints.Add(Constraint.Value);

            // Update trajectory connectivity (match C++ line 667-669)
            UpdateTrajectoryConnectivityUnsafe(Constraint.Value);
        }

        // 9. Run optimization synchronously (matches C++ ManualRelocalization behavior)
        RunOptimizationSync();

        // 10. Invoke callback with result
        if (_localizationResultCallback != null)
        {
            // Get optimized pose from trajectory node poses
            Rigid3d globalPose = Rigid3d.Identity;
            lock (_dataLock)
            {
                if (_trajectoryNodePoses.Contains(lastNodeId))
                {
                    globalPose = _trajectoryNodePoses[lastNodeId].GlobalPose;
                }
            }

            var localizationResult = new LocalizationResult
            {
                NodeId = lastNodeId,
                SubmapId = Constraint.Value.SubmapId,
                GlobalPose = globalPose,
                Score = Score
            };
            _localizationResultCallback(localizationResult);
        }

        return true;
    }

    /// <summary>
    /// Drains the work queue. Match C++ DrainWorkQueue() implementation (line 849-873).
    /// Processes work items until queue is empty or optimization is needed.
    /// When queue is empty, schedules WhenDone callback to HandleWorkQueue (OnConstraintBuilderResult).
    /// </summary>
    public override void DrainWorkQueue()
    {
        // Match C++ DrainWorkQueue (line 849-866): Process work items until queue is empty or optimization is needed
        // Use WorkQueue's DrainWorkQueue method which handles the processing logic
        _workQueue.DrainWorkQueue();

        // Match C++ (line 867): Log remaining work items
        // Note: WorkQueue exposes Count property, but we skip logging in production

        // Match C++ (line 868-872): Schedule WhenDone callback when queue is empty
        // In C++, this schedules HandleWorkQueue to be called when constraint builder finishes
        // In C#, OnConstraintBuilderResult already serves as the HandleWorkQueue equivalent
        // and is called automatically when constraints are ready via MaybeAddConstraint callback
        // So we don't need to schedule it again here
    }

    /// <summary>
    /// Finishes a trajectory (matches xloc_cpp FinishTrajectory implementation).
    /// Marks trajectory and all its submaps as finished, and triggers optimization.
    /// </summary>
    public override void FinishTrajectory(int trajectoryId)
    {
        // Match C++: Add work item to work queue (not direct lock)
        // This ensures thread-safe serialization with other operations
        AddWorkItem($"FinishTrajectory {trajectoryId}", () =>
        {
            lock (_dataLock)
            {
                // Check if trajectory is already finished (match C++ CHECK)
                if (_trajectoryStates.TryGetValue(trajectoryId, out IPoseGraph.TrajectoryState value) && value == IPoseGraph.TrajectoryState.Finished)
                {
                    throw new InvalidOperationException(
                        $"Trajectory {trajectoryId} is already finished");
                }

                // Mark trajectory as FINISHED (match C++ line 968)
                _trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Finished;

                // Mark all submaps of this trajectory as finished (match C++ lines 970-972)
                // In C++, this sets data_.submap_data.at(submap.id).state = SubmapState::kFinished
                // In RobotApp, we mark submaps as insertion finished since we don't have separate state tracking
                foreach (var submapEntry in _submapData.BeginOfTrajectory(trajectoryId))
                {
                    submapEntry.Data.Submap?.InsertionFinished = true;
                    _finishedSubmapIds.Add(submapEntry.Id);
                }

                // Trim old sensor data to bound memory growth for finished trajectories
                TrimSensorDataForFinishedTrajectory(trajectoryId);
            }

            // Match C++: Return kRunOptimization to trigger optimization after finishing
            // This ensures optimization runs after trajectory is marked as finished
            return WorkItemResult.RunOptimization;
        });
    }

    public override void FreezeTrajectory(int trajectoryId)
    {
        lock (_dataLock)
        {
            _trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Frozen;
        }
    }

    public override void AddSubmapFromProto(Rigid3d globalPose, Models.Mapping.Submap submap)
    {
        Submap? runtimeSubmap = null;
        if (submap.Submap2D.HasValue)
        {
            // Convert from proto to runtime Submap2D
            var conversionTables = new ValueConversionTables();
            runtimeSubmap = new Mapping.D2D.Submap2D(submap.Submap2D.Value, conversionTables);
        }

        var submapId = submap.SubmapId;
        var runtimeSubmapId = new SubmapId(submapId.TrajectoryId, submapId.SubmapIndex);
        _submapData.Insert(runtimeSubmapId, new IPoseGraph.SubmapData(runtimeSubmap, globalPose));

        // Populate finished submap cache for submaps loaded from proto
        if (runtimeSubmap is Mapping.D2D.Submap2D submap2D && submap2D.InsertionFinished)
        {
            _finishedSubmapIds.Add(runtimeSubmapId);
        }
    }

    public override void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node)
    {
        var nodeData = TrajectoryNodeOperations.FromProto(node.NodeData);
        var trajectoryNode = new TrajectoryNode
        {
            ConstantData = nodeData,
            GlobalPose = globalPose
        };

        var nodeId = node.NodeId;
        var runtimeNodeId = new NodeId(nodeId.TrajectoryId, nodeId.NodeIndex);

        _trajectoryNodes.Insert(runtimeNodeId, trajectoryNode);
        _trajectoryNodePoses.Insert(runtimeNodeId, new TrajectoryNodePose
        {
            GlobalPose = globalPose,
            ConstantPoseData = new ConstantPoseData
            {
                Time = nodeData.Time,
                LocalPose = nodeData.LocalPose
            }
        });
    }

    public override void SetTrajectoryDataFromProto(Models.Mapping.TrajectoryData data)
    {
        // Convert from proto to runtime TrajectoryData
        var trajectoryData = new IPoseGraph.TrajectoryData(
            data.GravityConstant,
            data.ImuCalibration.HasValue ? (Quaternion?)data.ImuCalibration.Value : null,
            data.FixedFrameOriginInMap.HasValue ? (Rigid3d?)data.FixedFrameOriginInMap.Value : null
        );
        _trajectoryData[data.TrajectoryId] = trajectoryData;
    }

    /// <summary>
    /// Adds a node to a submap (matches C++ AddNodeToSubmap).
    /// C++: Only inserts node_id into submap_data.node_ids, does NOT create constraints here.
    /// Constraints are created in ComputeConstraintsForNode.
    /// </summary>
    public override void AddNodeToSubmap(NodeId nodeId, SubmapId submapId)
    {
        // Match C++: AddWorkItem to work queue (line 1120)
        var capturedNodeId = nodeId;
        var capturedSubmapId = submapId;

        AddWorkItem($"AddNodeToSubmap TrajectoryId={nodeId.TrajectoryId} SubmapIndex={submapId.SubmapIndex} NodeIndex={nodeId.NodeIndex}", () =>
        {
            lock (_dataLock)
            {
                // Match C++: if (CanAddWorkItemModifying(submap_id.trajectory_id))
                // Check if trajectory is finished or deleted
                if (!_trajectoryStates.TryGetValue(capturedSubmapId.TrajectoryId, out var state) ||
                    state == IPoseGraph.TrajectoryState.Finished ||
                    state == IPoseGraph.TrajectoryState.Deleted)
                {
                    return WorkItemResult.DoNotRunOptimization;
                }

                // Match C++: data_.submap_data.at(submap_id).node_ids.insert(node_id)
                // Track node-to-submap insertion for constraint building
                if (!_submapNodeInsertions.TryGetValue(capturedSubmapId, out var nodes))
                {
                    nodes = [];
                    _submapNodeInsertions[capturedSubmapId] = nodes;
                }
                nodes.Add(capturedNodeId);
            }
            return WorkItemResult.DoNotRunOptimization;
        });
    }

    public override void AddSerializedConstraints(List<IPoseGraph.Constraint> constraints)
    {
        _constraints.AddRange(constraints);
    }

    private readonly List<PoseGraphTrimmer> _trimmers = [];

    public override void AddTrimmer(PoseGraphTrimmer trimmer)
    {
        _trimmers.Add(trimmer);
    }

    private void RunTrimmers()
    {
        if (_trimmers.Count == 0)
        {
            return;
        }

        foreach (var trimmer in _trimmers)
        {
            trimmer.Trim(new TrimmablePoseGraph2D(this));
        }

        lock (_dataLock)
        {
            var totalSubmapsAfter = _submapData.Count;
            var submapsByTrajectoryAfter = new Dictionary<int, int>();
            foreach (var kvp in _submapData)
            {
                var trajId = kvp.Id.TrajectoryId;
                submapsByTrajectoryAfter.TryGetValue(trajId, out var count);
                submapsByTrajectoryAfter[trajId] = count + 1;
            }
        }
    }

    private class TrimmablePoseGraph2D(PoseGraph2D poseGraph) : ITrimmable
    {
        public int NumSubmaps(int trajectoryId)
        {
            lock (poseGraph._dataLock)
            {
                int count = 0;
                foreach (var kvp in poseGraph._submapData)
                {
                    if (kvp.Id.TrajectoryId == trajectoryId)
                    {
                        count++;
                    }
                }
                return count;
            }
        }

        public List<SubmapId> GetSubmapIds(int trajectoryId)
        {
            lock (poseGraph._dataLock)
            {
                var result = new List<SubmapId>();
                foreach (var kvp in poseGraph._submapData)
                {
                    if (kvp.Id.TrajectoryId == trajectoryId)
                    {
                        result.Add(kvp.Id);
                    }
                }
                return result;
            }
        }

        public MapById<SubmapId, IPoseGraph.SubmapData> GetOptimizedSubmapData()
        {
            lock (poseGraph._dataLock)
            {
                // Return a copy to avoid external modification
                var result = new MapById<SubmapId, IPoseGraph.SubmapData>();
                foreach (var kvp in poseGraph._submapData)
                {
                    result.Insert(kvp.Id, kvp.Data);
                }
                return result;
            }
        }

        public MapById<NodeId, TrajectoryNode> GetTrajectoryNodes()
        {
            lock (poseGraph._dataLock)
            {
                // Return a copy to avoid external modification
                var result = new MapById<NodeId, TrajectoryNode>();
                foreach (var kvp in poseGraph._trajectoryNodes)
                {
                    result.Insert(kvp.Id, kvp.Data);
                }
                return result;
            }
        }

        public List<IPoseGraph.Constraint> GetConstraints()
        {
            lock (poseGraph._dataLock)
            {
                return [.. poseGraph._constraints];
            }
        }

        public void TrimSubmap(SubmapId submapId)
        {
            lock (poseGraph._dataLock)
            {
                // Match C++ TrimSubmap (pose_graph_2d.cc line 1638-1729)
                if (!poseGraph._submapData.Contains(submapId))
                    return;

                // Nodes that are still INTRA_SUBMAP constrained to other submaps once this submap is gone
                var nodesToRetain = new HashSet<NodeId>();
                foreach (var kvp in poseGraph._submapData)
                {
                    if (!kvp.Id.Equals(submapId) && poseGraph._submapNodeInsertions.TryGetValue(kvp.Id, out var nodeIds))
                    {
                        foreach (var n in nodeIds)
                            nodesToRetain.Add(n);
                    }
                }

                // Nodes exclusively associated to submapId
                var nodesToRemove = new HashSet<NodeId>();
                if (poseGraph._submapNodeInsertions.TryGetValue(submapId, out var thisSubmapNodeIds))
                {
                    foreach (var n in thisSubmapNodeIds)
                    {
                        if (!nodesToRetain.Contains(n))
                            nodesToRemove.Add(n);
                    }
                }

                // Remove all constraints related to submap_id
                poseGraph._constraints.RemoveAll(c => c.SubmapId.Equals(submapId));

                // Remove all constraints related to nodes_to_remove; track other submaps losing constraints
                var otherSubmapIdsLosingConstraints = new HashSet<SubmapId>();
                var constraintsAfterRemoval = new List<IPoseGraph.Constraint>();
                foreach (var c in poseGraph._constraints)
                {
                    if (!nodesToRemove.Contains(c.NodeId))
                        constraintsAfterRemoval.Add(c);
                    else
                        otherSubmapIdsLosingConstraints.Add(c.SubmapId);
                }
                poseGraph._constraints.Clear();
                poseGraph._constraints.AddRange(constraintsAfterRemoval);

                // Keep only submaps that have no inter-submap constraints left
                foreach (var c in poseGraph._constraints)
                {
                    if (c.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap &&
                        otherSubmapIdsLosingConstraints.Contains(c.SubmapId))
                        otherSubmapIdsLosingConstraints.Remove(c.SubmapId);
                }
                foreach (var sid in otherSubmapIdsLosingConstraints)
                    poseGraph.DeleteConstraintBuilderScanMatcher(sid);

                // Mark submap as trimmed and remove its data
                poseGraph._submapData.Trim(submapId);
                poseGraph.DeleteConstraintBuilderScanMatcher(submapId);
                poseGraph._optimizationProblem.TrimSubmap(submapId);
                if (poseGraph._globalSubmapPoses2D.Contains(submapId))
                    poseGraph._globalSubmapPoses2D.Trim(submapId);
                poseGraph._submapNodeInsertions.Remove(submapId);
                poseGraph._finishedSubmapIds.Remove(submapId);

                // Remove nodes_to_remove from pose graph and optimization problem
                foreach (var nodeId in nodesToRemove)
                {
                    poseGraph._trajectoryNodes.Trim(nodeId);
                    poseGraph._trajectoryNodePoses.Trim(nodeId);
                    poseGraph._optimizationProblem.TrimTrajectoryNode(nodeId);
                    foreach (var set in poseGraph._submapNodeInsertions.Values)
                        set.Remove(nodeId);
                }
            }
        }

        public bool IsFinished(int trajectoryId)
        {
            return poseGraph.IsTrajectoryFinished(trajectoryId);
        }

        public void SetTrajectoryState(int trajectoryId, IPoseGraph.TrajectoryState state)
        {
            lock (poseGraph._dataLock)
            {
                poseGraph._trajectoryStates[trajectoryId] = state;
            }
        }
    }

    public override List<List<int>> GetConnectedTrajectories()
    {
        // Analyze constraints to find connected components of trajectories
        var visited = new HashSet<int>();
        var result = new List<List<int>>();

        // Get all trajectory IDs
        var allTrajectoryIds = new HashSet<int>();
        foreach (var kvp in _trajectoryNodes)
        {
            allTrajectoryIds.Add(kvp.Id.TrajectoryId);
        }
        foreach (var kvp in _submapData)
        {
            allTrajectoryIds.Add(kvp.Id.TrajectoryId);
        }

        // Build connectivity graph from constraints
        var graph = new Dictionary<int, HashSet<int>>();
        foreach (var constraint in _constraints)
        {
            var nodeTrajectoryId = constraint.NodeId.TrajectoryId;
            var submapTrajectoryId = constraint.SubmapId.TrajectoryId;

            if (!graph.TryGetValue(nodeTrajectoryId, out var nodeNeighbors))
            {
                nodeNeighbors = [];
                graph[nodeTrajectoryId] = nodeNeighbors;
            }
            nodeNeighbors.Add(submapTrajectoryId);

            if (!graph.TryGetValue(submapTrajectoryId, out var submapNeighbors))
            {
                submapNeighbors = [];
                graph[submapTrajectoryId] = submapNeighbors;
            }
            submapNeighbors.Add(nodeTrajectoryId);
        }

        // Find connected components using DFS
        foreach (var trajectoryId in allTrajectoryIds)
        {
            if (visited.Contains(trajectoryId))
            {
                continue;
            }

            var component = new List<int>();
            var stack = new Stack<int>();
            stack.Push(trajectoryId);
            visited.Add(trajectoryId);

            while (stack.Count > 0)
            {
                var current = stack.Pop();
                component.Add(current);

                if (graph.TryGetValue(current, out var neighbors))
                {
                    foreach (var neighbor in neighbors)
                    {
                        if (!visited.Add(neighbor))
                        {
                            stack.Push(neighbor);
                        }
                    }
                }
            }

            if (component.Count > 0)
            {
                result.Add(component);
            }
        }

        return result;
    }

    public override Dictionary<int, List<ImuData>> GetImuData()
    {
        lock (_dataLock)
        {
            // Return deep copy to avoid external modification
            var result = new Dictionary<int, List<ImuData>>();
            foreach (var kvp in _imuData)
            {
                result[kvp.Key] = [.. kvp.Value];
            }
            return result;
        }
    }

    public override Dictionary<int, List<OdometryData>> GetOdometryData()
    {
        lock (_dataLock)
        {
            // Return deep copy to avoid external modification
            var result = new Dictionary<int, List<OdometryData>>();
            foreach (var kvp in _odometryData)
            {
                result[kvp.Key] = [.. kvp.Value];
            }
            return result;
        }
    }

    public override Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData()
    {
        lock (_dataLock)
        {
            // Return deep copy to avoid external modification
            var result = new Dictionary<int, List<FixedFramePoseData>>();
            foreach (var kvp in _fixedFramePoseData)
            {
                result[kvp.Key] = [.. kvp.Value];
            }
            return result;
        }
    }

    public override Dictionary<string, IPoseGraph.LandmarkNode> GetLandmarkNodes()
    {
        return new Dictionary<string, IPoseGraph.LandmarkNode>(_landmarkNodes);
    }

    /// <summary>
    /// Match C++ SetInitialTrajectoryPose: store (to_trajectory_id, pose, time) and set local-to-global for from_trajectory.
    /// Used when adding first submap in InitializeGlobalSubmapPoses to Connect(from, to, time).
    /// </summary>
    public override void SetInitialTrajectoryPose(int fromTrajectoryId, int toTrajectoryId, Rigid3d pose, long time)
    {
        lock (_dataLock)
        {
            _initialTrajectoryPoses[fromTrajectoryId] = (toTrajectoryId, pose, time);

            if (!_localToGlobalTransforms.ContainsKey(fromTrajectoryId))
            {
                var targetTransform = _localToGlobalTransforms.TryGetValue(toTrajectoryId, out var transform)
                    ? transform
                    : GetLocalToGlobalTransformUnsafe(toTrajectoryId);
                var sourceTransform = targetTransform * pose;
                _localToGlobalTransforms[fromTrajectoryId] = sourceTransform;
            }
        }
    }

    /// <summary>
    /// Adds a new node with constant_data. Its constant_data.LocalPose was
    /// determined by scan matching against insertion_submaps.front() and the
    /// node data was inserted into the insertion_submaps. If
    /// insertion_submaps.front().InsertionFinished is true, data was inserted into
    /// this submap for the last time.
    /// 
    /// Matches C++ implementation: AddNode calls AppendNode SYNCHRONOUSLY,
    /// then queues ComputeConstraintsForNode to work queue. This ensures fast return
    /// while constraint building happens asynchronously.
    /// </summary>
    public NodeId AddNode(
        TrajectoryNode.Data constantData,
        int trajectoryId,
        List<Mapping.D2D.Submap2D> insertionSubmaps)
    {
        if (insertionSubmaps == null || insertionSubmaps.Count == 0)
        {
            throw new ArgumentException("insertion_submaps cannot be null or empty");
        }

        // Compute optimized pose (same as C++ GetLocalToGlobalTransform)
        var localToGlobalTransform = GetLocalToGlobalTransform(trajectoryId);
        var localPose = constantData.LocalPose;
        var optimizedPose = localToGlobalTransform * localPose;

        // Validate poses
        if (double.IsNaN(localPose.Translation.X) || double.IsNaN(localPose.Translation.Y) ||
            double.IsInfinity(localPose.Translation.X) || double.IsInfinity(localPose.Translation.Y))
        {
            throw new InvalidOperationException($"Local pose is NaN or Infinity: {localPose.Translation.X}, {localPose.Translation.Y}");
        }
        if (double.IsNaN(optimizedPose.Translation.X) || double.IsNaN(optimizedPose.Translation.Y) ||
            double.IsInfinity(optimizedPose.Translation.X) || double.IsInfinity(optimizedPose.Translation.Y))
        {
            throw new InvalidOperationException($"Optimized pose is NaN or Infinity: {optimizedPose.Translation.X}, {optimizedPose.Translation.Y}");
        }

        // Call AppendNodeUnsafe SYNCHRONOUSLY with lock (matches C++ AppendNode behavior)
        // This is fast and blocking, but necessary to return nodeId immediately
        NodeId nodeId;
        bool newlyFinishedSubmap;
        lock (_dataLock)
        {
            nodeId = AppendNodeUnsafe(constantData, trajectoryId, insertionSubmaps, optimizedPose);
            // Check if submap is newly finished (must check here before queuing)
            newlyFinishedSubmap = insertionSubmaps[0].InsertionFinished;
        }

        // Queue constraint computation to work queue (matches C++ ComputeConstraintsForNode)
        // This is done asynchronously so AddNode can return quickly
        var capturedNodeId = nodeId;
        var capturedConstantData = constantData;
        var capturedTrajectoryId = trajectoryId;
        var capturedInsertionSubmaps = insertionSubmaps;
        var capturedNewlyFinishedSubmap = newlyFinishedSubmap;

        AddWorkItem($"AddNode trajectoryId={trajectoryId}", () =>
        {
            // Match C++ ComputeConstraintsForNode execution order:
            // 1. lock(mutex_) { build constraints, collect tasks } unlock
            // 2. ComputeConstraint for each finished submap (no lock)
            // 3. ComputeConstraint for old nodes vs newly finished submap (no lock)
            // 4. NotifyEndOfNode()
            // 5. lock(mutex_) { ++counter, check optimization } unlock
            List<(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)> finishedSubmapConstraintTasks;
            List<(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)> newlyFinishedSubmapConstraintTasks;

            // Step 1: lock - build constraints, collect tasks
            lock (_dataLock)
            {
                ComputeConstraintsForNodeUnsafe(
                    capturedNodeId,
                    capturedConstantData,
                    capturedTrajectoryId,
                    capturedInsertionSubmaps,
                    capturedNewlyFinishedSubmap,
                    out finishedSubmapConstraintTasks,
                    out newlyFinishedSubmapConstraintTasks);
            }

            // Step 2-3: ComputeConstraint dispatches without lock (match C++ line 445-474)
            if (_constraintBuilder != null)
            {
                void ProcessTask(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)
                {
                    // Match C++: None means ComputeConstraint decided not to create any constraint
                    if (kind == ConstraintTaskKind.None)
                        return;
                    if (kind == ConstraintTaskKind.Global)
                        _constraintBuilder.MaybeAddGlobalConstraint(submapId, taskNodeId, submap, node, callback: OnConstraintBuilderResult);
                    else if (kind == ConstraintTaskKind.Localization && localizationPoses != null)
                        _constraintBuilder.MaybeAddLocalizationConstraint(submapId, taskNodeId, submap, node, localizationPoses, callback: OnConstraintBuilderResult);
                    else
                        _constraintBuilder.MaybeAddConstraint(submapId, taskNodeId, submap, node, initialRelativePose, callback: OnConstraintBuilderResult);
                }
                foreach (var (submapId, taskNodeId, submap, node, initialRelativePose, localizationPoses, kind) in finishedSubmapConstraintTasks)
                    ProcessTask(submapId, taskNodeId, submap, node, initialRelativePose, localizationPoses, kind);
                foreach (var (submapId, taskNodeId, submap, node, initialRelativePose, localizationPoses, kind) in newlyFinishedSubmapConstraintTasks)
                    ProcessTask(submapId, taskNodeId, submap, node, initialRelativePose, localizationPoses, kind);

                // Step 4: NotifyEndOfNode (match C++ line 476)
                _constraintBuilder.NotifyEndOfNode();
            }

            // Step 5: lock - increment counter and check optimization (match C++ line 477-488)
            lock (_dataLock)
            {
                return CheckIfOptimizationNeededUnsafe(capturedTrajectoryId)
                    ? WorkItemResult.RunOptimization
                    : WorkItemResult.DoNotRunOptimization;
            }
        });

        return nodeId;
    }

    /// <summary>
    /// Appends a node to the trajectory (matches C++ AppendNode).
    /// Only appends node and submap to data structures - fast and synchronous.
    /// Constraint building is done separately in ComputeConstraintsForNodeUnsafe.
    /// Assumes lock (_dataLock) held by caller.
    /// </summary>
    private NodeId AppendNodeUnsafe(
        TrajectoryNode.Data constantData,
        int trajectoryId,
        List<Mapping.D2D.Submap2D> insertionSubmaps,
        Rigid3d optimizedPose)
    {
        // Ensure trajectory state exists
        AddTrajectoryIfNeededUnsafe(trajectoryId);

        if (!_trajectoryStates.TryGetValue(trajectoryId, out IPoseGraph.TrajectoryState value))
        {
            value = IPoseGraph.TrajectoryState.Active;
            _trajectoryStates[trajectoryId] = value;
        }

        // Check if trajectory is finished or deleted
        if (value == IPoseGraph.TrajectoryState.Finished || value == IPoseGraph.TrajectoryState.Deleted)
        {
            throw new InvalidOperationException(
                $"Cannot add node to finished or deleted trajectory {trajectoryId}");
        }

        // Append node to trajectory (matches C++ AppendNode)
        var nodeId = _trajectoryNodes.Append(trajectoryId, new TrajectoryNode
        {
            ConstantData = constantData,
            GlobalPose = optimizedPose
        });

        // Bump version so OccupancyGridManager knows new data is available.
        _nodeInsertionVersion++;

        // CRITICAL FIX: Do NOT insert into _trajectoryNodePoses here.
        // In C++ code, GetTrajectoryNodePoses() computes the result from trajectory_nodes
        // on demand, not from a separate member variable. Inserting here causes
        // CanAppend to be set to false for the trajectory in _trajectoryNodePoses,
        // which can cause issues when appending subsequent nodes.
        // The _trajectoryNodePoses member variable is only used when loading from proto
        // (AddNodeFromProto), not during normal operation.

        // Match C++: Test if the 'insertion_submap.back()' is one we never saw before.
        // C++ line 152-162: if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
        //                      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))->data.submap != insertion_submaps.back())
        // CRITICAL FIX: Check if trajectory is frozen before checking for existing submaps
        // Frozen trajectories cannot be appended to, so we must always create new submaps for new trajectories
        bool isNewSubmap = true;
        bool trajectoryIsFrozen = _trajectoryStates.TryGetValue(trajectoryId, out var trajectoryState) &&
                                  trajectoryState == IPoseGraph.TrajectoryState.Frozen;

        if (!trajectoryIsFrozen && _submapData.SizeOfTrajectoryOrZero(trajectoryId) > 0)
        {
            // Find the last submap in the trajectory (highest SubmapIndex)
            SubmapId? lastSubmapId = null;
            int maxIndex = -1;
            foreach (var kvp in _submapData.BeginOfTrajectory(trajectoryId))
            {
                if (kvp.Id.SubmapIndex > maxIndex)
                {
                    maxIndex = kvp.Id.SubmapIndex;
                    lastSubmapId = kvp.Id;
                }
            }

            if (lastSubmapId.HasValue && _submapData.Contains(lastSubmapId.Value))
            {
                var lastSubmap = _submapData[lastSubmapId.Value].Submap;
                if (lastSubmap == insertionSubmaps[^1])
                {
                    isNewSubmap = false;
                }
            }
        }

        // Match C++: We grow 'data_.submap_data' as needed. This code assumes that the first
        // time we see a new submap is as 'insertion_submaps.back()'.
        // CRITICAL FIX: For new trajectories (not frozen), always append new submaps
        // For frozen trajectories, submaps should have been loaded from map, so this should not happen
        if (isNewSubmap)
        {
            // Match C++: const SubmapId submap_id = data_.submap_data.Append(trajectory_id, InternalSubmapData());
            // Match C++: data_.submap_data.at(submap_id).submap = insertion_submaps.back();
            _ = _submapData.Append(trajectoryId, new IPoseGraph.SubmapData(insertionSubmaps[^1], Rigid3d.Identity));

            // CRITICAL FIX: Update localToGlobalTransform immediately after creating the first submap.
            // For localization trajectories we set initial pose via SetInitialTrajectoryPose; _globalSubmapPoses2D
            // is only updated later in InitializeGlobalSubmapPosesUnsafe (called from async work item). So
            // ComputeLocalToGlobalTransform would return Identity here and would overwrite the MCL pose.
            // Preserve existing _localToGlobalTransforms when this trajectory has an initial pose (localization).
            if (_submapData.SizeOfTrajectoryOrZero(trajectoryId) == 1)
            {
                if (!_initialTrajectoryPoses.ContainsKey(trajectoryId))
                {
                    _localToGlobalTransforms[trajectoryId] = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
                }
                // else: keep _localToGlobalTransforms[trajectoryId] from SetInitialTrajectoryPose (MCL pose)
            }
        }
        else
        {
            // Not a new submap - node is being added to existing submap
            //
            // WARNING: ADDITIONAL C# LOGIC NOT IN ORIGINAL C++ CODE
            // =====================================================
            // This zero-pose submap correction is C# specific and does NOT exist in the original
            // Cartographer C++ implementation (pose_graph_2d.cc). It was added to handle an edge case
            // where the first submap is initialized with zero global pose but the first node has a
            // non-zero pose (e.g., when using MCL/localization with initial pose).
            //
            // Purpose: Synchronize the submap's global pose with the node's pose to ensure
            // ComputeLocalToGlobalTransform returns correct values for subsequent nodes.
            //
            // Trigger conditions:
            // 1. Only one submap exists in the trajectory
            // 2. Submap has zero global pose (X, Y, rotation all < 1e-6)
            // 3. Node has non-zero global pose (any of X, Y, rotation > 1e-6)
            //
            // Potential impacts:
            // - May cause different behavior from C++ in edge cases
            // - Recommended to verify MCL initialization scenarios work correctly
            // - Consider removing if upstream C++ code is updated to handle this case
            //
            // TODO: Verify with domain experts if this is the correct approach
            if (_submapData.SizeOfTrajectoryOrZero(trajectoryId) == 1)
            {
                // Only one submap exists - check if it has zero pose and node has non-zero pose
                SubmapId? firstSubmapId = null;
                foreach (var kvp in _submapData.BeginOfTrajectory(trajectoryId))
                {
                    if (firstSubmapId == null || kvp.Id.SubmapIndex < firstSubmapId.Value.SubmapIndex)
                    {
                        firstSubmapId = kvp.Id;
                    }
                }

                if (firstSubmapId.HasValue && _submapData.Contains(firstSubmapId.Value))
                {
                    var submapData = _submapData[firstSubmapId.Value];
                    var currentGlobalPose = submapData.Pose;
                    var currentGlobalPose2D = TransformOperations.Project2D(currentGlobalPose);
                    var optimizedPose2D = TransformOperations.Project2D(optimizedPose);

                    // Check if submap has zero pose and node has non-zero pose
                    bool submapHasZeroPose = Math.Abs(currentGlobalPose2D.Translation.X) < 1e-6 &&
                                             Math.Abs(currentGlobalPose2D.Translation.Y) < 1e-6 &&
                                             Math.Abs(currentGlobalPose2D.Rotation) < 1e-6;
                    bool nodeHasNonZeroPose = Math.Abs(optimizedPose2D.Translation.X) > 1e-6 ||
                                              Math.Abs(optimizedPose2D.Translation.Y) > 1e-6 ||
                                              Math.Abs(optimizedPose2D.Rotation) > 1e-6;

                    if (submapHasZeroPose && nodeHasNonZeroPose)
                    {
                        // Update submap global pose from node pose
                        var updatedGlobalPose = optimizedPose;
                        _submapData[firstSubmapId.Value] = new IPoseGraph.SubmapData(submapData.Submap, updatedGlobalPose);

                        // Update localToGlobalTransform
                        var transform = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
                        _localToGlobalTransforms[trajectoryId] = transform;
                    }
                }
            }
        }

        return nodeId;
    }

    /// <summary>
    /// Adds trajectory if needed (assumes lock held). Match C++ AddTrajectoryIfNeeded: trajectory_connectivity_state.Add, global_localization_samplers_.
    /// </summary>
    private void AddTrajectoryIfNeededUnsafe(int trajectoryId)
    {
        if (!_trajectoryStates.ContainsKey(trajectoryId))
        {
            _trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Active;
        }
        _trajectoryConnectivityState.Add(trajectoryId);
        if (!_globalLocalizationSamplers.ContainsKey(trajectoryId))
        {
            var ratio = _options.GlobalSamplingRatio;
            if (ratio <= 0) ratio = 0.003;
            _globalLocalizationSamplers[trajectoryId] = new FixedRatioSampler(ratio);
        }
    }

    /// <summary>
    /// Get latest node time for (node_id, submap_id) (match C++ GetLatestNodeTime). Assumes lock held.
    /// </summary>
    private long GetLatestNodeTimeUnsafe(NodeId nodeId, SubmapId submapId)
    {
        if (!_trajectoryNodes.Contains(nodeId))
            return 0;
        if (!_submapData.Contains(submapId))
            return _trajectoryNodes[nodeId].ConstantData?.Time ?? 0;
        var time = _trajectoryNodes[nodeId].ConstantData?.Time ?? 0;
        if (_submapNodeInsertions.TryGetValue(submapId, out var nodeIds) && nodeIds.Count > 0)
        {
            // SortedSet.Max is O(log N), matching C++ std::set::rbegin() which is O(1).
            var lastNodeId = nodeIds.Max;
            if (_trajectoryNodes.Contains(lastNodeId))
            {
                var lastTime = _trajectoryNodes[lastNodeId].ConstantData?.Time ?? 0;
                if (lastTime > time) time = lastTime;
            }
        }
        return time;
    }

    /// <summary>
    /// Sets localization initial poses for relocalizing against the map.
    /// Match C++: SetLocalizationInitialPoses (pose_graph_2d.cc:1467-1475)
    /// Sets initial poses, isRelocalized=false, ToggleSearchingForRelocalization(true), num_relocalization_constraint_search_=0.
    /// </summary>
    public override void SetLocalizationInitialPoses(IReadOnlyList<Rigid3d> localizationInitialPoses)
    {
        lock (_dataLock)
        {
            _localizationInitialPoses = localizationInitialPoses?.ToList() ?? [];
            _isRelocalized = false;
            _constraintBuilder?.ToggleSearchingForRelocalization(true);
            _numRelocalizationConstraintSearch = 0;
        }
    }

    /// <summary>
    /// Match C++ SetLocalizationMode.
    /// </summary>
    public void SetLocalizationMode(bool value)
    {
        lock (_dataLock) { _localizationMode = value; }
    }

    /// <summary>
    /// Match C++ SetLocalizationCallback: set callback invoked when relocalization search reaches max nodes without success.
    /// </summary>
    public void SetLocalizationCallback(LocalizationSearchCallback? callback)
    {
        _localizationSearchCallback = callback;
    }

    /// <summary>
    /// Match C++ matching_score_enabled: enable/disable pose confidence scoring.
    /// </summary>
    public void SetMatchingScoreEnabled(bool value)
    {
        lock (_dataLock) { _matchingScoreEnabled = value; }
    }

    /// <summary>
    /// Gets the current node scores for pose confidence tracking.
    /// Match C++: node_scores_ accessor.
    /// </summary>
    public List<(long Time, double Score)> GetNodeScores()
    {
        lock (_dataLock) { return [.. _nodeScores]; }
    }

    /// <summary>
    /// Computes pose confidence for a node against finished submaps.
    /// Match C++: ComputeNodePoseConfident (pose_graph_2d.cc:491-563)
    /// </summary>
    private void ComputeNodePoseConfident(NodeId nodeId, List<SubmapId> finishedSubmapIds)
    {
        // Get node constant data
        if (!_trajectoryNodes.Contains(nodeId))
            return;

        var node = _trajectoryNodes[nodeId];
        var constantData = node.ConstantData;
        if (constantData == null)
            return;

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

        // Track best probability for each point
        var pointcloudScore = new double[pointCloud.Count];
        for (int i = 0; i < pointcloudScore.Length; i++)
            pointcloudScore[i] = 0.0;

        // Get optimization problem data
        var optimizationNodeData = _optimizationProblem.NodeData();
        var optimizationSubmapData = _optimizationProblem.SubmapData();

        if (!optimizationNodeData.Contains(nodeId))
            return;

        var nodeGlobalPose2D = optimizationNodeData[nodeId].GlobalPose2D;

        // Match C++ (line 506-544): iterate through finished submaps
        foreach (var submapId in finishedSubmapIds)
        {
            // Match C++ (line 508-510): skip same trajectory
            if (nodeId.TrajectoryId == submapId.TrajectoryId)
                continue;

            if (!optimizationSubmapData.Contains(submapId))
                continue;

            // Match C++ (line 512-516): compute initial_relative_pose
            var submapGlobalPose2D = optimizationSubmapData[submapId].GlobalPose;
            var initialRelativePose = submapGlobalPose2D.Inverse() * nodeGlobalPose2D;

            // Match C++ (line 518-521): skip if distance > max_constraint_distance
            if (_options.ConstraintBuilderOptions.HasValue &&
                initialRelativePose.Translation.Length() > _options.ConstraintBuilderOptions.Value.MaxConstraintDistance)
                continue;

            // Get submap and grid
            if (!_submapData.Contains(submapId))
                continue;

            var submapData = _submapData[submapId];
            if (submapData.Submap is not Mapping.D2D.Submap2D submap2D)
                continue;

            var grid = submap2D.Grid;
            if (grid is not Mapping.D2D.ProbabilityGrid probabilityGrid)
                continue;

            // Match C++ (line 526-527): compute node_pose_in_local
            var submapLocalPose2D = TransformOperations.Project2D(submap2D.LocalPose);
            var nodePoseInLocal = submapLocalPose2D * initialRelativePose;

            var limits = probabilityGrid.Limits;

            // Match C++ (line 531-544): compute probability for each point
            for (int i = 0; i < pointCloud.Count; i++)
            {
                var point = pointCloud[i];
                // Match C++: node_pose_in_local.cast<double>() * point_cloud[i].position.head<2>()
                var transformedP = nodePoseInLocal * new Vector2(point.Position.X, point.Position.Y);
                var cellIndex = limits.GetCellIndex(transformedP);
                var prob = probabilityGrid.GetProbability(cellIndex);

                if (prob > pointcloudScore[i])
                    pointcloudScore[i] = prob;
            }
        }

        // Match C++ (line 550-556): compute sum_prob (count of points with score >= 0.5)
        double sumProb = 0.0;
        foreach (var s in pointcloudScore)
        {
            if (s >= 0.5)
                sumProb += 1;
        }

        // Match C++ (line 556): best_score = sum_prob / pointcloud_score.size()
        double bestScore = sumProb / pointcloudScore.Length;

        // Match C++ (line 559-562): add to node_scores_ with max 30 entries
        _nodeScores.AddLast((constantData.Time, bestScore));
        if (_nodeScores.Count > MaxNodeScoresCount)
            _nodeScores.RemoveFirst();
    }

    /// <summary>
    /// Match C++ UpdateTrajectoryConnectivity: update last connection time for (node_id.trajectory_id, submap_id.trajectory_id).
    /// Assumes lock (_dataLock) held by caller.
    /// </summary>
    private void UpdateTrajectoryConnectivityUnsafe(IPoseGraph.Constraint constraint)
    {
        if (constraint.ConstraintTag != IPoseGraph.Constraint.Tag.InterSubmap) return;
        var time = GetLatestNodeTimeUnsafe(constraint.NodeId, constraint.SubmapId);
        _trajectoryConnectivityState.Connect(constraint.NodeId.TrajectoryId, constraint.SubmapId.TrajectoryId, time);
    }

    /// <summary>
    /// Match C++ ComputeConstraint (line 273-313): decide Local, Localization, or Global for (nodeId, submapId). Assumes lock held.
    /// Extended for single-trajectory loop closure: when EnableSingleTrajectoryLoopClosure is true and
    /// initialRelativePose distance exceeds threshold, use Global search instead of Local.
    /// </summary>
    private ConstraintTaskKind ComputeConstraintKindUnsafe(
        NodeId nodeId,
        SubmapId submapId,
        MapById<SubmapId, Optimization.SubmapSpec2D> optimizationSubmapData,
        Rigid2d? initialRelativePose = null)
    {
        var nodeTime = GetLatestNodeTimeUnsafe(nodeId, submapId);
        var lastConnectionTime = _trajectoryConnectivityState.LastConnectionTime(nodeId.TrajectoryId, submapId.TrajectoryId);
        // C++ global_constraint_search_after_n_seconds: add seconds to time (assume time is in microseconds)
        var threshold = lastConnectionTime + (long)(_options.GlobalConstraintSearchAfterNSeconds * 1e6);
        var searchingRelocalization = _constraintBuilder?.IsSearchingForRelocalization ?? false;

        // === SINGLE-TRAJECTORY LOOP CLOSURE EXTENSION ===
        // When the node and submap are on the same trajectory, but the initial relative pose
        // distance is large (indicating possible loop closure), use Global search instead of Local.
        // This allows detecting loop closures when robot returns to a previously visited area.
        if (nodeId.TrajectoryId == submapId.TrajectoryId &&
            _options.EnableSingleTrajectoryLoopClosure &&
            initialRelativePose.HasValue)
        {
            var distance = initialRelativePose.Value.Translation.Length();
            var loopClosureThreshold = _options.SingleTrajectoryLoopClosureDistanceThreshold;

            // If distance is large but within max constraint distance, this could be a loop closure
            // Use Global search (MatchFullSubmap) for better detection
            var maxConstraintDistance = _options.ConstraintBuilderOptions?.MaxConstraintDistance ?? 15.0;
            if (distance > loopClosureThreshold && distance <= maxConstraintDistance)
            {
                return ConstraintTaskKind.Global;
            }
        }
        // === END EXTENSION ===

        bool maybeLocal = nodeId.TrajectoryId == submapId.TrajectoryId
            || (!_localizationMode && nodeTime < threshold)
            || (_localizationMode && !searchingRelocalization);
        if (maybeLocal) return ConstraintTaskKind.Local;
        if (_localizationMode && searchingRelocalization) return ConstraintTaskKind.Localization;
        if (_globalLocalizationSamplers.TryGetValue(nodeId.TrajectoryId, out var sampler) && sampler.Pulse())
        {
            return ConstraintTaskKind.Global;
        }
        // Match C++: when Pulse() returns false, ComputeConstraint does nothing (no constraint created)
        return ConstraintTaskKind.None;
    }

    /// <summary>
    /// Constraint task kind (match C++ ComputeConstraint: local / localization / global).
    /// </summary>
    private enum ConstraintTaskKind { None, Local, Localization, Global }

    /// <summary>
    /// Computes constraints for a node (matches C++ ComputeConstraintsForNode).
    /// This is called from work queue after AppendNodeUnsafe completes.
    /// Assumes lock held.
    /// Outputs constraint tasks to be executed outside lock to avoid blocking AddNode.
    /// Optimization check is done separately in the caller after NotifyEndOfNode (match C++ line 477-488).
    /// </summary>
    private void ComputeConstraintsForNodeUnsafe(
        NodeId nodeId,
        TrajectoryNode.Data constantData,
        int trajectoryId,
        List<Mapping.D2D.Submap2D> insertionSubmaps,
        bool newlyFinishedSubmap,
        out List<(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)> finishedSubmapConstraintTasks,
        out List<(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)> newlyFinishedSubmapConstraintTasks)
    {
        // CRITICAL FIX: Cache SubmapData() and NodeData() at the start to avoid multiple calls
        var optimizationSubmapData = _optimizationProblem.SubmapData();
        var optimizationNodeData = _optimizationProblem.NodeData();

        // Get optimized pose for the node - read from _trajectoryNodes instead of _trajectoryNodePoses
        // because _trajectoryNodePoses is only populated when loading from proto, not during normal operation
        var optimizedPose = _trajectoryNodes[nodeId].GlobalPose;
        var localToGlobal = GetLocalToGlobalTransformUnsafe(trajectoryId);

        // Initialize global submap poses and get submap IDs (match C++: pass time for initial_trajectory_poses Connect)
        var submapIds = InitializeGlobalSubmapPosesUnsafe(trajectoryId, constantData.Time, insertionSubmaps);

        // Match C++: local_pose_2d = Project2D(local_pose * Rotation(gravity_alignment.inverse()))
        var gravityAlignmentInverse = Quaternion.Conjugate(constantData.GravityAlignment);
        var localPoseWithGravityRotation = constantData.LocalPose * new Rigid3d(Vector3.Zero, gravityAlignmentInverse);
        var localPose2D = TransformOperations.Project2D(localPoseWithGravityRotation);

        // Match C++: global_pose_2d = submap_data.at(matching_id).global_pose * ComputeSubmapPose(insertion_submaps.front()).inverse() * local_pose_2d
        var matchingId = submapIds[0];
        var matchingSubmapPose2D = optimizationSubmapData[matchingId].GlobalPose;
        var frontSubmapPose2D = TransformOperations.Project2D(insertionSubmaps[0].LocalPose);
        var globalPose2D = matchingSubmapPose2D * frontSubmapPose2D.Inverse() * localPose2D;

        // Add node to optimization problem
        _optimizationProblem.AddNode(
            trajectoryId,
            constantData.Time,
            localPose2D,
            globalPose2D,
            TransformOperations.Embed3D(globalPose2D) * new Rigid3d(Vector3.Zero, constantData.GravityAlignment),
            constantData.GravityAlignment);

        // FIXED: Match C++ order - Create INTRA_SUBMAP constraints FIRST (lines 406-423)
        // Then collect finished submaps (lines 429-434)
        var translationWeight = _options.MatcherTranslationWeight;
        var rotationWeight = _options.MatcherRotationWeight;

        // Match C++: Create constraints for all insertion_submaps using submap_ids
        for (int i = 0; i < insertionSubmaps.Count; i++)
        {
            var submapId = submapIds[i];
            var insertionSubmap = insertionSubmaps[i];

            // Match C++: Track node_ids for this submap (line 412: data_.submap_data.at(submap_id).node_ids.emplace(node_id))
            if (!_submapNodeInsertions.TryGetValue(submapId, out var insertionNodes))
            {
                insertionNodes = [];
                _submapNodeInsertions[submapId] = insertionNodes;
            }
            insertionNodes.Add(nodeId);

            // Match C++: constraint_transform = ComputeSubmapPose(*insertion_submaps[i]).inverse() * local_pose_2d
            // Note: local_pose_2d is already gravity-aligned (computed above)
            var computeSubmapPose = TransformOperations.Project2D(insertionSubmap.LocalPose);
            var constraintTransform2D = computeSubmapPose.Inverse() * localPose2D;
            var constraintTransform = TransformOperations.Embed3D(constraintTransform2D);

            // FIXED: Removed identity skip check - match C++ which creates ALL constraints
            // FIXED: Removed duplicate check - match C++ which uses push_back without checking

            // Match C++: Constraint{submap_id, node_id, {Embed3D(constraint_transform), matcher_translation_weight, matcher_rotation_weight}, INTRA_SUBMAP}
            _constraints.Add(new IPoseGraph.Constraint(
                submapId,
                nodeId,
                new IPoseGraph.Constraint.Pose(
                    constraintTransform,
                    translationWeight,
                    rotationWeight
                ),
                IPoseGraph.Constraint.Tag.IntraSubmap
            ));
        }

        // Match C++: Find finished submaps to compute constraints against (line 429-434)
        // C++ ComputeConstraint (line 273-370) chooses per (node_id, submap_id): Local, Localization, or Global.
        finishedSubmapConstraintTasks = [];
        newlyFinishedSubmapConstraintTasks = [];

        // Collect finished submap constraint tasks using cached _finishedSubmapIds
        // instead of scanning all submaps. O(_finishedSubmapIds.Count) with O(1) lookups.
        if (_trajectoryNodes.Contains(nodeId))
        {
            var node = _trajectoryNodes[nodeId];
            var nodeGlobalPose2D = optimizationNodeData[nodeId].GlobalPose2D;

            foreach (var finishedSubmapId in _finishedSubmapIds)
            {
                // Skip submaps that contain this node (INTRA already handled)
                if (_submapNodeInsertions.TryGetValue(finishedSubmapId, out var insertedNodeIds) && insertedNodeIds.Contains(nodeId))
                    continue;
                if (!optimizationSubmapData.Contains(finishedSubmapId))
                    continue;
                if (!_submapData.Contains(finishedSubmapId))
                    continue;
                var submapDataEntry = _submapData[finishedSubmapId];
                if (submapDataEntry.Submap is not Mapping.D2D.Submap2D submap2D)
                    continue;

                var submapGlobalPose2D = optimizationSubmapData[finishedSubmapId].GlobalPose;
                var initialRelativePose = submapGlobalPose2D.Inverse() * nodeGlobalPose2D;
                var kind = ComputeConstraintKindUnsafe(nodeId, finishedSubmapId, optimizationSubmapData, initialRelativePose);
                if (kind == ConstraintTaskKind.None)
                    continue;
                List<Rigid2d>? localizationPoses = null;
                if (kind == ConstraintTaskKind.Localization)
                {
                    if (_localizationInitialPoses.Count == 0)
                        localizationPoses = [initialRelativePose];
                    else
                    {
                        localizationPoses = [];
                        foreach (var globalPose3d in _localizationInitialPoses)
                        {
                            var rel2d = submapGlobalPose2D.Inverse() * TransformOperations.Project2D(globalPose3d);
                            localizationPoses.Add(rel2d);
                        }
                    }
                }
                finishedSubmapConstraintTasks.Add((finishedSubmapId, nodeId, submap2D, node, initialRelativePose, localizationPoses, kind));
            }
        }

        // Match C++: Handle newly_finished_submap (line 435-442)
        HashSet<NodeId>? newlyFinishedSubmapNodeIds = null;
        if (newlyFinishedSubmap)
        {
            var newlyFinishedSubmapId = submapIds[0];
            // FIXED: Match C++ - set state to kFinished (use InsertionFinished = true)
            if (_submapData.Contains(newlyFinishedSubmapId))
            {
                var submapData = _submapData[newlyFinishedSubmapId];
                if (submapData.Submap is Mapping.D2D.Submap2D finishedSubmap2D)
                {
                    // Match C++: finished_submap_data.state = SubmapState::kFinished
                    finishedSubmap2D.InsertionFinished = true;
                    _finishedSubmapIds.Add(newlyFinishedSubmapId);
                }
            }
            // Track node IDs for this newly finished submap
            if (_submapNodeInsertions.TryGetValue(newlyFinishedSubmapId, out var nodeIds))
            {
                newlyFinishedSubmapNodeIds = [.. nodeIds];
            }
        }

        // Match C++: Compute constraints for finished submaps (line 445-447)
        // C++: for (const auto& submap_id : finished_submap_ids) { ComputeConstraint(node_id, submap_id); }
        // C# ComputeConstraintKindUnsafe + ProcessTask dispatch: Local -> MaybeAddConstraint, Localization -> MaybeAddLocalizationConstraint (with _localizationInitialPoses), Global -> MaybeAddGlobalConstraint.
        // Constraint tasks are executed outside lock to avoid blocking AddNode.

        // Match C++: Handle newly_finished_submap - compute constraints for old nodes (line 465-475)
        if (newlyFinishedSubmap && _constraintBuilder != null)
        {
            var newlyFinishedSubmapId = submapIds[0];
            if (_submapData.Contains(newlyFinishedSubmapId) &&
                _submapData[newlyFinishedSubmapId].Submap is Mapping.D2D.Submap2D newlyFinishedSubmap2D &&
                optimizationSubmapData.Contains(newlyFinishedSubmapId))
            {
                // Use EnumerateUnsafe() - safe here because we hold _dataLock and
                // optimizationNodeData is not modified during this iteration.
                // Avoids O(N) snapshot allocation that GetEnumerator().ToList() would create.
                var newlyFinishedSubmapPose2D = optimizationSubmapData[newlyFinishedSubmapId].GlobalPose;

                foreach (var nodeKvp in optimizationNodeData.EnumerateUnsafe())
                {
                    var oldNodeId = nodeKvp.Id;
                    // Match C++: if (newly_finished_submap_node_ids.count(node_id) == 0)
                    if (newlyFinishedSubmapNodeIds != null && newlyFinishedSubmapNodeIds.Contains(oldNodeId))
                        continue;
                    if (!_trajectoryNodes.Contains(oldNodeId))
                        continue;

                    var oldNode = _trajectoryNodes[oldNodeId];
                    var oldNodeGlobalPose2D = nodeKvp.Data.GlobalPose2D;
                    var initialRelativePose = newlyFinishedSubmapPose2D.Inverse() * oldNodeGlobalPose2D;
                    var kind = ComputeConstraintKindUnsafe(oldNodeId, newlyFinishedSubmapId, optimizationSubmapData, initialRelativePose);
                    if (kind == ConstraintTaskKind.None)
                        continue;
                    List<Rigid2d>? localizationPoses = null;
                    if (kind == ConstraintTaskKind.Localization)
                    {
                        if (_localizationInitialPoses.Count == 0)
                            localizationPoses = [initialRelativePose];
                        else
                        {
                            localizationPoses = [];
                            foreach (var globalPose3d in _localizationInitialPoses)
                            {
                                var rel2d = newlyFinishedSubmapPose2D.Inverse() * TransformOperations.Project2D(globalPose3d);
                                localizationPoses.Add(rel2d);
                            }
                        }
                    }
                    newlyFinishedSubmapConstraintTasks.Add((newlyFinishedSubmapId, oldNodeId, newlyFinishedSubmap2D, oldNode, initialRelativePose, localizationPoses, kind));
                }
            }
        }

        // Match C++ (line 449-458): relocalization counter and callback
        if (_localizationMode && (_constraintBuilder?.IsSearchingForRelocalization ?? false))
        {
            _numRelocalizationConstraintSearch++;
            var maxNodes = _options.MaxNumberRelocalizationNodes > 0 ? _options.MaxNumberRelocalizationNodes : 12;
            if (_numRelocalizationConstraintSearch >= maxNodes)
            {
                _localizationSearchCallback?.Invoke(-1, 0, Rigid3d.Identity);
                _constraintBuilder?.ToggleSearchingForRelocalization(false);
            }
        }

        // Match C++ (line 460-461): Compute pose confidence if enabled and relocated
        if (_isRelocalized && _matchingScoreEnabled)
        {
            // Collect finished submap IDs for ComputeNodePoseConfident
            var finishedSubmapIds = finishedSubmapConstraintTasks.Select(t => t.submapId).ToList();
            ComputeNodePoseConfident(nodeId, finishedSubmapIds);
        }

        // Match C++ (line 463): current_trajectory_id = node_id.trajectory_id
        _currentTrajectoryId = trajectoryId;

        // Match C++: constraint_builder_.NotifyEndOfNode() and optimization check
        // are done in the caller after constraint dispatch (match C++ line 476-488)
    }

    /// <summary>
    /// Initializes global submap poses (matches C++ InitializeGlobalSubmapPoses).
    /// Assumes lock held.
    /// </summary>
    private List<SubmapId> InitializeGlobalSubmapPosesUnsafe(int trajectoryId, long time, List<Mapping.D2D.Submap2D> insertionSubmaps)
    {
        if (insertionSubmaps.Count == 0)
        {
            throw new ArgumentException("insertion_submaps cannot be empty");
        }

        var optimizationSubmapData = _optimizationProblem.SubmapData();

        // Handle case with 1 insertion_submap.
        // C++ assumes this only happens at initialization (SubmapId index 0).
        // C# ForceNewSubmapAndInsert can produce 1-submap results mid-operation,
        // so we find the matching SubmapId by object reference instead of hardcoding index 0.
        if (insertionSubmaps.Count == 1)
        {
            // Find the SubmapId in _submapData that matches insertionSubmaps[0] by object reference
            SubmapId? matchingSubmapId = null;
            foreach (var kvp in _submapData.BeginOfTrajectory(trajectoryId))
            {
                if (kvp.Data.Submap == insertionSubmaps[0])
                {
                    matchingSubmapId = kvp.Id;
                }
            }

            if (!matchingSubmapId.HasValue)
            {
                throw new InvalidOperationException(
                    $"Cannot find matching submap in _submapData for trajectory {trajectoryId}, " +
                    $"insertionSubmap={insertionSubmaps[0].GetHashCode()}, " +
                    $"_submapData.size={_submapData.SizeOfTrajectoryOrZero(trajectoryId)}, " +
                    $"optimization.size={optimizationSubmapData.SizeOfTrajectoryOrZero(trajectoryId)}");
            }

            // Ensure the submap is in the optimization problem
            if (!optimizationSubmapData.Contains(matchingSubmapId.Value))
            {
                if (_initialTrajectoryPoses.TryGetValue(trajectoryId, out var initialPose))
                {
                    _trajectoryConnectivityState.Connect(trajectoryId, initialPose.ToTrajectoryId, time);
                }

                var localToGlobal = GetLocalToGlobalTransformUnsafe(trajectoryId);
                var submapPose2D = TransformOperations.Project2D(localToGlobal * insertionSubmaps[0].LocalPose);
                _optimizationProblem.AddSubmap(matchingSubmapId.Value, submapPose2D);

                if (!_globalSubmapPoses2D.Contains(matchingSubmapId.Value))
                    _globalSubmapPoses2D.Insert(matchingSubmapId.Value, new Optimization.SubmapSpec2D(submapPose2D));
            }

            return [matchingSubmapId.Value];
        }

        // Match C++: Handle case with 2 insertion_submaps
        if (insertionSubmaps.Count != 2)
        {
            throw new ArgumentException($"Expected 1 or 2 insertion_submaps, got {insertionSubmaps.Count}");
        }

        // Find last submap ID (highest SubmapIndex) from optimization problem
        SubmapId? lastSubmapId = null;
        int maxIndex = -1;
        foreach (var kvp in optimizationSubmapData)
        {
            if (kvp.Id.TrajectoryId == trajectoryId && kvp.Id.SubmapIndex > maxIndex)
            {
                maxIndex = kvp.Id.SubmapIndex;
                lastSubmapId = kvp.Id;
            }
        }

        if (lastSubmapId == null)
        {
            throw new InvalidOperationException($"No submaps in optimization problem for trajectory {trajectoryId}");
        }

        if (!lastSubmapId.HasValue || !_submapData.Contains(lastSubmapId.Value))
        {
            throw new InvalidOperationException($"Last submap not found for trajectory {trajectoryId}");
        }

        // Match C++: Check if last_submap_id.submap == insertion_submaps.front()
        if (_submapData[lastSubmapId.Value].Submap == insertionSubmaps[0])
        {
            // Match C++: insertion_submaps.back() is new
            // C++: optimization_problem_->AddSubmap(trajectory_id, first_submap_pose * ComputeSubmapPose(insertion_submaps[0]).inverse() * ComputeSubmapPose(insertion_submaps[1]))
            var firstSubmapPose = optimizationSubmapData[lastSubmapId.Value].GlobalPose;
            var computeSubmapPose0 = TransformOperations.Project2D(insertionSubmaps[0].LocalPose);
            var computeSubmapPose1 = TransformOperations.Project2D(insertionSubmaps[1].LocalPose);
            var newSubmapPose2D = firstSubmapPose * computeSubmapPose0.Inverse() * computeSubmapPose1;
            _optimizationProblem.AddSubmap(trajectoryId, newSubmapPose2D);

            // CRITICAL FIX: Also update _globalSubmapPoses2D to match C++ behavior
            // This ensures GetLocalToGlobalTransform can read from _globalSubmapPoses2D immediately
            var newSubmapId = new SubmapId(trajectoryId, lastSubmapId.Value.SubmapIndex + 1);
            if (!_globalSubmapPoses2D.Contains(newSubmapId))
                _globalSubmapPoses2D.Insert(newSubmapId, new Optimization.SubmapSpec2D(newSubmapPose2D));

            // Match C++: return {last_submap_id, SubmapId{trajectory_id, last_submap_id.submap_index + 1}}
            return [lastSubmapId.Value, newSubmapId];
        }

        // Match C++: Check if last_submap_id.submap == insertion_submaps.back()
        if (_submapData[lastSubmapId.Value].Submap == insertionSubmaps[1])
        {
            // Match C++: front_submap_id = {trajectory_id, last_submap_id.submap_index - 1}
            var frontSubmapId = new SubmapId(trajectoryId, lastSubmapId.Value.SubmapIndex - 1);

            // Match C++: CHECK(data_.submap_data.at(front_submap_id).submap == insertion_submaps.front())
            if (!_submapData.Contains(frontSubmapId) || _submapData[frontSubmapId].Submap != insertionSubmaps[0])
            {
                throw new InvalidOperationException(
                    $"Front submap mismatch for trajectory {trajectoryId}: " +
                    $"frontSubmapId={frontSubmapId}, lastSubmapId={lastSubmapId.Value}, " +
                    $"frontExists={_submapData.Contains(frontSubmapId)}, " +
                    $"_submapData.size={_submapData.SizeOfTrajectoryOrZero(trajectoryId)}, " +
                    $"optimization.size={optimizationSubmapData.SizeOfTrajectoryOrZero(trajectoryId)}");
            }

            return [frontSubmapId, lastSubmapId.Value];
        }

        // C# FIX: Handle case where last optimization submap doesn't match either insertion submap.
        // This can happen when ForceNewSubmapAndInsert (C#-only) disrupts the normal submap lifecycle,
        // or when trimming causes the optimization problem to be out of sync with _submapData.
        // Find the actual submap IDs by scanning _submapData for matching submap objects.
        SubmapId? foundFrontId = null;
        SubmapId? foundBackId = null;
        foreach (var kvp in _submapData.BeginOfTrajectory(trajectoryId))
        {
            if (kvp.Data.Submap == insertionSubmaps[0])
                foundFrontId = kvp.Id;
            if (kvp.Data.Submap == insertionSubmaps[1])
                foundBackId = kvp.Id;
        }

        if (!foundFrontId.HasValue || !foundBackId.HasValue)
        {
            throw new InvalidOperationException(
                $"Submap mismatch for trajectory {trajectoryId}: " +
                $"lastSubmapId={lastSubmapId.Value}, " +
                $"lastSubmap={_submapData[lastSubmapId.Value].Submap?.GetHashCode()}, " +
                $"insertionSubmaps[0]={insertionSubmaps[0].GetHashCode()}, " +
                $"insertionSubmaps[1]={insertionSubmaps[1].GetHashCode()}, " +
                $"foundFront={foundFrontId.HasValue}, foundBack={foundBackId.HasValue}, " +
                $"_submapData.size={_submapData.SizeOfTrajectoryOrZero(trajectoryId)}, " +
                $"optimization.size={optimizationSubmapData.SizeOfTrajectoryOrZero(trajectoryId)}");
        }

        // Add missing submaps to the optimization problem
        Debug.WriteLine($"[PoseGraph2D] Submap chain recovery for trajectory {trajectoryId}: " +
            $"lastOptimization={lastSubmapId.Value.SubmapIndex}, " +
            $"front={foundFrontId.Value.SubmapIndex}, back={foundBackId.Value.SubmapIndex}");

        // Ensure front submap is in the optimization problem
        if (!optimizationSubmapData.Contains(foundFrontId.Value))
        {
            var localToGlobal = GetLocalToGlobalTransformUnsafe(trajectoryId);
            var frontPose2D = TransformOperations.Project2D(localToGlobal * insertionSubmaps[0].LocalPose);
            _optimizationProblem.AddSubmap(foundFrontId.Value, frontPose2D);
            if (!_globalSubmapPoses2D.Contains(foundFrontId.Value))
                _globalSubmapPoses2D.Insert(foundFrontId.Value, new Optimization.SubmapSpec2D(frontPose2D));
        }

        // Ensure back submap is in the optimization problem
        if (!optimizationSubmapData.Contains(foundBackId.Value))
        {
            var frontPose = optimizationSubmapData.Contains(foundFrontId.Value)
                ? optimizationSubmapData[foundFrontId.Value].GlobalPose
                : TransformOperations.Project2D(GetLocalToGlobalTransformUnsafe(trajectoryId) * insertionSubmaps[0].LocalPose);
            var computeSubmapPose0 = TransformOperations.Project2D(insertionSubmaps[0].LocalPose);
            var computeSubmapPose1 = TransformOperations.Project2D(insertionSubmaps[1].LocalPose);
            var backPose2D = frontPose * computeSubmapPose0.Inverse() * computeSubmapPose1;
            _optimizationProblem.AddSubmap(foundBackId.Value, backPose2D);
            if (!_globalSubmapPoses2D.Contains(foundBackId.Value))
                _globalSubmapPoses2D.Insert(foundBackId.Value, new Optimization.SubmapSpec2D(backPose2D));
        }

        return [foundFrontId.Value, foundBackId.Value];
    }

    /// <summary>
    /// Checks if optimization is needed (assumes lock held).
    /// </summary>
    /// <summary>
    /// Match C++ (line 481-489): run optimization if optimize_every_n_nodes reached, or in localization mode before relocalized and under max relocalization nodes.
    /// FIXED: Use GLOBAL counter like C++, not per-trajectory.
    /// FIXED: Check > instead of >= to match C++ logic.
    /// Assumes lock (_dataLock) held by caller.
    /// </summary>
    private bool CheckIfOptimizationNeededUnsafe(int trajectoryId)
    {
        // Match C++: ++num_nodes_since_last_loop_closure_ (line 478)
        _numNodesSinceLastLoopClosure++;

        // Match C++: Check > not >= (line 482)
        if (_options.OptimizeEveryNNodes > 0 && _numNodesSinceLastLoopClosure > _options.OptimizeEveryNNodes)
        {
            Debug.WriteLine($"[PoseGraph2D] Optimization triggered: nodes_since_last={_numNodesSinceLastLoopClosure} > OptimizeEveryNNodes={_options.OptimizeEveryNNodes}");
            return true;
        }
        var maxReloc = _options.MaxNumberRelocalizationNodes > 0 ? _options.MaxNumberRelocalizationNodes : 12;
        if (_localizationMode && !_isRelocalized && _numRelocalizationConstraintSearch < maxReloc)
        {
            Debug.WriteLine($"[PoseGraph2D] Optimization triggered (localization mode): is_relocalized={_isRelocalized}, reloc_search={_numRelocalizationConstraintSearch}/{maxReloc}");
            return true;
        }
        return false;
    }

    /// <summary>
    /// Unsafe version of GetLocalToGlobalTransform (assumes lock held).
    /// Match C++: Reads from data_.global_submap_poses_2d (not optimization_problem directly).
    /// </summary>
    private Rigid3d GetLocalToGlobalTransformUnsafe(int trajectoryId)
    {
        if (_localToGlobalTransforms.TryGetValue(trajectoryId, out var transform))
        {
            return transform;
        }
        // CRITICAL FIX: Match C++ behavior - read from _globalSubmapPoses2D instead of optimization_problem
        // This avoids potential blocking/slowness when optimization_problem is being modified.
        // Match C++ line 1501: ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, trajectory_id)
        var computedTransform = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
        // Cache the computed transform for next time
        // Note: ComputeLocalToGlobalTransform already logs when returning Identity
        _localToGlobalTransforms[trajectoryId] = computedTransform;
        return computedTransform;
    }

    /// <summary>
    /// Match C++ lines 1477-1496: GetInterpolatedGlobalTrajectoryPose(trajectory_id, time)
    /// Returns the interpolated global pose for a trajectory at a given time.
    /// Used by ComputeLocalToGlobalTransform when no submaps exist but initial_trajectory_poses has an entry.
    /// Assumes lock (_dataLock) held by caller.
    /// </summary>
    private Rigid3d GetInterpolatedGlobalTrajectoryPose(int trajectoryId, long time)
    {
        // Match C++ line 1479: CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0)
        var trajectorySize = _trajectoryNodes.SizeOfTrajectoryOrZero(trajectoryId);
        if (trajectorySize == 0)
        {
            // No nodes in trajectory, return identity
            return Rigid3d.Identity;
        }

        // Match C++ line 1480: const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time)
        // LowerBound returns null when time > all node times (EndOfTrajectory case)
        var it = _trajectoryNodes.LowerBound(trajectoryId, time, node => node.Time);

        // Get first and last nodes for boundary checks
        var lastNode = _trajectoryNodes.GetLastOfTrajectory(trajectoryId);
        if (lastNode == null)
        {
            return Rigid3d.Identity;
        }

        // Match C++ line 1485-1488: if (it == EndOfTrajectory) return last node's global_pose
        // In C#, LowerBound returns null when time > all node times
        if (it == null)
        {
            return lastNode.Value.Data.GlobalPose;
        }

        // Match C++ line 1481-1484: if (it == BeginOfTrajectory) return first node's global_pose
        var beginItEnumerable = _trajectoryNodes.BeginOfTrajectory(trajectoryId);
        var hasBeginIt = beginItEnumerable.Any();
        if (!hasBeginIt)
        {
            return Rigid3d.Identity;
        }
        var beginIt = beginItEnumerable.First();

        if (it.Value.Id.Equals(beginIt.Id))
        {
            // Return first node's global pose
            return beginIt.Data.GlobalPose;
        }

        // Get current node from iterator (safe now - it is not null)
        var itNode = it.Value;

        // Match C++ lines 1489-1496: Interpolate between prev(it) and it
        // Find the previous node
        MapById<NodeId, TrajectoryNode>.IdDataReference? prevNode = null;
        foreach (var kvp in _trajectoryNodes.BeginOfTrajectory(trajectoryId))
        {
            if (kvp.Id.Equals(itNode.Id))
            {
                break;
            }
            prevNode = kvp;
        }

        if (prevNode == null)
        {
            // No previous node, return current node's pose
            return itNode.Data.GlobalPose;
        }

        // Match C++ lines 1489-1495: Interpolate between prev(it) and it
        return TransformOperations.Interpolate(
            prevNode.Value.Data.GlobalPose,
            prevNode.Value.Data.Time,
            itNode.Data.GlobalPose,
            itNode.Data.Time,
            time);
    }

    /// <summary>
    /// Computes the local to global map frame transform based on the given global submap poses.
    /// Match C++: ComputeLocalToGlobalTransform(global_submap_poses, trajectory_id)
    /// C++ uses the LAST optimized submap (std::prev(end_it)->id), not the first
    /// CRITICAL FIX: Use GetLastOfTrajectory() instead of BeginOfTrajectory().OrderBy().ToList()
    /// to avoid O(n log n) sorting overhead when there are many submaps.
    /// </summary>
    private Rigid3d ComputeLocalToGlobalTransform(MapById<SubmapId, Optimization.SubmapSpec2D> globalSubmapPoses, int trajectoryId)
    {
        // Match C++: Find the LAST submap for this trajectory (std::prev(end_it)->id)
        // CRITICAL FIX: Use GetLastOfTrajectory() instead of OrderBy().ToList() for O(1) access
        var lastSubmapRef = globalSubmapPoses.GetLastOfTrajectory(trajectoryId);
        if (lastSubmapRef == null)
        {
            // Match C++ lines 1547-1554: Check initial_trajectory_poses if no submaps
            if (_initialTrajectoryPoses.TryGetValue(trajectoryId, out var initialPose))
            {
                // Match C++ lines 1549-1551: return GetInterpolatedGlobalTrajectoryPose(...) * relative_pose
                return GetInterpolatedGlobalTrajectoryPose(initialPose.ToTrajectoryId, initialPose.Time)
                       * initialPose.Pose;
            }
            // WARNING: No submaps and no initial pose - returning Identity means local pose = global pose
            // This is expected during initialization but may indicate a problem if it persists.
            Debug.WriteLine($"[PoseGraph2D] ComputeLocalToGlobalTransform returning Identity: " +
                $"trajectoryId={trajectoryId}, no submaps in globalSubmapPoses and no initialTrajectoryPose");
            return Rigid3d.Identity;
        }

        // Match C++: Use LAST optimized submap (std::prev(end_it)->id)
        var lastOptimizedSubmapId = lastSubmapRef.Value.Id;
        var optimizedSubmapSpec = lastSubmapRef.Value.Data;

        // Get the submap from our data
        if (!_submapData.Contains(lastOptimizedSubmapId))
        {
            Debug.WriteLine($"[PoseGraph2D] ComputeLocalToGlobalTransform returning Identity: " +
                $"trajectoryId={trajectoryId}, lastOptimizedSubmapId={lastOptimizedSubmapId} not found in _submapData");
            return Rigid3d.Identity;
        }

        if (_submapData[lastOptimizedSubmapId].Submap is not Mapping.D2D.Submap2D submap)
        {
            Debug.WriteLine($"[PoseGraph2D] ComputeLocalToGlobalTransform returning Identity: " +
                $"trajectoryId={trajectoryId}, submap at {lastOptimizedSubmapId} is not Submap2D");
            return Rigid3d.Identity;
        }

        // Match C++: Compute transform: Embed3D(global_pose) * local_pose.inverse()
        var globalSubmapPose = TransformOperations.Embed3D(optimizedSubmapSpec.GlobalPose);
        var localSubmapPose = submap.LocalPose;
        var transform = globalSubmapPose * localSubmapPose.Inverse();
        return transform;
    }

    /// <summary>
    /// Computes local to global transform from old submap poses (for extrapolation).
    /// Match C++: ComputeLocalToGlobalTransform(global_submap_poses_2d, trajectory_id)
    /// </summary>
    private Rigid3d ComputeLocalToGlobalTransformFromSubmapPoses(
        MapById<SubmapId, Rigid2d> oldSubmapPoses,
        int trajectoryId)
    {
        // Find the LAST submap for this trajectory (match C++: std::prev(end_it)->id)
        SubmapId? lastSubmapId = null;
        foreach (var kvp in oldSubmapPoses)
        {
            if (kvp.Id.TrajectoryId == trajectoryId)
            {
                if (lastSubmapId == null || kvp.Id.SubmapIndex > lastSubmapId.Value.SubmapIndex)
                {
                    lastSubmapId = kvp.Id;
                }
            }
        }

        if (lastSubmapId == null)
        {
            return Rigid3d.Identity;
        }

        // Get the submap from _submapData to access LocalPose
        if (!_submapData.Contains(lastSubmapId.Value))
        {
            return Rigid3d.Identity;
        }

        var submapData = _submapData[lastSubmapId.Value];
        if (submapData.Submap is not Mapping.D2D.Submap2D submap)
        {
            return Rigid3d.Identity;
        }

        // Compute transform: global_submap_pose * local_submap_pose^-1
        var globalSubmapPose2D = oldSubmapPoses[lastSubmapId.Value];
        var globalSubmapPose = TransformOperations.Embed3D(globalSubmapPose2D);
        var localSubmapPose = submap.LocalPose;
        var transform = globalSubmapPose * localSubmapPose.Inverse();
        return transform;
    }

    /// <summary>
    /// Match C++ (line 1706, 1714): Delete scan matcher for submap when trimming.
    /// Called by TrimmablePoseGraph2D.TrimSubmap.
    /// </summary>
    internal void DeleteConstraintBuilderScanMatcher(SubmapId submapId)
    {
        _constraintBuilder?.DeleteScanMatcher(submapId);
    }

    /// <summary>
    /// Callback invoked when ConstraintBuilder2D finishes computing constraints.
    /// Matches C++ HandleWorkQueue logic (line 772-847):
    /// 1. Add constraints to data structures (with dataLock)
    /// 2. Run optimization
    /// 3. Invoke global SLAM callback
    /// 4. Update trajectory connectivity
    /// 5. Delete trajectories if needed
    /// 6. Handle trimming
    /// 7. Reset loop closure counter
    /// 8. Drain work queue
    /// </summary>
    private void OnConstraintBuilderResult(ConstraintBuilder2DResult result)
    {
        if (result.Constraints == null || result.Constraints.Count == 0)
        {
            return;
        }

        // === LOOP CLOSURE CONSTRAINTS RECEIVED LOG ===
        var interSubmapConstraints = result.Constraints.Where(c => c.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap).ToList();
        
        // Match C++ HandleWorkQueue (line 774-778): Add constraints with dataLock (mutex_)
        // C++ uses data_.constraints.insert() without duplicate checking
        // FIXED: Removed O(n) duplicate check - match C++ which just inserts all constraints
        lock (_dataLock)
        {
            foreach (var constraint in result.Constraints)
            {
                _constraints.Add(constraint);
                UpdateTrajectoryConnectivityUnsafe(constraint);
            }
        }

        // Match C++ HandleWorkQueue (line 780): Run optimization SYNCHRONOUSLY
        // This matches C++ behavior where RunOptimization() is synchronous
        RunOptimizationSync();

        // Match C++ HandleWorkQueue (line 782-805): Invoke global SLAM optimization callback
        InvokeGlobalSlamOptimizationCallback();

        // Match C++ HandleWorkQueue (line 807-844): Process constraints and cleanup
        lock (_dataLock)
        {
            // Match C++ HandleWorkQueue (line 809-811): Update trajectory connectivity (done in Add constraints above via UpdateTrajectoryConnectivityUnsafe)

            // Match C++ HandleWorkQueue (line 812): Delete trajectories if needed
            // Note: DeleteTrajectoriesIfNeeded requires deletion_state tracking which is not implemented in C#
            // For now, we skip this as trajectories are deleted immediately in C#

            // Match C++ HandleWorkQueue (line 813-822): Handle trimming
            var trimmingHandle = new TrimmablePoseGraph2D(this);
            foreach (var trimmer in _trimmers)
            {
                trimmer.Trim(trimmingHandle);
            }

            // Match C++ HandleWorkQueue (line 817-822): Remove finished trimmers
            // Note: PoseGraphTrimmer in C# doesn't have IsFinished() method, so we skip this
            // In C++, trimmers can be marked as finished and removed, but C# implementation doesn't support this

            // Match C++ HandleWorkQueue (line 824): Reset loop closure counter
            // FIXED: Reset GLOBAL counter like C++
            var oldCounter = _numNodesSinceLastLoopClosure;
            _numNodesSinceLastLoopClosure = 0;

            // Match C++ HandleWorkQueue (line 826-843): Update metrics
            // Note: Metrics are not implemented in C# version, so we skip this
        }

        // Match C++ HandleWorkQueue (line 846): Drain work queue
        DrainWorkQueue();
    }

    /// <summary>
    /// Gets the total number of optimization requests made.
    /// This tracks how many times RunOptimizationSync() was called.
    /// </summary>
    public override int WorkItemsAdded => (int)Interlocked.Read(ref _optimizationRequested);

    /// <summary>
    /// Gets the total number of optimizations completed.
    /// This tracks how many times optimization actually finished executing.
    /// </summary>
    public override int WorkItemsCompleted => (int)Interlocked.Read(ref _optimizationCompleted);

    /// <summary>
    /// Gets the number of pending optimization requests.
    /// This is the difference between requested and completed optimizations.
    /// </summary>
    public override int WorkItemsPending => (int)(Interlocked.Read(ref _optimizationRequested) - Interlocked.Read(ref _optimizationCompleted));

    /// <summary>
    /// Gets the current number of items in the work queue.
    /// </summary>
    public override int WorkQueueCount => _workQueue.Count;

    /// <summary>
    /// Gets the number of nodes started in the constraint builder.
    /// </summary>
    public override int ConstraintBuilderNodesStarted => _constraintBuilder?.GetNumStartedNodes() ?? 0;

    /// <summary>
    /// Gets the number of nodes finished in the constraint builder.
    /// </summary>
    public override int ConstraintBuilderNodesFinished => _constraintBuilder?.GetNumFinishedNodes() ?? 0;

    /// <summary>
    /// Gets the total number of trajectory nodes in the pose graph.
    /// Used for progress tracking during optimization (matches _trajectoryNodes.Count in WaitForAllComputations).
    /// </summary>
    public override int TrajectoryNodesCount
    {
        get
        {
            lock (_dataLock)
            {
                return _trajectoryNodes.Count;
            }
        }
    }

    /// <summary>
    /// Gets the total number of constraint tasks dispatched for scan matching.
    /// </summary>
    public override int ConstraintTasksTotal => _constraintBuilder?.GetNumConstraintTasksDispatched() ?? 0;

    /// <summary>
    /// Gets the number of constraint tasks that have finished scan matching.
    /// </summary>
    public override int ConstraintTasksFinished => _constraintBuilder?.GetNumConstraintTasksFinished() ?? 0;



    /// <summary>
    /// Disposes resources, including work queue and waiting for optimization thread to complete.
    /// </summary>
    public void Dispose()
    {
        // Unsubscribe from work queue events to prevent memory leaks
        _workQueue?.OptimizationNeeded -= OnOptimizationNeeded;

        // Wait for optimization thread to complete (with timeout)
        if (_optimizationThread != null)
        {
            if (_optimizationThread.IsAlive)
            {
                if (!_optimizationThread.Join(TimeSpan.FromSeconds(30)))
                {
                    // Thread didn't finish in time, but continue cleanup
                    // Optimization thread is foreground thread, so it will complete eventually
                }
            }
        }

        // Clear large collections to free memory
        // CRITICAL: Try to acquire lock with timeout to avoid deadlock
        bool lockAcquired = false;
        try
        {
            if (Monitor.TryEnter(_dataLock, TimeSpan.FromSeconds(5)))
            {
                lockAcquired = true;
                _imuData.Clear();
                _odometryData.Clear();
                _fixedFramePoseData.Clear();
                _submapNodeInsertions.Clear();
                _finishedSubmapIds.Clear();
                _activeSubmapNodeInsertions.Clear();
            }
            else
            {
                // Continue anyway - collections will be GC'd when object is disposed
            }
        }
        finally
        {
            if (lockAcquired)
            {
                Monitor.Exit(_dataLock);
            }
        }

        _workQueue?.Dispose();

        // Dispose constraint builder (disposes CeresScanMatcher2D + SemaphoreSlim)
        _constraintBuilder?.Dispose();

        GC.SuppressFinalize(this);
    }
}