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

namespace CartographerSharp.Mapping.Internal.D3D;

/// <summary>
/// Internal submap data structure for PoseGraph3D.
/// </summary>
internal struct InternalSubmapData3D
{
    public Mapping.D3D.Submap3D? Submap { get; set; }
}

/// <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 in 3D:
/// 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 PoseGraph3D : PoseGraph, IDisposable
{
    private readonly PoseGraphOptions _options;
    private readonly ThreadPoolInterface? _threadPool;
    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 = [];
    private readonly Dictionary<int, InitialTrajectoryPose> _initialTrajectoryPoses = [];

    // 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 = [];
    
    // ConstraintBuilder3D for inter-submap constraint computation
    private readonly ConstraintBuilder3D? _constraintBuilder;

    // Periodic optimization tracking
    private readonly Dictionary<int, int> _numNodesSinceLastOptimization = [];
    private readonly Lock _optimizationLock = new();
    private Thread? _optimizationThread;
    private bool _optimizationInProgress = false;

    // Track node-to-submap insertions for constraint builder
    private readonly Dictionary<SubmapId, HashSet<NodeId>> _submapNodeInsertions = [];

    // Trimmer support
    private readonly List<PoseGraphTrimmer> _trimmers = [];

    private OptimizationProblem3D? _optimizationProblem;

    // CRITICAL FIX: Match C++ - Work queue for serializing operations (similar to PoseGraph2D)
    // Without work queue, constraint computation blocks AddNode caller and can cause lock contention.
    private readonly WorkQueue _workQueue;

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

    // Track work items for status reporting
    private int _workItemsAdded = 0;
    private int _workItemsCompleted = 0;

    /// <summary>
    /// Gets the total number of work items added to the work queue.
    /// </summary>
    public override int WorkItemsAdded => _workItemsAdded;

    /// <summary>
    /// Gets the total number of work items completed by the work queue.
    /// </summary>
    public override int WorkItemsCompleted => _workItemsCompleted;

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

    /// <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.
    /// </summary>
    public override int TrajectoryNodesCount
    {
        get
        {
            lock (_dataLock)
            {
                return _trajectoryNodes.Count;
            }
        }
    }

    public override int ConstraintTasksTotal => 0;
    public override int ConstraintTasksFinished => 0;

    // Constructor
    public PoseGraph3D(PoseGraphOptions options, OptimizationProblem3D? optimizationProblem = null, ThreadPoolInterface? threadPool = null) : base()
    {
        _options = options;
        _threadPool = threadPool;

        // CRITICAL FIX: Initialize work queue for async constraint computation (match PoseGraph2D)
        _workQueue = new WorkQueue();
        _workQueue.OptimizationNeeded += OnOptimizationNeeded;

        // Get OptimizationProblemOptions from PoseGraphOptions or use defaults
        var optimizationProblemOptions = _options.OptimizationProblemOptions ?? new OptimizationProblemOptions();
        _optimizationProblem = optimizationProblem ?? new OptimizationProblem3D(optimizationProblemOptions);

        // Setup ConstraintBuilder3D if options are provided and threadPool is available
        // Match C++ (pose_graph_3d.cc): Pass thread_pool to ConstraintBuilder3D
        if (_options.ConstraintBuilderOptions.HasValue && _threadPool != null)
        {
            var constraintBuilderOptions = _options.ConstraintBuilderOptions.Value;
            _constraintBuilder = new ConstraintBuilder3D(constraintBuilderOptions, _threadPool);
        }
    }

    /// <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)
    {
        Interlocked.Increment(ref _workItemsAdded);

        Func<WorkItemResult> wrappedWorkItem = () =>
        {
            try
            {
                return workItem();
            }
            finally
            {
                Interlocked.Increment(ref _workItemsCompleted);
            }
        };

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

    /// <summary>
    /// Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
    /// determined by scan matching against 'insertion_submaps.front()' and the
    /// node data was inserted into the 'insertion_submaps'.
    ///
    /// CRITICAL FIX: Match C++ implementation - AppendNode is SYNCHRONOUS,
    /// constraint computation is QUEUED to work queue for async processing.
    /// </summary>
    public NodeId AddNode(
        TrajectoryNode.Data constantData,
        int trajectoryId,
        List<Mapping.D3D.Submap3D> insertionSubmaps)
    {
        var optimizedPose = GetLocalToGlobalTransformInternal(trajectoryId) * constantData.LocalPose;

        // Call AppendNodeUnsafe SYNCHRONOUSLY with lock (matches C++ AppendNode behavior)
        // This is fast and blocking, but necessary to return nodeId immediately
        NodeId nodeId;
        bool newlyFinishedSubmap;
        List<SubmapId> submapIds;

        lock (_dataLock)
        {
            nodeId = AppendNodeUnsafe(constantData, trajectoryId, insertionSubmaps, optimizedPose, out submapIds);
            // Check if first submap is newly finished (must check here before queuing)
            newlyFinishedSubmap = insertionSubmaps.Count > 0 && 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 capturedSubmapIds = submapIds;
        var capturedOptimizedPose = optimizedPose;
        var capturedNewlyFinishedSubmap = newlyFinishedSubmap;

        AddWorkItem($"AddNode trajectoryId={trajectoryId} nodeIndex={nodeId.NodeIndex}", () =>
        {
            WorkItemResult result;

            lock (_dataLock)
            {
                result = ComputeConstraintsForNodeUnsafe(
                    capturedNodeId,
                    capturedConstantData,
                    capturedTrajectoryId,
                    capturedInsertionSubmaps,
                    capturedSubmapIds,
                    capturedOptimizedPose,
                    capturedNewlyFinishedSubmap);
            }

            return result;
        });

        return nodeId;
    }

    /// <summary>
    /// Appends a node to the trajectory (matches C++ AppendNode).
    /// Only appends node and submap to data structures with intra-submap constraints - 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.D3D.Submap3D> insertionSubmaps,
        Rigid3d optimizedPose,
        out List<SubmapId> submapIds)
    {
        AddTrajectoryIfNeeded(trajectoryId);

        var nodeId = _trajectoryNodes.Append(
            trajectoryId,
            new TrajectoryNode
            {
                ConstantData = constantData,
                GlobalPose = optimizedPose
            });

        // Add trajectory node pose
        _trajectoryNodePoses.Insert(nodeId, new TrajectoryNodePose
        {
            GlobalPose = optimizedPose,
            ConstantPoseData = new ConstantPoseData
            {
                Time = constantData.Time,
                LocalPose = constantData.LocalPose
            }
        });

        // Test if the 'insertion_submaps.back()' is one we never saw before
        var lastSubmap = insertionSubmaps[^1];
        var lastSubmapFound = false;
        if (_submapData.SizeOfTrajectoryOrZero(trajectoryId) > 0)
        {
            // Check if last submap in trajectory matches
            var trajectorySubmaps = _submapData.BeginOfTrajectory(trajectoryId).ToList();
            if (trajectorySubmaps.Count > 0)
            {
                var lastTrajectorySubmap = trajectorySubmaps[^1];
                if (lastTrajectorySubmap.Data.Submap == lastSubmap)
                {
                    lastSubmapFound = true;
                }
            }
        }

        if (!lastSubmapFound)
        {
            // We grow 'data_.submap_data' as needed
            _submapData.Append(trajectoryId, new IPoseGraph.SubmapData(lastSubmap, optimizedPose));
        }

        // Add intra-submap constraints for all insertion submaps
        // Use matcher weights from options for constraint weights
        var translationWeight = _options.MatcherTranslationWeight;
        var rotationWeight = _options.MatcherRotationWeight;

        // Find or create submap IDs for insertion submaps
        submapIds = [];
        foreach (var submap in insertionSubmaps)
        {
            SubmapId? foundId = null;
            foreach (var kvp in _submapData)
            {
                if (kvp.Id.TrajectoryId == trajectoryId && kvp.Data.Submap == submap)
                {
                    foundId = kvp.Id;
                    break;
                }
            }

            if (foundId.HasValue)
            {
                submapIds.Add(foundId.Value);
            }
        }

        // Add intra-submap constraints
        // C++ line 337-338: constraint_transform = insertion_submaps[i]->local_pose().inverse() * local_pose
        // Constraint transform must be computed from local poses, not global poses
        foreach (var submapId in submapIds)
        {
            // Find the corresponding insertion submap for this submapId
            Mapping.D3D.Submap3D? insertionSubmap = null;
            foreach (var submap in insertionSubmaps)
            {
                if (_submapData.Contains(submapId) && _submapData[submapId].Submap == submap)
                {
                    insertionSubmap = submap;
                    break;
                }
            }

            if (insertionSubmap == null)
                continue;

            // Compute constraint transform from local poses (C++ line 338)
            var localPose = constantData.LocalPose;
            var constraintTransform = insertionSubmap.LocalPose.Inverse() * localPose;

            // Track node-to-submap insertion for constraint builder
            if (!_submapNodeInsertions.TryGetValue(submapId, out var nodes))
            {
                nodes = [];
                _submapNodeInsertions[submapId] = nodes;
            }
            nodes.Add(nodeId);

            // Check if constraint already exists
            var constraintExists = _constraints.Any(c =>
                c.NodeId.Equals(nodeId) && c.SubmapId.Equals(submapId));

            if (!constraintExists)
            {
                _constraints.Add(new IPoseGraph.Constraint(
                    submapId,
                    nodeId,
                    new IPoseGraph.Constraint.Pose(
                        constraintTransform,
                        translationWeight,
                        rotationWeight
                    ),
                    IPoseGraph.Constraint.Tag.IntraSubmap
                ));
            }
        }

        return nodeId;
    }

    /// <summary>
    /// Computes inter-submap constraints for a node.
    /// Called from work queue after AppendNodeUnsafe.
    /// Assumes lock (_dataLock) held by caller.
    /// </summary>
    private WorkItemResult ComputeConstraintsForNodeUnsafe(
        NodeId nodeId,
        TrajectoryNode.Data constantData,
        int trajectoryId,
        List<Mapping.D3D.Submap3D> insertionSubmaps,
        List<SubmapId> submapIds,
        Rigid3d optimizedPose,
        bool newlyFinishedSubmap)
    {
        // Schedule inter-submap constraint search with ConstraintBuilder3D if available
        if (_constraintBuilder != null && insertionSubmaps.Count > 0 && submapIds.Count > 0)
        {
            var firstSubmap = insertionSubmaps[0];
            var firstSubmapId = submapIds[0];

            if (firstSubmap.InsertionFinished)
            {
                bool constraintsAdded = false;

                // Find finished submaps to match against (inter-submap constraints)
                foreach (var kvp in _submapData)
                {
                    if (kvp.Id.TrajectoryId == trajectoryId &&
                        kvp.Data.Submap is Mapping.D3D.Submap3D finishedSubmap3D &&
                        finishedSubmap3D.InsertionFinished &&
                        kvp.Id != firstSubmapId) // Don't match against itself
                    {
                        var node = _trajectoryNodes[nodeId];
                        var finishedSubmapPose = kvp.Data.Pose;

                        // Schedule constraint computation (inter-submap constraint)
                        _constraintBuilder.MaybeAddConstraint(
                            kvp.Id,
                            nodeId,
                            finishedSubmap3D,
                            node,
                            optimizedPose,
                            finishedSubmapPose);
                        constraintsAdded = true;
                    }
                }

                // Also try global constraint search if configured
                if (_options.GlobalSamplingRatio > 0)
                {
                    // Sample based on global_sampling_ratio
                    var random = new Random((int)nodeId.NodeIndex);
                    if (random.NextDouble() < _options.GlobalSamplingRatio)
                    {
                        foreach (var kvp in _submapData)
                        {
                            if (kvp.Data.Submap is Mapping.D3D.Submap3D globalSubmap3D &&
                                globalSubmap3D.InsertionFinished)
                            {
                                var node = _trajectoryNodes[nodeId];
                                var globalSubmapPose = kvp.Data.Pose;

                                // Get rotations for global matching
                                // MaybeAddGlobalConstraint takes Quaternion, which we can extract from Rigid3d
                                var globalNodeRotation = optimizedPose.Rotation;
                                var globalSubmapRotation = globalSubmapPose.Rotation;

                                _constraintBuilder.MaybeAddGlobalConstraint(
                                    kvp.Id,
                                    nodeId,
                                    globalSubmap3D,
                                    node,
                                    globalNodeRotation,
                                    globalSubmapRotation);
                                constraintsAdded = true;
                            }
                        }
                    }
                }

                // Call WhenDone once after all constraint computations are scheduled
                if (constraintsAdded)
                {
                    _constraintBuilder.WhenDone(OnConstraintBuilderResult);
                }
            }
        }

        // Check if periodic optimization should be triggered
        // FIXED: Use GLOBAL counter instead of per-trajectory (match C++ behavior)
        if (!_numNodesSinceLastOptimization.TryGetValue(trajectoryId, out int value))
        {
            value = 0;
            _numNodesSinceLastOptimization[trajectoryId] = value;
        }
        _numNodesSinceLastOptimization[trajectoryId] = ++value;

        if (_options.OptimizeEveryNNodes > 0 && value >= _options.OptimizeEveryNNodes)
        {
            // Reset counter and signal optimization needed
            _numNodesSinceLastOptimization[trajectoryId] = 0;
            return WorkItemResult.RunOptimization;
        }

        return WorkItemResult.DoNotRunOptimization;
    }

    /// <summary>
    /// Callback invoked when ConstraintBuilder3D finishes computing constraints.
    /// Match C++ HandleWorkQueue behavior: Add constraints then run optimization.
    /// </summary>
    private void OnConstraintBuilderResult(ConstraintBuilder3DResult result)
    {
        if (result.Constraints == null || result.Constraints.Count == 0)
        {
            return;
        }

        // Match C++ HandleWorkQueue: Add constraints with dataLock
        // FIXED: Use _dataLock instead of _optimizationLock for data access
        lock (_dataLock)
        {
            foreach (var constraint in result.Constraints)
            {
                _constraints.Add(constraint);
            }
        }

        // Match C++ HandleWorkQueue: Run optimization SYNCHRONOUSLY after adding constraints
        RunOptimizationSync();

        // Invoke global SLAM optimization callback
        InvokeGlobalSlamOptimizationCallback();
    }

    /// <summary>
    /// Adds connectivity and sampler for a trajectory if it does not exist.
    /// </summary>
    private void AddTrajectoryIfNeeded(int trajectoryId)
    {
        if (!_trajectoryStates.TryGetValue(trajectoryId, out IPoseGraph.TrajectoryState state))
        {
            state = IPoseGraph.TrajectoryState.Active;
            _trajectoryStates[trajectoryId] = state;
        }

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

    /// <summary>
    /// Gets the local to global transform for a trajectory (private helper).
    /// </summary>
    private Rigid3d GetLocalToGlobalTransformInternal(int trajectoryId)
    {
        if (_localToGlobalTransforms.TryGetValue(trajectoryId, out var transform))
        {
            return transform;
        }
        return Rigid3d.Identity;
    }

    /// <summary>
    /// Computes the local to global map frame transform based on the given global submap poses.
    /// </summary>
    private Rigid3d ComputeLocalToGlobalTransform(
        MapById<SubmapId, SubmapSpec3D> globalSubmapPoses,
        int trajectoryId)
    {
        // Find first submap for this trajectory
        var firstSubmapEnum = globalSubmapPoses.BeginOfTrajectory(trajectoryId);
        var firstSubmapList = firstSubmapEnum.ToList();
        if (firstSubmapList.Count == 0)
        {
            return Rigid3d.Identity;
        }

        var firstSubmap = firstSubmapList[0];
        
        // Get the submap from our data
        var submapId = firstSubmap.Id;
        if (!_submapData.Contains(submapId))
        {
            return Rigid3d.Identity;
        }

        if (_submapData[submapId].Submap is not Mapping.D3D.Submap3D submap)
        {
            return Rigid3d.Identity;
        }

        // Compute transform: global_submap_pose * local_submap_pose^-1
        var globalSubmapPose = firstSubmap.Data.GlobalPose;
        var localSubmapPose = submap.LocalPose;
        return globalSubmapPose * localSubmapPose.Inverse();
    }

    // PoseGraphInterface implementation
    public override void AddImuData(int trajectoryId, ImuData imuData)
    {
        if (!_imuData.TryGetValue(trajectoryId, out List<ImuData>? value))
        {
            value = [];
            _imuData[trajectoryId] = value;
        }

        value.Add(imuData);
        
        _optimizationProblem?.AddImuData(trajectoryId, imuData);
    }

    public override void AddOdometryData(int trajectoryId, OdometryData odometryData)
    {
        if (!_odometryData.TryGetValue(trajectoryId, out List<OdometryData>? value))
        {
            value = [];
            _odometryData[trajectoryId] = value;
        }

        value.Add(odometryData);
        
        _optimizationProblem?.AddOdometryData(trajectoryId, odometryData);
    }

    public override void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData)
    {
        if (!_fixedFramePoseData.TryGetValue(trajectoryId, out List<FixedFramePoseData>? value))
        {
            value = [];
            _fixedFramePoseData[trajectoryId] = value;
        }

        value.Add(fixedFramePoseData);
        
        _optimizationProblem?.AddFixedFramePoseData(trajectoryId, fixedFramePoseData);
    }

    public override void AddLandmarkData(int trajectoryId, LandmarkData landmarkData)
    {
        foreach (var observation in landmarkData.LandmarkObservations)
        {
            if (!_landmarkNodes.TryGetValue(observation.Id, out IPoseGraph.LandmarkNode node))
            {
                node = new IPoseGraph.LandmarkNode();
                _landmarkNodes[observation.Id] = node;
            }

            node.LandmarkObservations.Add(new IPoseGraph.LandmarkNode.LandmarkObservation(
                trajectoryId,
                landmarkData.Time,
                observation.LandmarkToTrackingTransform,
                observation.TranslationWeight,
                observation.RotationWeight
            ));
            _landmarkNodes[observation.Id] = node;
        }
    }

    public override void FinishTrajectory(int trajectoryId)
    {
        if (_trajectoryStates.ContainsKey(trajectoryId))
        {
            _trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Finished;
        }
    }

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

    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 void DeleteTrajectory(int trajectoryId)
    {
        _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);
        }
        
        // Remove constraints involving this trajectory
        _constraints.RemoveAll(c => 
            c.SubmapId.TrajectoryId == trajectoryId || 
            c.NodeId.TrajectoryId == trajectoryId);
    }

    public override void AddSubmapFromProto(Rigid3d globalPose, Models.Mapping.Submap submap)
    {
        if (!submap.Submap3D.HasValue)
        {
            throw new ArgumentException("Submap must contain Submap3D", nameof(submap));
        }

        var submap3D = new Mapping.D3D.Submap3D(submap.Submap3D.Value);
        var trajectoryId = submap.SubmapId.TrajectoryId;
        var submapIndex = submap.SubmapId.SubmapIndex;
        var submapId = new SubmapId(trajectoryId, submapIndex);

        _submapData.Insert(submapId, new IPoseGraph.SubmapData(submap3D, globalPose));
        
        _optimizationProblem?.InsertSubmap(submapId, globalPose);
    }

    public override void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node)
    {
        var nodeData = TrajectoryNodeOperations.FromProto(node.NodeData);
        var trajectoryId = node.NodeId.TrajectoryId;
        var nodeIndex = node.NodeId.NodeIndex;
        var nodeId = new NodeId(trajectoryId, nodeIndex);

        _trajectoryNodes.Insert(nodeId, new TrajectoryNode
        {
            ConstantData = nodeData,
            GlobalPose = globalPose
        });

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

        _optimizationProblem?.InsertTrajectoryNode(nodeId, new NodeSpec3D(
                nodeData.Time,
                nodeData.LocalPose,
                globalPose
            ));
    }

    public override void SetTrajectoryDataFromProto(Models.Mapping.TrajectoryData data)
    {
        var trajectoryData = new IPoseGraph.TrajectoryData(
            data.GravityConstant,
            data.ImuCalibration.HasValue ? (Quaternion)data.ImuCalibration.Value : Quaternion.Identity,
            data.FixedFrameOriginInMap.HasValue ? (Rigid3d)data.FixedFrameOriginInMap.Value : null
        );

        // Store trajectory data - trajectory ID should be in the proto
        var trajectoryId = data.TrajectoryId;
        _trajectoryData[trajectoryId] = trajectoryData;

        // Set trajectory data in optimization problem if available
        if (_optimizationProblem != null)
        {
            // Note: OptimizationProblem3D may have SetTrajectoryData method
            // For now, trajectory data is stored in _trajectoryData dictionary
            // and will be used during optimization if needed
        }
    }

    public override void AddNodeToSubmap(NodeId nodeId, SubmapId submapId)
    {
        // C++ line 763-772: AddNodeToSubmap only inserts node_id into submap_data.node_ids
        // It does NOT compute constraints here. Constraints are computed in ComputeConstraintsForNode
        // Track node-to-submap insertion for constraint builder
        if (!_submapNodeInsertions.TryGetValue(submapId, out var nodes))
        {
            nodes = [];
            _submapNodeInsertions[submapId] = nodes;
        }
        nodes.Add(nodeId);
        
        // Note: Constraints are computed in AppendNode/ComputeConstraintsForNode, not here
        // This method only tracks the node-to-submap relationship
    }

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

    public override void RunFinalOptimization()
    {
        var optimizationProblemOptions = _options.OptimizationProblemOptions ?? new OptimizationProblemOptions();
        _optimizationProblem ??= new OptimizationProblem3D(optimizationProblemOptions);

        // Sync submaps and nodes to optimization problem
        // Add all submaps
        foreach (var kvp in _submapData)
        {
            if (!_optimizationProblem.SubmapData().Contains(kvp.Id))
            {
                _optimizationProblem.InsertSubmap(kvp.Id, kvp.Data.Pose);
            }
        }

        // Add all nodes
        foreach (var kvp in _trajectoryNodes)
        {
            if (!_optimizationProblem.NodeData().Contains(kvp.Id))
            {
                var nodeData = kvp.Data.ConstantData;
                if (nodeData != null)
                {
                    _optimizationProblem.InsertTrajectoryNode(kvp.Id, new NodeSpec3D(
                        nodeData.Time,
                        nodeData.LocalPose,
                        kvp.Data.GlobalPose
                    ));
                }
            }
        }

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

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

        // Use MaxNumFinalIterations for final optimization if available
        // This allows more iterations for better convergence in final optimization
        if (_options.MaxNumFinalIterations > 0)
        {
            _optimizationProblem.SetMaxNumIterations(_options.MaxNumFinalIterations);
        }

        // Solve optimization problem
        _optimizationProblem.Solve(Constraints(), trajectoryStates, landmarkNodes);

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

        // Update poses from optimization results
        // Update submap poses
        var optimizedSubmapData = _optimizationProblem.SubmapData();
        foreach (var kvp in optimizedSubmapData)
        {
            if (_submapData.Contains(kvp.Id))
            {
                var submapData = _submapData[kvp.Id];
                _submapData[kvp.Id] = new IPoseGraph.SubmapData(submapData.Submap, kvp.Data.GlobalPose);
            }
        }

        // Update node poses
        var optimizedNodeData = _optimizationProblem.NodeData();
        foreach (var kvp in optimizedNodeData)
        {
            if (_trajectoryNodes.Contains(kvp.Id))
            {
                var optimizedPose3D = kvp.Data.GlobalPose;
                
                // Update trajectory node pose
                if (_trajectoryNodePoses.Contains(kvp.Id))
                {
                    var nodePose = _trajectoryNodePoses[kvp.Id];
                    nodePose.GlobalPose = optimizedPose3D;
                    _trajectoryNodePoses[kvp.Id] = nodePose;
                }

                // Update trajectory node
                var node = _trajectoryNodes[kvp.Id];
                node.GlobalPose = optimizedPose3D;
                _trajectoryNodes[kvp.Id] = node;
            }
        }

        // Update local to global transforms
        foreach (var trajectoryId in _trajectoryStates.Keys)
        {
            var transform = ComputeLocalToGlobalTransform(_optimizationProblem.SubmapData(), trajectoryId);
            _localToGlobalTransforms[trajectoryId] = transform;
        }

        // Run trimmers after optimization
        RunTrimmers();
    }

    /// <summary>
    /// Runs optimization (can be called periodically or as final optimization).
    /// Thread-safe: only one optimization runs at a time.
    /// </summary>
    private void RunOptimization()
    {
        lock (_optimizationLock)
        {
            // If optimization is already in progress, skip
            if (_optimizationInProgress)
            {
                return;
            }

            _optimizationInProgress = true;
        }

        // Run optimization on dedicated high-priority thread to avoid blocking
        _optimizationThread = new Thread(() =>
        {
            Thread.BeginThreadAffinity();
            try
            {
                RunFinalOptimization();
            }
            finally
            {
                Thread.EndThreadAffinity();
                lock (_optimizationLock)
                {
                    _optimizationInProgress = false;
                }
            }
        })
        {
            Priority = ThreadPriority.Highest,
            IsBackground = false,
            Name = "CartographerOptimization3D"
        };
        _optimizationThread.Start();
    }

    /// <summary>
    /// Runs optimization synchronously in the calling thread.
    /// Match C++ RunOptimization() implementation and PoseGraph2D.RunOptimizationSync().
    /// Thread-safe: only one optimization runs at a time.
    /// If optimization is already running, skips (does not queue).
    /// </summary>
    private void RunOptimizationSync()
    {
        lock (_optimizationLock)
        {
            // If optimization is already in progress, skip
            if (_optimizationInProgress)
            {
                return;
            }

            _optimizationInProgress = true;
        }

        try
        {
            // Match C++: Check if optimization problem is empty
            if (_optimizationProblem == null || _optimizationProblem.SubmapData().IsEmpty)
            {
                return;
            }

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

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

            // Solve optimization problem with lock for data access
            lock (_dataLock)
            {
                _optimizationProblem.Solve(Constraints(), trajectoryStates, landmarkNodes);

                // Update poses from optimization results
                var optimizedSubmapData = _optimizationProblem.SubmapData();
                foreach (var kvp in optimizedSubmapData)
                {
                    if (_submapData.Contains(kvp.Id))
                    {
                        var submapData = _submapData[kvp.Id];
                        _submapData[kvp.Id] = new IPoseGraph.SubmapData(submapData.Submap, kvp.Data.GlobalPose);
                    }
                }

                // Update node poses
                var optimizedNodeData = _optimizationProblem.NodeData();
                foreach (var kvp in optimizedNodeData)
                {
                    if (_trajectoryNodes.Contains(kvp.Id))
                    {
                        var optimizedPose3D = kvp.Data.GlobalPose;

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

                        // Update trajectory node
                        var node = _trajectoryNodes[kvp.Id];
                        node.GlobalPose = optimizedPose3D;
                        _trajectoryNodes[kvp.Id] = node;
                    }
                }

                // Update local to global transforms
                foreach (var trajectoryId in _trajectoryStates.Keys)
                {
                    var transform = ComputeLocalToGlobalTransform(_optimizationProblem.SubmapData(), trajectoryId);
                    _localToGlobalTransforms[trajectoryId] = transform;
                }
            }

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

            // Run trimmers after optimization
            RunTrimmers();
        }
        finally
        {
            lock (_optimizationLock)
            {
                _optimizationInProgress = false;
            }
        }
    }

    /// <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);
    }

    /// <summary>
    /// Runs all registered trimmers.
    /// </summary>
    private void RunTrimmers()
    {
        foreach (var trimmer in _trimmers)
        {
            trimmer.Trim(new TrimmablePoseGraph3D(this));
        }
    }

    public override MapById<SubmapId, IPoseGraph.SubmapData> GetAllSubmapData()
    {
        return _submapData;
    }

    public override IPoseGraph.SubmapData GetSubmapData(SubmapId submapId)
    {
        if (_submapData.Contains(submapId))
        {
            return _submapData[submapId];
        }
        return new IPoseGraph.SubmapData(null, Rigid3d.Identity);
    }

    public override MapById<SubmapId, IPoseGraph.SubmapPose> GetAllSubmapPoses()
    {
        var result = new MapById<SubmapId, IPoseGraph.SubmapPose>();
        foreach (var kvp in _submapData)
        {
            var version = 0;
            if (kvp.Data.Submap is Mapping.D3D.Submap3D submap3D)
            {
                version = submap3D.NumRangeData;
            }
            result.Insert(kvp.Id, new IPoseGraph.SubmapPose(version, kvp.Data.Pose));
        }
        return result;
    }

    public override Rigid3d GetLocalToGlobalTransform(int trajectoryId)
    {
        if (_localToGlobalTransforms.TryGetValue(trajectoryId, out var transform))
        {
            return transform;
        }
        return Rigid3d.Identity;
    }

    public override MapById<NodeId, TrajectoryNode> GetTrajectoryNodes()
    {
        return _trajectoryNodes;
    }

    public override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
    {
        return _trajectoryNodePoses;
    }

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

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

    public override void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false)
    {
        _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 Dictionary<string, IPoseGraph.LandmarkNode> GetLandmarkNodes()
    {
        return new Dictionary<string, IPoseGraph.LandmarkNode>(_landmarkNodes);
    }

    public override Dictionary<int, IPoseGraph.TrajectoryData> GetTrajectoryData()
    {
        return new Dictionary<int, IPoseGraph.TrajectoryData>(_trajectoryData);
    }

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

    public override void SetInitialTrajectoryPose(int fromTrajectoryId, int toTrajectoryId, Rigid3d pose, long time)
    {
        _initialTrajectoryPoses[fromTrajectoryId] = new InitialTrajectoryPose(toTrajectoryId, pose, time);
    }

    /// <summary>
    /// Sets localization initial poses for relocalizing against the map.
    /// Match C++: SetLocalizationInitialPoses (pose_graph_3d.h:166) - empty implementation in 3D.
    /// </summary>
    public override void SetLocalizationInitialPoses(IReadOnlyList<Rigid3d> localizationInitialPoses)
    {
        // Empty implementation - 3D pose graph does not support relocalization
    }

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

    /// <summary>
    /// 3D pose graph does not use transform_to_map (C++ override is empty).
    /// </summary>
    public override void SetTransformToMap(Rigid3d transform) { }

    public override Rigid3d GetTransformToMap() => Rigid3d.Identity;

    public override List<List<int>> GetConnectedTrajectories()
    {
        // Simplified implementation - full implementation would use trajectory connectivity state
        var result = new List<List<int>>();
        var processed = new HashSet<int>();
        
        foreach (var trajectoryId in _trajectoryStates.Keys)
        {
            if (processed.Contains(trajectoryId))
                continue;
                
            var connected = new List<int> { trajectoryId };
            processed.Add(trajectoryId);
            
            // Find connected trajectories via constraints
            foreach (var constraint in _constraints)
            {
                var submapTrajId = constraint.SubmapId.TrajectoryId;
                var nodeTrajId = constraint.NodeId.TrajectoryId;
                
                if (submapTrajId == trajectoryId && !processed.Contains(nodeTrajId))
                {
                    connected.Add(nodeTrajId);
                    processed.Add(nodeTrajId);
                }
                else if (nodeTrajId == trajectoryId && !processed.Contains(submapTrajId))
                {
                    connected.Add(submapTrajId);
                    processed.Add(submapTrajId);
                }
            }
            
            result.Add(connected);
        }
        
        return result;
    }

    public override Dictionary<int, List<ImuData>> GetImuData()
    {
        return new Dictionary<int, List<ImuData>>(_imuData);
    }

    public override Dictionary<int, List<OdometryData>> GetOdometryData()
    {
        return new Dictionary<int, List<OdometryData>>(_odometryData);
    }

    public override Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData()
    {
        return new Dictionary<int, List<FixedFramePoseData>>(_fixedFramePoseData);
    }

    public override void AddTrimmer(PoseGraphTrimmer trimmer)
    {
        ArgumentNullException.ThrowIfNull(trimmer);

        _trimmers.Add(trimmer);
    }

    /// <summary>
    /// Trimmable interface implementation for PoseGraph3D.
    /// </summary>
    private class TrimmablePoseGraph3D(PoseGraph3D poseGraph) : ITrimmable
    {
        public int NumSubmaps(int trajectoryId)
        {
            int count = 0;
            foreach (var kvp in poseGraph._submapData)
            {
                if (kvp.Id.TrajectoryId == trajectoryId)
                {
                    count++;
                }
            }
            return count;
        }

        public List<SubmapId> GetSubmapIds(int trajectoryId)
        {
            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()
        {
            return poseGraph._submapData;
        }

        public MapById<NodeId, TrajectoryNode> GetTrajectoryNodes()
        {
            return poseGraph._trajectoryNodes;
        }

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

        public void TrimSubmap(SubmapId submapId)
        {
            poseGraph._submapData.Trim(submapId);
        }

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

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

    public override Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps)
    {
        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)
                {
                    var nodePose = _trajectoryNodePoses.Contains(kvp.Id) 
                        ? _trajectoryNodePoses[kvp.Id].GlobalPose 
                        : Rigid3d.Identity;
                    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.D3D.Submap3D submap3D)
                    {
                        if (includeUnfinishedSubmaps || submap3D.InsertionFinished)
                        {
                            trajectory.Submaps.Add(new Models.Mapping.Trajectory.Submap(
                                kvp.Id.SubmapIndex,
                                (Rigid3dProto)kvp.Data.Pose
                            ));
                        }
                    }
                }
            }
            
            proto.Trajectories.Add(trajectory);
        }
        
        return proto;
    }

    // Manual compute methods (not implemented for 3D pose graph - these are 2D-only features)

    /// <summary>
    /// Manual constraint computation is not implemented for 3D pose graphs.
    /// This feature is only available in 2D SLAM (PoseGraph2D).
    /// </summary>
    public override (double Score, IPoseGraph.Constraint? Constraint) ManualComputeConstraint(
        NodeId nodeId, SubmapId submapId)
    {
        throw new NotImplementedException("Manual compute methods are not implemented for 3D pose graph. Use PoseGraph2D for these features.");
    }

    /// <summary>
    /// Manual constraint score computation is not implemented for 3D pose graphs.
    /// This feature is only available in 2D SLAM (PoseGraph2D).
    /// </summary>
    public override double ManualComputeConstraintScore(
        NodeId nodeId, SubmapId submapId, Rigid3d initialPose)
    {
        throw new NotImplementedException("Manual compute methods are not implemented for 3D pose graph. Use PoseGraph2D for these features.");
    }

    /// <summary>
    /// Manual scan matcher is not implemented for 3D pose graphs.
    /// This feature is only available in 2D SLAM (PoseGraph2D).
    /// </summary>
    public override double ManualComputeScanMatcher(
        NodeId nodeId, SubmapId submapId, Rigid3d initialPose, out Rigid3d poseManualEstimate)
    {
        poseManualEstimate = Rigid3d.Identity;
        throw new NotImplementedException("Manual compute methods are not implemented for 3D pose graph. Use PoseGraph2D for these features.");
    }

    /// <summary>
    /// Manual relocalization is not implemented for 3D pose graphs.
    /// This feature is only available in 2D SLAM (PoseGraph2D).
    /// </summary>
    public override bool ManualRelocalization(
        int trajectoryId, out double score, Rigid2d initialPose, LocalizationResultCallback? callback)
    {
        score = 0.0;
        throw new NotImplementedException("Manual relocalization is not implemented for 3D pose graph. Use PoseGraph2D for this feature.");
    }

    /// <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;

        // Dispose work queue first (this will drain remaining items and stop background thread)
        _workQueue.Dispose();

        // Wait for optimization thread to complete (with timeout)
        if (_optimizationThread != null)
        {
            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
        // Note: After optimization thread is joined, it's safe to clear without lock
        // but we use lock for extra safety in case of concurrent access
        lock (_optimizationLock)
        {
            _imuData.Clear();
            _odometryData.Clear();
            _fixedFramePoseData.Clear();
            _submapNodeInsertions.Clear();
        }

        GC.SuppressFinalize(this);
    }
}