/*
 * 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.Models.Mapping;
using CartographerSharp.Transform;
using CeresSharp;
using CeresSharp.Enums;
using  RobotNet10.Shared.Numbers;
using QuaternionManifold = CeresSharp.QuaternionManifold;

namespace CartographerSharp.Mapping.Internal.Optimization;

/// <summary>
/// Submap specification for optimization.
/// </summary>
public struct SubmapSpec2D(Rigid2d globalPose)
{
    public Rigid2d GlobalPose { get; set; } = globalPose;
}

/// <summary>
/// Node specification for optimization.
/// </summary>
public struct NodeSpec2D(
    long time,
    Rigid2d localPose2D,
    Rigid2d globalPose2D,
    Rigid3d globalPose,
    Quaternion gravityAlignment)
{
    public long Time { get; set; } = time;
    public Rigid2d LocalPose2D { get; set; } = localPose2D;
    public Rigid2d GlobalPose2D { get; set; } = globalPose2D;
    public Rigid3d GlobalPose { get; set; } = globalPose;
    public Quaternion GravityAlignment { get; set; } = gravityAlignment;
}

/// <summary>
/// Optimization problem for 2D pose graph.
/// </summary>
public class OptimizationProblem2D(OptimizationProblemOptions options)
{
    private int? _maxNumIterations; // Override value set via SetMaxNumIterations()
    private readonly MapById<SubmapId, SubmapSpec2D> _submapData = new();
    private readonly MapById<NodeId, NodeSpec2D> _nodeData = new();
    private readonly Dictionary<string, Rigid3d> _landmarkData = [];

    // Sensor data storage (simplified - using List for now, full implementation would use MapByTime)
    private readonly Dictionary<int, List<Sensor.OdometryData>> _odometryData = [];
    private readonly Dictionary<int, List<Sensor.FixedFramePoseData>> _fixedFramePoseData = [];
    private readonly Dictionary<int, IPoseGraph.TrajectoryData> _trajectoryData = [];

    /// <summary>
    /// Sets maximum number of iterations (match C++ SetMaxNumIterations).
    /// </summary>
    public void SetMaxNumIterations(int maxNumIterations)
    {
        _maxNumIterations = maxNumIterations;
    }

    /// <summary>
    /// Adds a submap to the optimization problem.
    /// FIX: Use GetNextSubmapIndex instead of SizeOfTrajectoryOrZero to handle trimmed trajectories.
    /// After trimming, Count can collide with existing indices (e.g., trim index 0 leaves {1,2},
    /// Count=2 would create index 2 which already exists). Using Max()+1 avoids this.
    /// </summary>
    public void AddSubmap(int trajectoryId, Rigid2d globalPose)
    {
        var nextIndex = GetNextSubmapIndex(trajectoryId);
        var submapId = new SubmapId(trajectoryId, nextIndex);
        _submapData.Insert(submapId, new SubmapSpec2D(globalPose));
    }

    /// <summary>
    /// Gets the next sequential submap index for a trajectory, accounting for trimmed entries.
    /// </summary>
    private int GetNextSubmapIndex(int trajectoryId)
    {
        var size = _submapData.SizeOfTrajectoryOrZero(trajectoryId);
        if (size == 0) return 0;
        // Find the max existing index and use max + 1
        var last = _submapData.GetLastOfTrajectory(trajectoryId);
        return last.HasValue ? last.Value.Id.SubmapIndex + 1 : 0;
    }

    /// <summary>
    /// Adds a submap to the optimization problem with a specific SubmapId.
    /// If the submap already exists, it will be updated with the new pose.
    /// </summary>
    public void AddSubmap(SubmapId submapId, Rigid2d globalPose)
    {
        if (_submapData.Contains(submapId))
        {
            // Update existing submap pose
            var existingSpec = _submapData[submapId];
            existingSpec.GlobalPose = globalPose;
            _submapData[submapId] = existingSpec;
        }
        else
        {
            _submapData.Insert(submapId, new SubmapSpec2D(globalPose));
        }
    }

    /// <summary>
    /// Adds a node to the optimization problem.
    /// FIX: Use GetLastOfTrajectory to compute next index, same reasoning as AddSubmap.
    /// </summary>
    public void AddNode(
        int trajectoryId,
        long time,
        Rigid2d localPose2D,
        Rigid2d globalPose2D,
        Rigid3d globalPose,
        Quaternion gravityAlignment)
    {
        var size = _nodeData.SizeOfTrajectoryOrZero(trajectoryId);
        int nextIndex;
        if (size == 0)
        {
            nextIndex = 0;
        }
        else
        {
            var last = _nodeData.GetLastOfTrajectory(trajectoryId);
            nextIndex = last.HasValue ? last.Value.Id.NodeIndex + 1 : 0;
        }
        var nodeId = new NodeId(trajectoryId, nextIndex);
        _nodeData.Insert(nodeId, new NodeSpec2D(time, localPose2D, globalPose2D, globalPose, gravityAlignment));
    }

    /// <summary>
    /// Adds a node to the optimization problem (backward compatibility).
    /// </summary>
    public void AddNode(int trajectoryId, Rigid2d globalPose2D, Rigid3d globalPose)
    {
        // Use default values for missing fields (for backward compatibility)
        var nodeId = new NodeId(trajectoryId, _nodeData.SizeOfTrajectoryOrZero(trajectoryId));
        var localPose2D = globalPose2D; // Assume same as global for backward compatibility
        _nodeData.Insert(nodeId, new NodeSpec2D(0, localPose2D, globalPose2D, globalPose, Quaternion.Identity));
    }

    /// <summary>
    /// Inserts a node with a specific NodeId (match C++ InsertTrajectoryNode).
    /// Used when loading from proto to restore exact node IDs.
    /// </summary>
    public void InsertTrajectoryNode(NodeId nodeId, NodeSpec2D nodeData)
    {
        _nodeData.Insert(nodeId, nodeData);
        if (!_trajectoryData.ContainsKey(nodeId.TrajectoryId))
            _trajectoryData[nodeId.TrajectoryId] = default;
    }

    /// <summary>
    /// Gets submap data.
    /// </summary>
    public MapById<SubmapId, SubmapSpec2D> SubmapData()
    {
        return _submapData;
    }

    /// <summary>
    /// Gets node data.
    /// </summary>
    public MapById<NodeId, NodeSpec2D> NodeData()
    {
        return _nodeData;
    }

    /// <summary>
    /// Trims a submap from the optimization problem (match C++ optimization_problem_->TrimSubmap).
    /// </summary>
    public void TrimSubmap(SubmapId submapId)
    {
        _submapData.Trim(submapId);
    }

    /// <summary>
    /// Trims a node from the optimization problem (match C++ optimization_problem_->TrimTrajectoryNode).
    /// C++ also trims odometry_data_ and fixed_frame_pose_data_ by the time "gap" of the removed node
    /// (MapByTime.Trim): remove data strictly between prev and next node time, retaining first and last in gap.
    /// </summary>
    public void TrimTrajectoryNode(NodeId nodeId)
    {
        // Match C++: trim sensor data BEFORE trimming the node (Trim needs node_id in nodes to get gap)
        var (gapStart, gapEnd) = GetNodeTimeGapUnsafe(nodeId);
        if (gapStart < gapEnd)
        {
            TrimOdometryDataByGap(nodeId.TrajectoryId, gapStart, gapEnd);
            TrimFixedFramePoseDataByGap(nodeId.TrajectoryId, gapStart, gapEnd);
        }

        _nodeData.Trim(nodeId);
        if (_nodeData.SizeOfTrajectoryOrZero(nodeId.TrajectoryId) == 0)
            _trajectoryData.Remove(nodeId.TrajectoryId);
    }

    /// <summary>
    /// Gets (gap_start, gap_end) for the node: prev node time and next node time (match C++ MapByTime.Trim).
    /// Returns (long.MinValue, long.MaxValue) if node not found or trajectory has no prev/next.
    /// </summary>
    private (long gapStart, long gapEnd) GetNodeTimeGapUnsafe(NodeId nodeId)
    {
        if (!_nodeData.Contains(nodeId))
            return (long.MinValue, long.MaxValue);
        var list = _nodeData.BeginOfTrajectory(nodeId.TrajectoryId)
            .OrderBy(x => x.Id.GetIndex())
            .ToList();
        var idx = list.FindIndex(x => x.Id.GetIndex() == nodeId.NodeIndex);
        if (idx < 0)
            return (long.MinValue, long.MaxValue);
        long gapStart = idx > 0 ? list[idx - 1].Data.Time : long.MinValue;
        long gapEnd = idx < list.Count - 1 ? list[idx + 1].Data.Time : long.MaxValue;
        return (gapStart, gapEnd);
    }

    /// <summary>
    /// Trims odometry data in (gap_start, gap_end), retaining first and last in gap (match C++ MapByTime.Trim).
    /// </summary>
    private void TrimOdometryDataByGap(int trajectoryId, long gapStart, long gapEnd)
    {
        if (!_odometryData.TryGetValue(trajectoryId, out var list) || list.Count == 0)
            return;
        // Indices where time > gapStart && time < gapEnd
        var indicesInGap = new List<int>();
        for (int i = 0; i < list.Count; i++)
        {
            if (list[i].Time > gapStart && list[i].Time < gapEnd)
                indicesInGap.Add(i);
        }
        if (indicesInGap.Count <= 2)
            return;
        // Remove middle elements (keep first and last in gap)
        for (int k = indicesInGap.Count - 2; k >= 1; k--)
            list.RemoveAt(indicesInGap[k]);
        if (list.Count == 0)
            _odometryData.Remove(trajectoryId);
    }

    /// <summary>
    /// Trims fixed frame pose data in (gap_start, gap_end), retaining first and last in gap (match C++ MapByTime.Trim).
    /// </summary>
    private void TrimFixedFramePoseDataByGap(int trajectoryId, long gapStart, long gapEnd)
    {
        if (!_fixedFramePoseData.TryGetValue(trajectoryId, out var list) || list.Count == 0)
            return;
        var indicesInGap = new List<int>();
        for (int i = 0; i < list.Count; i++)
        {
            if (list[i].Time > gapStart && list[i].Time < gapEnd)
                indicesInGap.Add(i);
        }
        if (indicesInGap.Count <= 2)
            return;
        for (int k = indicesInGap.Count - 2; k >= 1; k--)
            list.RemoveAt(indicesInGap[k]);
        if (list.Count == 0)
            _fixedFramePoseData.Remove(trajectoryId);
    }

    /// <summary>
    /// Gets landmark data.
    /// </summary>
    public Dictionary<string, Rigid3d> LandmarkData()
    {
        return _landmarkData;
    }

    /// <summary>
    /// Adds odometry data.
    /// </summary>
    public void AddOdometryData(int trajectoryId, Sensor.OdometryData odometryData)
    {
        if (!_odometryData.TryGetValue(trajectoryId, out List<Sensor.OdometryData>? value))
        {
            value = [];
            _odometryData[trajectoryId] = value;
        }

        value.Add(odometryData);
    }

    /// <summary>
    /// Adds fixed frame pose data.
    /// </summary>
    public void AddFixedFramePoseData(int trajectoryId, Sensor.FixedFramePoseData fixedFramePoseData)
    {
        if (!_fixedFramePoseData.TryGetValue(trajectoryId, out List<Sensor.FixedFramePoseData>? value))
        {
            value = [];
            _fixedFramePoseData[trajectoryId] = value;
        }

        value.Add(fixedFramePoseData);
    }

    /// <summary>
    /// Sets trajectory data.
    /// </summary>
    public void SetTrajectoryData(int trajectoryId, IPoseGraph.TrajectoryData trajectoryData)
    {
        _trajectoryData[trajectoryId] = trajectoryData;
    }

    /// <summary>
    /// Interpolates odometry data at the given time.
    /// </summary>
    private Rigid3d? InterpolateOdometry(int trajectoryId, long time)
    {
        if (!_odometryData.TryGetValue(trajectoryId, out List<Sensor.OdometryData>? odometryList) || odometryList.Count == 0)
        {
            return null;
        }

        // Find the odometry data at or after the given time
        var it = odometryList.FirstOrDefault(o => o.Time >= time);
        if (it.Time == 0 && odometryList.Count > 0 && odometryList[0].Time > time)
        {
            // Time is before first odometry data
            return null;
        }

        if (it.Time == 0)
        {
            // Time is after all odometry data, find the last one
            if (odometryList.Count == 0)
                return null;
            it = odometryList[^1];
            if (it.Time == time)
            {
                return it.Pose;
            }
            return null; // Cannot extrapolate
        }

        // Find previous odometry data
        var prevIndex = odometryList.IndexOf(it);
        if (prevIndex == 0)
        {
            if (it.Time == time)
            {
                return it.Pose;
            }
            return null; // Cannot interpolate before first data
        }

        var prevIt = odometryList[prevIndex - 1];

        // Interpolate between prev and it
        var timeDiff = it.Time - prevIt.Time;
        if (timeDiff == 0)
        {
            return prevIt.Pose;
        }

        var interpolatedPose = TransformOperations.Interpolate(
            prevIt.Pose,
            prevIt.Time,
            it.Pose,
            it.Time,
            time
        );

        return interpolatedPose;
    }

    /// <summary>
    /// Calculates relative odometry between two nodes.
    /// </summary>
    private Rigid3d? CalculateOdometryBetweenNodes(
        int trajectoryId,
        NodeSpec2D firstNodeData,
        NodeSpec2D secondNodeData)
    {
        if (!_odometryData.ContainsKey(trajectoryId))
        {
            return null;
        }

        var firstOdometry = InterpolateOdometry(trajectoryId, firstNodeData.Time);
        var secondOdometry = InterpolateOdometry(trajectoryId, secondNodeData.Time);

        if (firstOdometry == null || secondOdometry == null)
        {
            return null;
        }

        // Compute relative odometry with gravity alignment
        var firstGravityRotation = new Rigid3d(Vector3.Zero, firstNodeData.GravityAlignment);
        var secondGravityRotation = new Rigid3d(Vector3.Zero, secondNodeData.GravityAlignment);

        var relativeOdometry = firstGravityRotation *
            firstOdometry.Value.Inverse() *
            secondOdometry.Value *
            secondGravityRotation.Inverse();

        return relativeOdometry;
    }

    /// <summary>
    /// Interpolates fixed frame pose data at the given time.
    /// </summary>
    private Rigid3d? InterpolateFixedFramePose(int trajectoryId, long time)
    {
        if (!_fixedFramePoseData.TryGetValue(trajectoryId, out List<Sensor.FixedFramePoseData>? fixedFramePoseList) || fixedFramePoseList.Count == 0)
        {
            return null;
        }

        // Find the fixed frame pose data at or after the given time
        var it = fixedFramePoseList.FirstOrDefault(f => f.Time >= time);
        if (it.Time == 0 && fixedFramePoseList.Count > 0 && fixedFramePoseList[0].Time > time)
        {
            return null;
        }

        if (it.Time == 0)
        {
            if (fixedFramePoseList.Count == 0)
                return null;
            it = fixedFramePoseList[^1];
            if (it.Time == time)
            {
                return it.Pose;
            }
            return null;
        }

        var prevIndex = fixedFramePoseList.IndexOf(it);
        if (prevIndex == 0)
        {
            if (it.Time == time)
            {
                return it.Pose;
            }
            return null;
        }

        var prevIt = fixedFramePoseList[prevIndex - 1];

        if (!prevIt.Pose.HasValue)
        {
            return null;
        }

        if (!it.Pose.HasValue)
        {
            return null;
        }

        var timeDiff = it.Time - prevIt.Time;
        if (timeDiff == 0)
        {
            return prevIt.Pose;
        }

        var interpolatedPose = TransformOperations.Interpolate(
            prevIt.Pose.Value,
            prevIt.Time,
            it.Pose.Value,
            it.Time,
            time
        );

        return interpolatedPose;
    }

    /// <summary>
    /// Solves the optimization problem.
    /// </summary>
    /// 
    // Log Ceres version from FullReport (first time only)
    static bool versionLogged = false;
    public void Solve(
        List<IPoseGraph.Constraint> constraints,
        Dictionary<int, IPoseGraph.TrajectoryState> trajectoriesState,
        Dictionary<string, IPoseGraph.LandmarkNode> landmarkNodes,
        int? maxNumIterationsOverride = null)
    {
        if (_nodeData.IsEmpty)
        {
            return;
        }
        
        // Verify Ceres library is loaded and log version info
        // This will be done when we get the first SolverSummary

        // Identify frozen trajectories
        var frozenTrajectories = new HashSet<int>();
        foreach (var kvp in trajectoriesState)
        {
            if (kvp.Value == IPoseGraph.TrajectoryState.Frozen)
            {
                frozenTrajectories.Add(kvp.Key);
            }
        }

        // Create manifolds BEFORE problem so they are disposed AFTER problem
        // (C# using var disposes in reverse declaration order)
        // This ensures native manifold handles remain valid during Problem.Dispose()
        using var sharedPose2DManifold = new CeresSharp.Pose2DManifold();
        using var sharedQuaternionManifold = new QuaternionManifold();

        // Create Ceres problem
        using var problem = new Problem();
        using var solverOptions = new SolverOptions();
        solverOptions.LinearSolverType = LinearSolverType.SparseSchur; // For large problems
        // Use override if provided, otherwise use _maxNumIterations (set via SetMaxNumIterations),
        // otherwise use MaxNumIterations from options, with fallback to default if not set
        solverOptions.MaxNumIterations = maxNumIterationsOverride ?? 
            (_maxNumIterations ?? (options.MaxNumIterations > 0 ? options.MaxNumIterations : 50)); // Default for pose graph optimization
        // Solution 1: Use single thread to eliminate race condition possibility
        // Multi-threading may cause parameter blocks to be modified concurrently during evaluation
        // This could lead to pose explosion when Ceres evaluates cost functions
        solverOptions.NumThreads = 1; // Single-threaded to prevent race conditions (was 4)
        // Relaxed tolerances to allow more iterations and better convergence
        // This prevents premature termination when cost is still decreasing
        // Increased tolerance values (less strict) to allow optimization to run more iterations
        // This is important when initial poses are close but need fine-tuning
        solverOptions.FunctionTolerance = 1e-6;      // More strict to allow more iterations (was 1e-4)
        solverOptions.GradientTolerance = 1e-10;    // More strict to allow more iterations (was 1e-8)
        solverOptions.ParameterTolerance = 1e-8;     // More strict to allow more iterations (was 1e-6)
        
        // Solution 3: Improve initial guess using odometry
        // First, improve node initial poses using odometry if available
        ImproveInitialPosesWithOdometry();

        // Convert poses to parameter arrays
        var submapParams = new Dictionary<SubmapId, double[]>();
        var nodeParams = new Dictionary<NodeId, double[]>();

        // Add submap parameter blocks
        // Match C++: Fix first submap or all submaps of frozen trajectories
        bool firstSubmap = true;
        int submapCount = 0;
        foreach (var kvp in _submapData)
        {
            var frozen = frozenTrajectories.Contains(kvp.Id.TrajectoryId);
            var pose = kvp.Data.GlobalPose;
            var poseParams = OptimizationHelpers.Rigid2dToParameters(pose);
            
            // Normalize rotation angle to [-π, π] to prevent bounds violations
            poseParams[2] = OptimizationHelpers.NormalizeAngleDifference(poseParams[2]);
            
            // Validate pose parameters (C# enhancement - C++ doesn't validate, but we keep for safety)
            if (double.IsNaN(poseParams[0]) || double.IsNaN(poseParams[1]) || double.IsNaN(poseParams[2]) ||
                double.IsInfinity(poseParams[0]) || double.IsInfinity(poseParams[1]) || double.IsInfinity(poseParams[2]))
            {
                continue; // Skip invalid submap, but don't update firstSubmap flag
            }
            
            submapParams[kvp.Id] = poseParams;
            
            // Match C++: AddParameterBlock first
            problem.AddParameterBlock(poseParams, 3);
            
            // C# enhancement: Set Pose2D manifold for rotation handling (C++ doesn't use manifold for 2D)
            // Shared manifold instance reused for all parameter blocks
            problem.SetManifold(poseParams, sharedPose2DManifold);
            
            // C# enhancement: Set bounds for translation to prevent explosion (C++ doesn't set bounds)
            // Use much tighter bounds based on initial pose to prevent large deviations
            const double MAX_POSE_BOUND = 10.0; // 10m - tight bound to prevent explosion
            double lowerX = poseParams[0] - MAX_POSE_BOUND;
            double upperX = poseParams[0] + MAX_POSE_BOUND;
            double lowerY = poseParams[1] - MAX_POSE_BOUND;
            double upperY = poseParams[1] + MAX_POSE_BOUND;
            
            problem.SetParameterLowerBound(poseParams, 0, lowerX); // x
            problem.SetParameterUpperBound(poseParams, 0, upperX);
            problem.SetParameterLowerBound(poseParams, 1, lowerY); // y
            problem.SetParameterUpperBound(poseParams, 1, upperY);
            // Note: Rotation bounds are NOT set because AngleManifold handles rotation constraints
            
            // Match C++: Fix first submap or all submaps of frozen trajectories
            // CRITICAL FIX: Only set firstSubmap = false AFTER successfully adding parameter block
            // This ensures the first VALID submap is fixed, not the first in iteration order
            if (firstSubmap || frozen)
            {
                // Fix the pose of the first submap or all submaps of a frozen trajectory
                problem.SetParameterBlockConstant(poseParams);
                // Only update firstSubmap flag after successfully fixing a submap
                if (firstSubmap)
                {
                    firstSubmap = false;
                }
            }
            submapCount++;
        }
        
        // Ensure we have at least one submap
        if (submapParams.Count == 0)
        {
            return; // Nothing to optimize
        }

        // Add node parameter blocks
        // Match C++: Fix nodes of frozen trajectories only (no first node fix)
        int nodeCount = 0;
        foreach (var kvp in _nodeData)
        {
            var frozen = frozenTrajectories.Contains(kvp.Id.TrajectoryId);
            var pose = kvp.Data.GlobalPose2D;
            var poseParams = OptimizationHelpers.Rigid2dToParameters(pose);
            
            // Normalize rotation angle to [-π, π] to prevent bounds violations
            // This is critical because rotation angles can accumulate and exceed ±3.0 rad bounds
            poseParams[2] = OptimizationHelpers.NormalizeAngleDifference(poseParams[2]);
            
            // Validate pose parameters (C# enhancement - C++ doesn't validate, but we keep for safety)
            if (double.IsNaN(poseParams[0]) || double.IsNaN(poseParams[1]) || double.IsNaN(poseParams[2]) ||
                double.IsInfinity(poseParams[0]) || double.IsInfinity(poseParams[1]) || double.IsInfinity(poseParams[2]))
            {
                continue; // Skip invalid node
            }
            
            nodeParams[kvp.Id] = poseParams;
            
            // Match C++: AddParameterBlock first
            problem.AddParameterBlock(poseParams, 3);
            
            // C# enhancement: Set Pose2D manifold for rotation handling (C++ doesn't use manifold for 2D)
            // Shared manifold instance reused for all parameter blocks
            problem.SetManifold(poseParams, sharedPose2DManifold);
            
            // C# enhancement: Set bounds for translation to prevent explosion (C++ doesn't set bounds)
            // Use much tighter bounds based on initial pose to prevent large deviations
            const double MAX_POSE_BOUND = 10.0; // 10m - tight bound to prevent explosion
            double lowerX = poseParams[0] - MAX_POSE_BOUND;
            double upperX = poseParams[0] + MAX_POSE_BOUND;
            double lowerY = poseParams[1] - MAX_POSE_BOUND;
            double upperY = poseParams[1] + MAX_POSE_BOUND;
            
            problem.SetParameterLowerBound(poseParams, 0, lowerX); // x
            problem.SetParameterUpperBound(poseParams, 0, upperX);
            problem.SetParameterLowerBound(poseParams, 1, lowerY); // y
            problem.SetParameterUpperBound(poseParams, 1, upperY);
            // Note: Rotation bounds are NOT set because AngleManifold handles rotation constraints
            
            // Match C++: Fix nodes of frozen trajectories only
            if (frozen)
            {
                problem.SetParameterBlockConstant(poseParams);
            }
            nodeCount++;
        }
        // Solution 4: Detect conflicting constraints BEFORE adding to optimization
        // Group constraints by (SubmapId, NodeId) to detect conflicts
        var constraintGroups = new Dictionary<(SubmapId, NodeId), List<IPoseGraph.Constraint>>();
        foreach (var constraint in constraints)
        {
            var key = (constraint.SubmapId, constraint.NodeId);
            if (!constraintGroups.TryGetValue(key, out var group))
            {
                group = [];
                constraintGroups[key] = group;
            }
            group.Add(constraint);
        }
        
        // Detect and handle conflicting constraints
        int conflictingConstraintsCount = 0;
        int groupsWithMultipleConstraints = 0;
        var validConstraints = new List<IPoseGraph.Constraint>();
        const double CONFLICT_THRESHOLD_TRANSLATION = 0.5; // 50cm difference indicates conflict
        const double CONFLICT_THRESHOLD_ROTATION = 0.2; // ~11 degrees difference indicates conflict
        
        foreach (var kvp in constraintGroups)
        {
            var (submapId, nodeId) = kvp.Key;
            var group = kvp.Value;
            
            if (group.Count <= 1)
            {
                // No conflict if only one constraint
                validConstraints.AddRange(group);
                continue;
            }
            
            groupsWithMultipleConstraints++;
            
            // Get parameter blocks to compute expected relative poses
            if (!submapParams.ContainsKey(submapId) || !nodeParams.ContainsKey(nodeId))
            {
                validConstraints.AddRange(group); // Add all if we can't check
                continue;
            }
            
            var submapParam = submapParams[submapId];
            var nodeParam = nodeParams[nodeId];
            
            // Compute current relative pose from parameters
            var currentSubmapPose = OptimizationHelpers.ParametersToRigid2d(submapParam);
            var currentNodePose = OptimizationHelpers.ParametersToRigid2d(nodeParam);
            var currentRelativePose = currentSubmapPose.Inverse() * currentNodePose;
            
            // Check each constraint against other constraints in the group
            var validGroupConstraints = new List<IPoseGraph.Constraint>();
            for (int i = 0; i < group.Count; i++)
            {
                var constraint1 = group[i];
                var constraintPose1 = TransformOperations.Project2D(constraint1.ConstraintPose.ZbarIj);
                
                bool isConflicting = false;
                string conflictReason = "";
                
                // Check against other constraints in group
                for (int j = 0; j < group.Count; j++)
                {
                    if (i == j) continue;
                    
                    var constraint2 = group[j];
                    var constraintPose2 = TransformOperations.Project2D(constraint2.ConstraintPose.ZbarIj);
                    
                    var translationDiff = Vector2.Distance(
                        new Vector2(constraintPose1.Translation.X, constraintPose1.Translation.Y),
                        new Vector2(constraintPose2.Translation.X, constraintPose2.Translation.Y)
                    );
                    var rotationDiff = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(
                        constraintPose1.Rotation - constraintPose2.Rotation
                    ));
                    
                    if (translationDiff > CONFLICT_THRESHOLD_TRANSLATION || rotationDiff > CONFLICT_THRESHOLD_ROTATION)
                    {
                        isConflicting = true;
                        conflictReason = $"conflicts with constraint {j} (translationDiff={translationDiff:F3}m, rotationDiff={rotationDiff:F3}rad)";
                        break;
                    }
                }
                
                if (isConflicting)
                {
                    conflictingConstraintsCount++;
                }
                else
                {
                    validGroupConstraints.Add(constraint1);
                }
            }
            
            // If all constraints are conflicting, keep the one with highest weight (most confident)
            if (validGroupConstraints.Count == 0 && group.Count > 0)
            {
                var bestConstraint = group.OrderByDescending(c => 
                    c.ConstraintPose.TranslationWeight + c.ConstraintPose.RotationWeight
                ).First();
                validGroupConstraints.Add(bestConstraint);
            }
            
            validConstraints.AddRange(validGroupConstraints);
        }
        
        // Add cost functions for constraints (using filtered validConstraints)
        int constraintCount = 0;
        int skippedInvalidConstraints = 0;
        foreach (var constraint in validConstraints)
        {
            try
            {
                // Validate constraint pose before creating cost function
                var constraintPose = constraint.ConstraintPose.ZbarIj;
                var constraintPose2D = TransformOperations.Project2D(constraintPose);
                
                // Check for invalid constraint pose
                if (double.IsNaN(constraintPose2D.Translation.X) || double.IsInfinity(constraintPose2D.Translation.X) ||
                    double.IsNaN(constraintPose2D.Translation.Y) || double.IsInfinity(constraintPose2D.Translation.Y) ||
                    double.IsNaN(constraintPose2D.Rotation) || double.IsInfinity(constraintPose2D.Rotation))
                {
                    skippedInvalidConstraints++;
                    continue;
                }
                
                // Validate weights
                if (double.IsNaN(constraint.ConstraintPose.TranslationWeight) || 
                    double.IsInfinity(constraint.ConstraintPose.TranslationWeight) ||
                    constraint.ConstraintPose.TranslationWeight <= 0 ||
                    double.IsNaN(constraint.ConstraintPose.RotationWeight) ||
                    double.IsInfinity(constraint.ConstraintPose.RotationWeight) ||
                    constraint.ConstraintPose.RotationWeight <= 0)
                {
                    skippedInvalidConstraints++;
                    continue;
                }

                // Validate parameter blocks exist
                if (!submapParams.ContainsKey(constraint.SubmapId))
                {
                    skippedInvalidConstraints++;
                    continue;
                }
                if (!nodeParams.ContainsKey(constraint.NodeId))
                {
                    skippedInvalidConstraints++;
                    continue;
                }
                
                // Validate parameter values
                var submapParam = submapParams[constraint.SubmapId];
                var nodeParam = nodeParams[constraint.NodeId];
                bool hasInvalidParams = false;
                for (int i = 0; i < 3; i++)
                {
                    if (double.IsNaN(submapParam[i]) || double.IsInfinity(submapParam[i]) ||
                        double.IsNaN(nodeParam[i]) || double.IsInfinity(nodeParam[i]))
                    {
                        hasInvalidParams = true;
                        break;
                    }
                }
                if (hasInvalidParams)
                {
                    skippedInvalidConstraints++;
                    continue;
                }
                
                // Method 3: Validate constraint pose magnitude (similar to Odometry constraints)
                // Skip constraints with very large relative poses (likely outliers)
                const double MAX_RELATIVE_TRANSLATION = 100.0; // 100m
                const double MAX_RELATIVE_ROTATION = 10.0; // ~573 degrees
                var translationMagnitude = Math.Sqrt(constraintPose2D.Translation.X * constraintPose2D.Translation.X + 
                                                     constraintPose2D.Translation.Y * constraintPose2D.Translation.Y);
                var rotationMagnitude = Math.Abs(constraintPose2D.Rotation);
                
                // Method 3: Additional validation - check if constraint is consistent with current relative pose
                var currentSubmapPose = OptimizationHelpers.ParametersToRigid2d(submapParam);
                var currentNodePose = OptimizationHelpers.ParametersToRigid2d(nodeParam);
                var currentRelativePose = currentSubmapPose.Inverse() * currentNodePose;
                var constraintRelativePose = constraintPose2D;
                
                var translationDiff = Vector2.Distance(
                    new Vector2(currentRelativePose.Translation.X, currentRelativePose.Translation.Y),
                    new Vector2(constraintRelativePose.Translation.X, constraintRelativePose.Translation.Y)
                );
                var rotationDiff = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(
                    currentRelativePose.Rotation - constraintRelativePose.Rotation
                ));
                
                // Warn if constraint is very different from current relative pose (potential bad constraint)
                const double CONSTRAINT_CONSISTENCY_THRESHOLD_TRANSLATION = 5.0; // 5m difference
                const double CONSTRAINT_CONSISTENCY_THRESHOLD_ROTATION = 1.0; // ~57 degrees difference
                if (translationDiff > CONSTRAINT_CONSISTENCY_THRESHOLD_TRANSLATION || rotationDiff > CONSTRAINT_CONSISTENCY_THRESHOLD_ROTATION)
                {
                }
                
                if (translationMagnitude > MAX_RELATIVE_TRANSLATION || rotationMagnitude > MAX_RELATIVE_ROTATION)
                {
                    skippedInvalidConstraints++;
                    continue;
                }

                var costFunction = SpaCostFunction2D.CreateAutoDiffCostFunction(
                    constraint.ConstraintPose
                );

                // Loop closure constraints should have a loss function (matching C++ implementation)
                LossFunction? lossFunction = null;
                var huberScale = options.HuberScale;
                // Only apply HuberLoss to INTER_SUBMAP constraints (matching C++ line 293-294)
                if (constraint.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap)
                {
                    lossFunction = new HuberLoss(huberScale);
                }

                // Validate parameter blocks are within expected bounds before adding constraint
                const double MAX_EXPECTED_POSE = 10.0;
                const double MAX_EXPECTED_ROTATION = 3.0;
                bool submapParamOutOfBounds = Math.Abs(submapParam[0]) > MAX_EXPECTED_POSE || Math.Abs(submapParam[1]) > MAX_EXPECTED_POSE || Math.Abs(submapParam[2]) > MAX_EXPECTED_ROTATION;
                bool nodeParamOutOfBounds = Math.Abs(nodeParam[0]) > MAX_EXPECTED_POSE || Math.Abs(nodeParam[1]) > MAX_EXPECTED_POSE || Math.Abs(nodeParam[2]) > MAX_EXPECTED_ROTATION;
                
                if (submapParamOutOfBounds || nodeParamOutOfBounds)
                {
                }

                problem.AddResidualBlock(
                    costFunction,
                    lossFunction,
                    [submapParam, nodeParam]
                );
                constraintCount++;
            }
            catch (Exception)
            {
                skippedInvalidConstraints++;
            }
        }

        // Store initial parameter values for post-solve bounds checking
        var submapInitialParams = new Dictionary<SubmapId, double[]>();
        foreach (var kvp in submapParams)
        {
            submapInitialParams[kvp.Key] = [kvp.Value[0], kvp.Value[1], kvp.Value[2]];
        }

        var nodeInitialParams = new Dictionary<NodeId, double[]>();
        foreach (var kvp in nodeParams)
        {
            nodeInitialParams[kvp.Key] = [kvp.Value[0], kvp.Value[1], kvp.Value[2]];
        }

        // Add landmark cost functions
        var landmarkParams = new Dictionary<string, (double[] rotation, double[] translation)>();
        AddLandmarkCostFunctions(landmarkNodes, nodeParams, landmarkParams, problem, sharedQuaternionManifold);

        // Add odometry constraints between consecutive nodes
        AddOdometryConstraints(nodeParams, problem, frozenTrajectories);

        // Add fixed frame pose constraints
        var fixedFrameParams = new Dictionary<int, double[]>();
        AddFixedFramePoseConstraints(nodeParams, fixedFrameParams, problem, frozenTrajectories);

        // Check if we have any residual blocks
        if (problem.NumResidualBlocks == 0)
        {
            return; // Nothing to optimize
        }
        
        
        // Solve with retry logic (Strategy 1 + Strategy 3)
        SolverSummary? summary = null;
        const int maxRetries = 3; // 0, 1, 2, 3 = 4 attempts total
        bool optimizationSucceeded = false;
        
        for (int retry = 0; retry <= maxRetries; retry++)
            {
                if (retry > 0)
                {
                    
                    // Try different solver configuration on retry
                    if (retry == 1)
                    {
                        // Retry 1: Use DenseSchur instead of SparseSchur
                        solverOptions.LinearSolverType = LinearSolverType.DenseSchur;
                        solverOptions.MaxNumIterations = Math.Min(10, solverOptions.MaxNumIterations); // Fewer iterations for retry
                    }
                    else if (retry == 2)
                    {
                        // Retry 2: Use DenseQR and even fewer iterations
                        solverOptions.LinearSolverType = LinearSolverType.DenseQr;
                        solverOptions.MaxNumIterations = Math.Min(5, solverOptions.MaxNumIterations);
                    }
                    else if (retry == 3)
                    {
                        // Reset parameters to initial values (not zero - frozen params must keep their values)
                        foreach (var kvp in submapParams)
                        {
                            if (submapInitialParams.TryGetValue(kvp.Key, out var initial))
                            {
                                kvp.Value[0] = initial[0];
                                kvp.Value[1] = initial[1];
                                kvp.Value[2] = initial[2];
                            }
                        }

                        foreach (var kvp in nodeParams)
                        {
                            if (nodeInitialParams.TryGetValue(kvp.Key, out var initial))
                            {
                                kvp.Value[0] = initial[0];
                                kvp.Value[1] = initial[1];
                                kvp.Value[2] = initial[2];
                            }
                        }

                        // Use DenseQR with minimal iterations
                        solverOptions.LinearSolverType = LinearSolverType.DenseQr;
                        solverOptions.MaxNumIterations = Math.Min(3, solverOptions.MaxNumIterations);
                    }
                }
                
                // Log parameter values BEFORE solving to detect external modifications
                // Log ALL parameter blocks to find which ones are exploding
                
                // Check for exploded poses BEFORE optimization
                int explodedSubmapsBefore = 0;
                int explodedNodesBefore = 0;
                const double MAX_EXPECTED_POSE = 10.0;
                const double MAX_EXPECTED_ROTATION = Math.PI; // Use π to match normalized angle range
                
                foreach (var kvp in submapParams)
                {
                    bool isExploded = Math.Abs(kvp.Value[0]) > MAX_EXPECTED_POSE ||
                                      Math.Abs(kvp.Value[1]) > MAX_EXPECTED_POSE ||
                                      Math.Abs(kvp.Value[2]) > MAX_EXPECTED_ROTATION;
                    if (isExploded)
                        explodedSubmapsBefore++;
                }
                
                foreach (var kvp in nodeParams)
                {
                    bool isExploded = Math.Abs(kvp.Value[0]) > MAX_EXPECTED_POSE ||
                                      Math.Abs(kvp.Value[1]) > MAX_EXPECTED_POSE ||
                                      Math.Abs(kvp.Value[2]) > MAX_EXPECTED_ROTATION;
                    if (isExploded)
                        explodedNodesBefore++;
                }
                
                // Dispose previous retry's SolverSummary before overwriting
                summary?.Dispose();
                summary = problem.Solve(solverOptions);

                // Check parameter values after solving to detect explosion
                
                int explodedSubmapsAfter = 0;
                int explodedNodesAfter = 0;
                int boundsViolationsSubmaps = 0;
                int boundsViolationsNodes = 0;
                
                foreach (var kvp in submapParams)
                {
                    // Compare against initial values to detect deviation from starting pose
                    if (submapInitialParams.TryGetValue(kvp.Key, out var initial))
                    {
                        bool hasBoundsViolation = Math.Abs(kvp.Value[0] - initial[0]) > 10.0 ||
                                                  Math.Abs(kvp.Value[1] - initial[1]) > 10.0 ||
                                                  Math.Abs(kvp.Value[2] - initial[2]) > 3.0;
                        if (hasBoundsViolation) boundsViolationsSubmaps++;
                    }

                    bool isExploded = Math.Abs(kvp.Value[0]) > MAX_EXPECTED_POSE ||
                                      Math.Abs(kvp.Value[1]) > MAX_EXPECTED_POSE ||
                                      Math.Abs(kvp.Value[2]) > MAX_EXPECTED_ROTATION;
                    if (isExploded) explodedSubmapsAfter++;
                }
                
                foreach (var kvp in nodeParams)
                {
                    // Compare against initial values to detect deviation from starting pose
                    if (nodeInitialParams.TryGetValue(kvp.Key, out var initial))
                    {
                        bool hasBoundsViolation = Math.Abs(kvp.Value[0] - initial[0]) > 10.0 ||
                                                  Math.Abs(kvp.Value[1] - initial[1]) > 10.0 ||
                                                  Math.Abs(kvp.Value[2] - initial[2]) > 3.0;
                        if (hasBoundsViolation) boundsViolationsNodes++;
                    }

                    bool isExploded = Math.Abs(kvp.Value[0]) > MAX_EXPECTED_POSE ||
                                      Math.Abs(kvp.Value[1]) > MAX_EXPECTED_POSE ||
                                      Math.Abs(kvp.Value[2]) > MAX_EXPECTED_ROTATION;
                    if (isExploded) explodedNodesAfter++;
                }
                
                if (summary != null && summary.TerminationType != TerminationType.Failure)
                {
                    optimizationSucceeded = true;
                    
                    // Check tolerance thresholds
                    double functionToleranceThreshold = summary.InitialCost * solverOptions.FunctionTolerance;
                    double costChangeRatio = (summary.InitialCost > 0) ? Math.Abs(summary.CostChange) / summary.InitialCost : 0.0;
                    bool functionToleranceReached = costChangeRatio <= solverOptions.FunctionTolerance;
                    
                    break; // Success
                }
            }
        
        if (summary == null)
        {
            return; // Cannot update poses without summary
        }

        try
        {
        if (!versionLogged)
        {
            var fullReport = summary.FullReport;
            // Extract version string from FullReport (format: "Solver Summary (v 2.2.0-...)")
            var versionMatch = System.Text.RegularExpressions.Regex.Match(fullReport, @"Solver Summary \(v ([^)]+)\)");
            if (versionMatch.Success)
            {
                var ceresVersion = versionMatch.Groups[1].Value;
                versionLogged = true;
            }
        }

        // Only update poses if optimization succeeded
        if (!optimizationSucceeded)
        {
            return;
        }

        // Store original parameter values to detect external modifications
        var originalSubmapParams = new Dictionary<SubmapId, double[]>();
        var originalNodeParams = new Dictionary<NodeId, double[]>();
        foreach (var kvp in submapParams)
        {
            originalSubmapParams[kvp.Key] = [kvp.Value[0], kvp.Value[1], kvp.Value[2]];
        }
        foreach (var kvp in nodeParams)
        {
            originalNodeParams[kvp.Key] = [kvp.Value[0], kvp.Value[1], kvp.Value[2]];
        }
        
        // Update poses from optimized parameters with validation and clamping
        const double MAX_POSE_VALUE = 1e6; // Maximum allowed pose value (1 million meters or radians)
        const double MAX_POSE_CHANGE = 1000.0; // Maximum allowed pose change per optimization (1000m or 1000rad)
        
        foreach (var kvp in submapParams)
        {
            var oldPose = _submapData[kvp.Key].GlobalPose;
            var poseParams = kvp.Value;
            
            // Log optimized pose values and check bounds violations
            const double MAX_POSE_BOUND = 10.0;
            const double MAX_ROTATION_BOUND = 3.0;
            double initialX = oldPose.Translation.X;
            double initialY = oldPose.Translation.Y;
            double initialTheta = oldPose.Rotation;
            double lowerX = initialX - MAX_POSE_BOUND;
            double upperX = initialX + MAX_POSE_BOUND;
            double lowerY = initialY - MAX_POSE_BOUND;
            double upperY = initialY + MAX_POSE_BOUND;
            double lowerTheta = initialTheta - MAX_ROTATION_BOUND;
            double upperTheta = initialTheta + MAX_ROTATION_BOUND;
            
            bool xOutOfBounds = poseParams[0] < lowerX || poseParams[0] > upperX;
            bool yOutOfBounds = poseParams[1] < lowerY || poseParams[1] > upperY;
            bool thetaOutOfBounds = poseParams[2] < lowerTheta || poseParams[2] > upperTheta;
            
            // Validate and clamp parameters
            bool paramsValid = true;
            for (int i = 0; i < 3; i++)
            {
                if (double.IsNaN(poseParams[i]) || double.IsInfinity(poseParams[i]))
                {
                    paramsValid = false;
                    break;
                }
                
                // Clamp to reasonable bounds
                if (Math.Abs(poseParams[i]) > MAX_POSE_VALUE)
                {
                    poseParams[i] = Math.Sign(poseParams[i]) * MAX_POSE_VALUE;
                }
            }
            
            if (!paramsValid)
                continue;
            
            // Normalize rotation angle to [-π, π] before converting to Rigid2d
            poseParams[2] = OptimizationHelpers.NormalizeAngleDifference(poseParams[2]);
            
            var newPose = OptimizationHelpers.ParametersToRigid2d(poseParams);
            
            // Check for excessive pose change
            var poseChange = Math.Sqrt(Math.Pow(newPose.Translation.X - oldPose.Translation.X, 2) + 
                                     Math.Pow(newPose.Translation.Y - oldPose.Translation.Y, 2));
            // Normalize angle difference to handle wrap-around (e.g., π and -π should be close, not 2π apart)
            var rotationChange = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(newPose.Rotation - oldPose.Rotation));
            
            if (poseChange > MAX_POSE_CHANGE || rotationChange > MAX_POSE_CHANGE)
            {
                continue;
            }
            
            var spec = _submapData[kvp.Key];
            spec.GlobalPose = newPose;
            _submapData[kvp.Key] = spec;
        }

        foreach (var kvp in nodeParams)
        {
            var poseParams = kvp.Value;
            var oldPose2D = _nodeData[kvp.Key].GlobalPose2D;
            
            // Log optimized pose values and check bounds violations (only first 10 nodes to avoid log spam)
            const double MAX_POSE_BOUND = 10.0;
            const double MAX_ROTATION_BOUND = Math.PI; // Use π to match normalized angle range
            double initialX = oldPose2D.Translation.X;
            double initialY = oldPose2D.Translation.Y;
            double initialTheta = oldPose2D.Rotation;
            double lowerX = initialX - MAX_POSE_BOUND;
            double upperX = initialX + MAX_POSE_BOUND;
            double lowerY = initialY - MAX_POSE_BOUND;
            double upperY = initialY + MAX_POSE_BOUND;
            double lowerTheta = initialTheta - MAX_ROTATION_BOUND;
            double upperTheta = initialTheta + MAX_ROTATION_BOUND;
            
            bool xOutOfBounds = poseParams[0] < lowerX || poseParams[0] > upperX;
            bool yOutOfBounds = poseParams[1] < lowerY || poseParams[1] > upperY;
            bool thetaOutOfBounds = poseParams[2] < lowerTheta || poseParams[2] > upperTheta;
            
            // Normalize rotation angle to [-π, π] before checking bounds and converting to Rigid2d
            poseParams[2] = OptimizationHelpers.NormalizeAngleDifference(poseParams[2]);
            
            // Re-check bounds after normalization
            xOutOfBounds = poseParams[0] < lowerX || poseParams[0] > upperX;
            yOutOfBounds = poseParams[1] < lowerY || poseParams[1] > upperY;
            thetaOutOfBounds = poseParams[2] < lowerTheta || poseParams[2] > upperTheta;
            
            if (xOutOfBounds || yOutOfBounds || thetaOutOfBounds)
            {
            }
            
            // Validate and clamp parameters
            bool paramsValid = true;
            for (int i = 0; i < 3; i++)
            {
                if (double.IsNaN(poseParams[i]) || double.IsInfinity(poseParams[i]))
                {
                    paramsValid = false;
                    break;
                }
                
                // Clamp to reasonable bounds
                if (Math.Abs(poseParams[i]) > MAX_POSE_VALUE)
                {
                    poseParams[i] = Math.Sign(poseParams[i]) * MAX_POSE_VALUE;
                }
            }
            
            if (!paramsValid)
                continue;
            
            var newPose2D = OptimizationHelpers.ParametersToRigid2d(poseParams);
            
            // Check for excessive pose change
            var poseChange = Math.Sqrt(Math.Pow(newPose2D.Translation.X - oldPose2D.Translation.X, 2) + 
                                     Math.Pow(newPose2D.Translation.Y - oldPose2D.Translation.Y, 2));
            // Normalize angle difference to handle wrap-around (e.g., π and -π should be close, not 2π apart)
            var rotationChange = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(newPose2D.Rotation - oldPose2D.Rotation));
            
            if (poseChange > MAX_POSE_CHANGE || rotationChange > MAX_POSE_CHANGE)
            {
                continue;
            }
            
            // Update GlobalPose3D by embedding 2D pose
            var newPose3D = TransformOperations.Embed3D(newPose2D);
            var spec = _nodeData[kvp.Key];
            spec.GlobalPose2D = newPose2D;
            spec.GlobalPose = newPose3D;
            _nodeData[kvp.Key] = spec;
        }

        // Update landmark poses from optimized parameters
        foreach (var kvp in landmarkParams)
        {
            var landmarkRotation = OptimizationHelpers.ParametersToQuaternion(kvp.Value.rotation);
            var landmarkTranslation = OptimizationHelpers.ParametersToVector3(kvp.Value.translation);
            _landmarkData[kvp.Key] = new Rigid3d(landmarkTranslation, landmarkRotation);
        }

        // Update fixed frame poses from optimized parameters
        foreach (var kvp in fixedFrameParams)
        {
            var pose2D = OptimizationHelpers.ParametersToRigid2d(kvp.Value);
            var pose3D = TransformOperations.Embed3D(pose2D);
            if (_trajectoryData.TryGetValue(kvp.Key, out IPoseGraph.TrajectoryData trajectoryData))
            {
                trajectoryData.FixedFrameOriginInMap = pose3D;
                _trajectoryData[kvp.Key] = trajectoryData;
            }
        }

        } // try
        finally
        {
            // SolverSummary holds native resources - must always be disposed
            summary?.Dispose();
        }
    }

    /// <summary>
    /// Adds landmark cost functions to the optimization problem.
    /// </summary>
    private void AddLandmarkCostFunctions(
        Dictionary<string, IPoseGraph.LandmarkNode> landmarkNodes,
        Dictionary<NodeId, double[]> nodeParams,
        Dictionary<string, (double[] rotation, double[] translation)> landmarkParams,
        Problem problem,
        QuaternionManifold sharedQuaternionManifold)
    {
        foreach (var landmarkNode in landmarkNodes)
        {
            var landmarkId = landmarkNode.Key;
            var node = landmarkNode.Value;

            foreach (var observation in node.LandmarkObservations ?? [])
            {
                // Find nodes before and after the observation time
                var trajectoryNodes = _nodeData.Where(n => n.Id.TrajectoryId == observation.TrajectoryId)
                    .OrderBy(n => n.Data.Time)
                    .ToList();

                if (trajectoryNodes.Count == 0)
                    continue;

                // Check if observation time is before first node
                if (observation.Time < trajectoryNodes[0].Data.Time)
                    continue;

                // Find next node
                var nextNode = trajectoryNodes.FirstOrDefault(n => n.Data.Time >= observation.Time);
                if (nextNode.Id.NodeIndex == 0 && nextNode.Data.Time == 0)
                {
                    // Time is after all nodes
                    continue;
                }

                var nextIndex = trajectoryNodes.IndexOf(nextNode);
                if (nextIndex == 0)
                {
                    nextIndex = 1; // Use first two nodes
                    if (nextIndex >= trajectoryNodes.Count)
                        continue;
                    nextNode = trajectoryNodes[nextIndex];
                }

                var prevNode = trajectoryNodes[nextIndex - 1];

                // Get node parameter blocks
                if (!nodeParams.ContainsKey(prevNode.Id) || !nodeParams.ContainsKey(nextNode.Id))
                    continue;

                // Initialize landmark parameters if not already added
                if (!landmarkParams.TryGetValue(landmarkId, out (double[] rotation, double[] translation) value))
                {
                    Rigid3d startingPoint;
                    if (node.GlobalLandmarkPose.HasValue)
                    {
                        startingPoint = node.GlobalLandmarkPose.Value;
                    }
                    else
                    {
                        // Match C++: GetInitialLandmarkPose
                        // Interpolate node poses and multiply by landmark_to_tracking_transform
                        var prevNodePose = nodeParams[prevNode.Id]; // [x, y, theta]
                        var nextNodePose = nodeParams[nextNode.Id]; // [x, y, theta]
                        
                        // Compute interpolation parameter
                        var interpolationParameter = OptimizationHelpers.ComputeInterpolationParameter(
                            observation.Time,
                            prevNode.Data.Time,
                            nextNode.Data.Time
                        );
                        
                        // Interpolate node poses (returns Quaternion rotation and Vector3 translation)
                        var (interpolatedRotation, interpolatedTranslation) = CostHelpers.InterpolateNodes2D(
                            prevNodePose,
                            prevNode.Data.GravityAlignment,
                            nextNodePose,
                            nextNode.Data.GravityAlignment,
                            interpolationParameter
                        );
                        
                        // Create interpolated pose from rotation and translation
                        var interpolatedPose = new Rigid3d(interpolatedTranslation, interpolatedRotation);
                        
                        // Match C++: starting_point = interpolated_pose * observation.landmark_to_tracking_transform
                        startingPoint = interpolatedPose * observation.LandmarkToTrackingTransform;
                    }

                    var (landmarkRotation, landmarkTranslation) = OptimizationHelpers.Rigid3dToParameters(startingPoint);
                    value = (landmarkRotation, landmarkTranslation);
                    landmarkParams[landmarkId] = value;

                    // Add parameter blocks
                    problem.AddParameterBlock(landmarkRotation, 4);
                    problem.AddParameterBlock(landmarkTranslation, 3);

                    // Set quaternion manifold (shared instance disposed after Problem)
                    problem.SetManifold(landmarkRotation, sharedQuaternionManifold);

                    // Set constant if frozen
                    if (node.Frozen)
                    {
                        problem.SetParameterBlockConstant(landmarkRotation);
                        problem.SetParameterBlockConstant(landmarkTranslation);
                    }
                }

                // Add cost function
                var costFunction = LandmarkCostFunction2D.CreateAutoDiffCostFunction(
                    observation,
                    prevNode.Data,
                    nextNode.Data
                );

                var huberScale = options.HuberScale;
                var lossFunction = new HuberLoss(huberScale);

                problem.AddResidualBlock(
                    costFunction,
                    lossFunction,
                    [
                        nodeParams[prevNode.Id],
                        nodeParams[nextNode.Id],
                        value.rotation,
                        value.translation
                    ]
                );
            }
        }
    }

    /// <summary>
    /// Solution 3: Improves initial poses using odometry data.
    /// This helps optimization start from better initial guess, reducing pose explosion.
    /// </summary>
    private void ImproveInitialPosesWithOdometry()
    {
        int improvedNodes = 0;
        int checkedNodes = 0;
        int skippedNoOdometry = 0;
        int skippedOutOfRange = 0;
        const double MAX_ODOMETRY_CORRECTION = 1.0; // Maximum 1m correction from odometry
        
        
        foreach (var trajectoryId in _nodeData.TrajectoryIds)
        {
            if (!_odometryData.TryGetValue(trajectoryId, out List<Sensor.OdometryData>? value) || value.Count == 0)
            {
                skippedNoOdometry++;
                continue;
            }
            
            var trajectoryNodes = _nodeData.BeginOfTrajectory(trajectoryId)
                .OrderBy(n => n.Id.NodeIndex)
                .ToList();
            
            if (trajectoryNodes.Count < 2)
                continue;
            
            // Use odometry to improve initial poses for consecutive nodes
            for (int i = 1; i < trajectoryNodes.Count; i++)
            {
                var prevNode = trajectoryNodes[i - 1];
                var currNode = trajectoryNodes[i];
                
                // Calculate relative odometry between nodes
                var relativeOdometry = CalculateOdometryBetweenNodes(
                    trajectoryId,
                    prevNode.Data,
                    currNode.Data
                );
                
                if (relativeOdometry == null)
                    continue;
                
                // Project to 2D
                var relativeOdometry2D = TransformOperations.Project2D(relativeOdometry.Value);
                
                // Compute expected current node pose based on previous node pose and odometry
                var prevPose2D = prevNode.Data.GlobalPose2D;
                var expectedPose2D = prevPose2D * relativeOdometry2D;
                
                // Get current node pose
                var currentPose2D = currNode.Data.GlobalPose2D;
                
                // Calculate difference
                var translationDiff = Vector2.Distance(
                    new Vector2(expectedPose2D.Translation.X, expectedPose2D.Translation.Y),
                    new Vector2(currentPose2D.Translation.X, currentPose2D.Translation.Y)
                );
                var rotationDiff = Math.Abs(OptimizationHelpers.NormalizeAngleDifference(
                    expectedPose2D.Rotation - currentPose2D.Rotation
                ));
                
                checkedNodes++;
                
                // If difference is significant but reasonable, use odometry-based pose
                if (translationDiff > 0.1 && translationDiff < MAX_ODOMETRY_CORRECTION && rotationDiff < 0.5)
                {
                    // Blend: use 70% odometry-based pose, 30% current pose
                    // This provides better initial guess while maintaining some stability
                    var blendedTranslation = new Vector2(
                        (0.7 * expectedPose2D.Translation.X + 0.3 * currentPose2D.Translation.X),
                        (0.7 * expectedPose2D.Translation.Y + 0.3 * currentPose2D.Translation.Y)
                    );
                    var blendedRotation = OptimizationHelpers.NormalizeAngleDifference(
                        0.7 * expectedPose2D.Rotation + 0.3 * currentPose2D.Rotation
                    );
                    
                    var improvedPose2D = new Rigid2d(blendedTranslation, blendedRotation);
                    var improvedPose3D = TransformOperations.Embed3D(improvedPose2D);
                    
                    var spec = _nodeData[currNode.Id];
                    spec.GlobalPose2D = improvedPose2D;
                    spec.GlobalPose = improvedPose3D;
                    _nodeData[currNode.Id] = spec;
                    
                    improvedNodes++;
                    
                    if (improvedNodes <= 10) // Log first 10 improvements
                    {
                    }
                }
                else
                {
                    skippedOutOfRange++;
                    if (checkedNodes <= 5 || translationDiff >= MAX_ODOMETRY_CORRECTION || rotationDiff >= 0.5)
                    {
                    }
                }
            }
        }
        
    }

    /// <summary>
    /// Adds odometry constraints between consecutive nodes.
    /// </summary>
    private void AddOdometryConstraints(
        Dictionary<NodeId, double[]> nodeParams,
        Problem problem,
        HashSet<int> frozenTrajectories)
    {
        // Constants for validating relative poses before adding constraints
        const double MAX_RELATIVE_TRANSLATION = 100.0; // 100m
        const double MAX_RELATIVE_ROTATION = 10.0; // ~573 degrees
        foreach (var trajectoryId in _nodeData.TrajectoryIds)
        {
            if (frozenTrajectories.Contains(trajectoryId))
                continue;

            var trajectoryNodes = _nodeData.BeginOfTrajectory(trajectoryId)
                .OrderBy(n => n.Id.NodeIndex)
                .Select(n => new { n.Id, n.Data })
                .ToList();

            for (int i = 1; i < trajectoryNodes.Count; i++)
            {
                var prevNode = trajectoryNodes[i - 1];
                var currNode = trajectoryNodes[i];

                // Only add constraint for consecutive nodes
                if (currNode.Id.NodeIndex != prevNode.Id.NodeIndex + 1)
                    continue;

                // Try to get relative odometry
                var relativeOdometry = CalculateOdometryBetweenNodes(
                    trajectoryId,
                    prevNode.Data,
                    currNode.Data
                );

                if (relativeOdometry != null)
                {
                    // Validate relative odometry pose before adding constraint
                    var relativeOdometry2D = TransformOperations.Project2D(relativeOdometry.Value);
                    var odomTranslationMagnitude = Math.Sqrt(relativeOdometry2D.Translation.X * relativeOdometry2D.Translation.X + 
                                                              relativeOdometry2D.Translation.Y * relativeOdometry2D.Translation.Y);
                    var odomRotationMagnitude = Math.Abs(relativeOdometry2D.Rotation);
                    
                    // Skip constraints with very large relative poses (likely outliers)
                    if (odomTranslationMagnitude > MAX_RELATIVE_TRANSLATION || odomRotationMagnitude > MAX_RELATIVE_ROTATION)
                    {
                        continue;
                    }
                    
                    // Add odometry constraint
                    // Match C++: nullptr loss function for odometry constraints
                    var constraintPose = new IPoseGraph.Constraint.Pose(
                        relativeOdometry.Value,
                        options.OdometryTranslationWeight,
                        options.OdometryRotationWeight
                    );
                    var costFunction = SpaCostFunction2D.CreateAutoDiffCostFunction(constraintPose);
                    problem.AddResidualBlock(
                        costFunction,
                        null, // Match C++: nullptr loss function
                        [nodeParams[prevNode.Id], nodeParams[currNode.Id]]
                    );
                }

                // Always add local SLAM pose constraint
                var relativeLocalSlamPose = TransformOperations.Embed3D(
                    prevNode.Data.LocalPose2D.Inverse() * currNode.Data.LocalPose2D
                );
                var relativeLocalSlamPose2D = TransformOperations.Project2D(relativeLocalSlamPose);
                var localSlamTranslationMagnitude = Math.Sqrt(relativeLocalSlamPose2D.Translation.X * relativeLocalSlamPose2D.Translation.X + 
                                                               relativeLocalSlamPose2D.Translation.Y * relativeLocalSlamPose2D.Translation.Y);
                var localSlamRotationMagnitude = Math.Abs(relativeLocalSlamPose2D.Rotation);
                
                // Skip constraints with very large relative poses (likely outliers)
                if (localSlamTranslationMagnitude > MAX_RELATIVE_TRANSLATION || localSlamRotationMagnitude > MAX_RELATIVE_ROTATION)
                    {
                        continue;
                    }
                
                var localSlamConstraintPose = new IPoseGraph.Constraint.Pose(
                    relativeLocalSlamPose,
                    options.LocalSlamPoseTranslationWeight,
                    options.LocalSlamPoseRotationWeight
                );
                var localSlamCostFunction = SpaCostFunction2D.CreateAutoDiffCostFunction(localSlamConstraintPose);
                // Match C++: nullptr loss function for local SLAM pose constraints
                problem.AddResidualBlock(
                    localSlamCostFunction,
                    null, // Match C++: nullptr loss function
                    [nodeParams[prevNode.Id], nodeParams[currNode.Id]]
                );
            }
        }
    }

    /// <summary>
    /// Adds fixed frame pose constraints.
    /// </summary>
    private void AddFixedFramePoseConstraints(
        Dictionary<NodeId, double[]> nodeParams,
        Dictionary<int, double[]> fixedFrameParams,
        Problem problem,
        HashSet<int> frozenTrajectories)
    {
        foreach (var trajectoryId in _nodeData.TrajectoryIds)
        {
            if (frozenTrajectories.Contains(trajectoryId))
                continue;

            if (!_fixedFramePoseData.ContainsKey(trajectoryId))
                continue;

            if (!_trajectoryData.ContainsKey(trajectoryId))
                continue;

            var trajectoryData = _trajectoryData[trajectoryId];
            var trajectoryNodes = _nodeData.BeginOfTrajectory(trajectoryId)
                .OrderBy(n => n.Data.Time)
                .ToList();

            bool fixedFramePoseInitialized = false;

            foreach (var node in trajectoryNodes)
            {
                var fixedFramePose = InterpolateFixedFramePose(trajectoryId, node.Data.Time);
                if (fixedFramePose == null)
                    continue;

                var constraintPose = new IPoseGraph.Constraint.Pose(
                    fixedFramePose.Value,
                    options.FixedFramePoseTranslationWeight,
                    options.FixedFramePoseRotationWeight
                );
                
                if (!fixedFramePoseInitialized)
                {
                    Rigid2d fixedFramePoseInMap;
                    if (trajectoryData.FixedFrameOriginInMap.HasValue)
                    {
                        fixedFramePoseInMap = TransformOperations.Project2D(trajectoryData.FixedFrameOriginInMap.Value);
                    }
                    else
                    {
                        // Initialize from node pose
                        var relativePose2D = TransformOperations.Project2D(constraintPose.ZbarIj);
                        fixedFramePoseInMap = node.Data.GlobalPose2D * relativePose2D.Inverse();
                    }

                    var fixedFramePoseParams = new double[3]
                    {
                        fixedFramePoseInMap.Translation.X,
                        fixedFramePoseInMap.Translation.Y,
                        fixedFramePoseInMap.Rotation
                    };

                    fixedFrameParams[trajectoryId] = fixedFramePoseParams;
                    problem.AddParameterBlock(fixedFramePoseParams, 3);
                    fixedFramePoseInitialized = true;
                }

                // Add cost function
                var costFunction = SpaCostFunction2D.CreateAutoDiffCostFunction(constraintPose);
                LossFunction? lossFunction = null;
                if (options.FixedFramePoseUseTolerantLoss)
                {
                    lossFunction = new TolerantLoss(
                        options.FixedFramePoseTolerantLossParamA,
                        options.FixedFramePoseTolerantLossParamB
                    );
                }

                problem.AddResidualBlock(
                    costFunction,
                    lossFunction,
                    [fixedFrameParams[trajectoryId], nodeParams[node.Id]]
                );
            }
        }
    }
}