/*
 * Copyright 2017 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.Sensor;
using CartographerSharp.Transform;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping;

/// <summary>
/// Keep poses for a certain duration to estimate linear and angular velocity.
/// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if
/// available to improve the extrapolation.
/// Match C++: cartographer/mapping/pose_extrapolator.h/cc
/// </summary>
public class PoseExtrapolator(long poseQueueDuration, double imuGravityTimeConstant) : IPoseExtrapolator
{
    private struct TimedPose(long time, Rigid3d pose)
    {
        public long Time { get; set; } = time;
        public Rigid3d Pose { get; set; } = pose;
    }

    // Use List instead of Queue for random access (similar to std::deque)
    private readonly List<TimedPose> _timedPoseQueue = [];
    private Vector3 _linearVelocityFromPoses = Vector3.Zero;
    private Vector3 _angularVelocityFromPoses = Vector3.Zero;

    private readonly List<ImuData> _imuData = [];
    private readonly List<OdometryData> _odometryData = [];
    private Vector3 _linearVelocityFromOdometry = Vector3.Zero;
    private Vector3 _angularVelocityFromOdometry = Vector3.Zero;
    private Vector3 _instantAngularVelocityFromOdometry = Vector3.Zero;

    // Lock for thread-safe access to _imuData and _odometryData
    private readonly Lock _dataLock = new();

    private readonly double _gravityTimeConstant = imuGravityTimeConstant;
    private ImuTracker? _imuTracker;
    private ImuTracker? _odometryImuTracker;
    private ImuTracker? _extrapolationImuTracker;

    // Match C++: cached_extrapolated_pose_ and cached_extrapolated_pose_filter
    private TimedPose? _cachedExtrapolatedPose;
    private TimedPose? _cachedExtrapolatedPoseFilter;

    // Odometry trajectory-based extrapolation: reference odometry pose at last AddPose time
    // and cached rotation from odom frame to global frame
    private OdometryData? _odometryAtLastPose;
    private Quaternion _odomToGlobalRotation = Quaternion.Identity;


    /// <summary>
    /// Match C++: InitializeWithImu factory method
    /// </summary>
    public static PoseExtrapolator InitializeWithImu(
        long poseQueueDuration,
        double imuGravityTimeConstant,
        ImuData imuData)
    {
        var extrapolator = new PoseExtrapolator(poseQueueDuration, imuGravityTimeConstant);
        extrapolator.AddImuData(imuData);

        // Initialize ImuTracker with first IMU data
        extrapolator._imuTracker = new ImuTracker(imuGravityTimeConstant, imuData.Time);
        extrapolator._imuTracker.AddImuLinearAccelerationObservation(imuData.LinearAcceleration);
        extrapolator._imuTracker.AddImuAngularVelocityObservation(imuData.AngularVelocity);
        extrapolator._imuTracker.Advance(imuData.Time);

        // Add initial pose with rotation from IMU tracker
        extrapolator.AddPose(
            imuData.Time,
            Rigid3d.FromRotation(extrapolator._imuTracker.Orientation));

        return extrapolator;
    }

    /// <summary>
    /// Returns diagnostic info about current odom/IMU data status for debugging.
    /// </summary>
    public (int OdomCount, int ImuCount, long LastOdomTime, long LastPoseTime, Vector3 LinVelOdom, Vector3 LinVelPose) GetDiagnostics()
    {
        lock (_dataLock)
        {
            return (
                OdomCount: _odometryData.Count,
                ImuCount: _imuData.Count,
                LastOdomTime: _odometryData.Count > 0 ? _odometryData[^1].Time : 0,
                LastPoseTime: _timedPoseQueue.Count > 0 ? _timedPoseQueue[^1].Time : 0,
                LinVelOdom: _linearVelocityFromOdometry,
                LinVelPose: _linearVelocityFromPoses
            );
        }
    }

    /// <summary>
    /// Returns detailed extrapolation breakdown for diagnostics.
    /// Match C++: velocity × dt extrapolation.
    /// </summary>
    public string GetExtrapolationBreakdown(long time)
    {
        if (_timedPoseQueue.Count == 0) return "NO_POSES";

        var newestTimedPose = _timedPoseQueue[^1];
        var extrapolationDelta = (time - newestTimedPose.Time) / 10_000_000.0;

        lock (_dataLock)
        {
            var angVelOdom = _angularVelocityFromOdometry;
            var basePose = newestTimedPose.Pose.Translation;

            if (_odometryData.Count >= 2 && _odometryAtLastPose.HasValue)
            {
                var odomRef = _odometryAtLastPose.Value;
                var idx = FindOdometryIndexBeforeTime(time);
                var odomWindow = (_odometryData[^1].Time - _odometryData[0].Time) / 10_000_000.0;

                if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
                {
                    var odomAtTime = _odometryData[idx];
                    var displacementOdom = odomAtTime.Pose.Translation - odomRef.Pose.Translation;
                    var displacementGlobal = Vector3.Transform(displacementOdom, _odomToGlobalRotation);
                    var tailDt = (time - odomAtTime.Time) / 10_000_000.0;
                    return $"PATH=odom_trajectory, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
                           $"displ=({displacementGlobal.X:F4},{displacementGlobal.Y:F4}), tailDt={tailDt:F4}s, " +
                           $"odomWindow={odomWindow:F3}s, odomCount={_odometryData.Count}, " +
                           $"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
                }
                else
                {
                    var vel = _linearVelocityFromOdometry;
                    return $"PATH=odom_vel_fallback, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
                           $"vel=({vel.X:F4},{vel.Y:F4}), odomWindow={odomWindow:F3}s, odomCount={_odometryData.Count}, " +
                           $"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
                }
            }
            else
            {
                var vel = _linearVelocityFromPoses;
                return $"PATH=pose_vel, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
                       $"vel=({vel.X:F4},{vel.Y:F4}), odomCount={_odometryData.Count}, " +
                       $"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
            }
        }
    }

    /// <summary>
    /// Returns the time span (ms) covered by the current odometry data window [oldest, newest].
    /// Large values relative to scan period indicate odom accumulated during a processing
    /// delay (e.g., grid resize blocking the SLAM thread in InsertIntoSubmap).
    /// </summary>
    public double GetOdometryWindowMs()
    {
        lock (_dataLock)
        {
            if (_odometryData.Count < 2) return 0;
            return (_odometryData[^1].Time - _odometryData[0].Time) / 10_000.0;
        }
    }

    /// <summary>
    /// Match C++: GetLastPoseTime
    /// </summary>
    public long GetLastPoseTime()
    {
        return _timedPoseQueue.Count > 0 ? _timedPoseQueue[^1].Time : long.MinValue;
    }

    /// <summary>
    /// Match C++: GetLastExtrapolatedTime
    /// </summary>
    public long GetLastExtrapolatedTime()
    {
        return _extrapolationImuTracker?.Time ?? long.MinValue;
    }

    /// <summary>
    /// Match C++: AddPose (pose_extrapolator.cc:70-91)
    /// </summary>
    public void AddPose(long time, Rigid3d pose)
    {
        // Match C++ lines 72-79: Initialize ImuTracker if needed
        if (_imuTracker == null)
        {
            long trackerStart = time;
            lock (_dataLock)
            {
                if (_imuData.Count > 0)
                {
                    trackerStart = Math.Min(trackerStart, _imuData[0].Time);
                }
            }
            _imuTracker = new ImuTracker(_gravityTimeConstant, trackerStart);
        }

        // Match C++ line 80: Add pose to queue
        _timedPoseQueue.Add(new TimedPose(time, pose));

        // Match C++ lines 81-84: Trim queue to keep at least 2 poses
        while (_timedPoseQueue.Count > 2 &&
               _timedPoseQueue[1].Time <= time - poseQueueDuration)
        {
            _timedPoseQueue.RemoveAt(0);
        }

        // Match C++ line 85: Update velocities from poses
        UpdateVelocitiesFromPoses();

        // Match C++ lines 86-88: Advance IMU tracker and trim data
        // FIX: Extended lock scope to include ImuTracker copies to prevent race condition with AddOdometryData
        lock (_dataLock)
        {
            AdvanceImuTracker(time, _imuTracker);
            TrimImuData();
            TrimOdometryData();

            // Save reference odometry pose for trajectory-based extrapolation
            if (_odometryData.Count > 0)
            {
                _odometryAtLastPose = _odometryData[0];
                _odomToGlobalRotation = pose.Rotation * Quaternion.Inverse(_odometryData[0].Pose.Rotation);
            }
            else
            {
                _odometryAtLastPose = null;
            }

            // Match C++ lines 89-90: Create copies of ImuTracker for odometry and extrapolation
            // These must be created inside lock to prevent race condition with AddOdometryData
            _odometryImuTracker = new ImuTracker(_imuTracker);
            _extrapolationImuTracker = new ImuTracker(_imuTracker);
        }

        // Invalidate cache when new pose is added
        _cachedExtrapolatedPose = null;

    }

    /// <summary>
    /// Match C++: AddImuData (pose_extrapolator.cc:93-98)
    /// </summary>
    public void AddImuData(ImuData imuData)
    {
        // Match C++ lines 94-95: CHECK that IMU time >= last pose time
        if (_timedPoseQueue.Count > 0 && imuData.Time < _timedPoseQueue[^1].Time)
        {
            var timeDiffMs = (_timedPoseQueue[^1].Time - imuData.Time) / TimeSpan.TicksPerMillisecond;
            if(timeDiffMs > 5)
            {
                throw new ArgumentException($"IMU data time ({imuData.Time}) must be >= last pose time ({_timedPoseQueue[^1].Time}), diff={timeDiffMs}ms");
            }
            else
            {
                return; // Ignore slightly out-of-order IMU data
            }
        }

        lock (_dataLock)
        {
            _imuData.Add(imuData);
            TrimImuData();
        }
    }

    /// <summary>
    /// Match C++: AddOdometryData (pose_extrapolator.cc:100-144)
    /// CRITICAL FIX: Do NOT reset odometry_imu_tracker_ - let it accumulate state
    /// </summary>
    public void AddOdometryData(OdometryData odometryData)
    {
        // Match C++ lines 105-106: CHECK that odometry time >= last pose time
        if (_timedPoseQueue.Count > 0 && odometryData.Time < _timedPoseQueue[^1].Time)
        {
            var timeDiffMs = (_timedPoseQueue[^1].Time - odometryData.Time) / TimeSpan.TicksPerMillisecond;
            if(timeDiffMs > 5)
            {
                throw new ArgumentException($"Odometry data time ({odometryData.Time}) must be >= last pose time ({_timedPoseQueue[^1].Time}), diff={timeDiffMs}ms");
            }
            else
            {
                return; // Ignore slightly out-of-order odometry data
            }
        }

        // FIX: Use single lock scope to prevent race condition with AddPose modifying _odometryImuTracker
        // Between two separate lock blocks, _odometryImuTracker could be replaced by AddPose from SLAM thread
        lock (_dataLock)
        {
            // Match C++ line 107-108: Add to queue and trim
            _odometryData.Add(odometryData);
            TrimOdometryData();

            // Match C++ lines 109-111: Need at least 2 odometry data points
            if (_odometryData.Count < 2)
            {
                return;
            }

            // Match C++: Use front() and back() of the queue (full window since last AddPose)
            var odometryDataOldest = _odometryData[0];
            var odometryDataNewest = _odometryData[^1];

            // Match C++ lines 116-117: Compute time delta
            var odometryTimeDelta = (odometryDataOldest.Time - odometryDataNewest.Time) / 10_000_000.0;

            // Safeguard: Skip if time delta is too small (< 1ms) to avoid noisy velocity estimates
            if (Math.Abs(odometryTimeDelta) < 0.001)
            {
                return;
            }

            // Match C++ lines 118-119: Compute pose delta
            var odometryPoseDelta = odometryDataNewest.Pose.Inverse() * odometryDataOldest.Pose;

            // Match C++ lines 120-123: Compute angular velocity from odometry
            var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(odometryPoseDelta.Rotation);
            _angularVelocityFromOdometry = new Vector3(
                angleAxis.X / odometryTimeDelta,
                angleAxis.Y / odometryTimeDelta,
                angleAxis.Z / odometryTimeDelta);

            // Instantaneous angular velocity from last 2 samples for tail-gap extrapolation
            if (_odometryData.Count >= 2)
            {
                var prevOdom = _odometryData[^2];
                var currOdom = _odometryData[^1];
                var dtInstant = (currOdom.Time - prevOdom.Time) / 10_000_000.0;
                if (dtInstant > 0.001)
                {
                    var instantDelta = Quaternion.Inverse(prevOdom.Pose.Rotation) * currOdom.Pose.Rotation;
                    var instantAxis = TransformOperations.RotationQuaternionToAngleAxisVector(instantDelta);
                    _instantAngularVelocityFromOdometry = instantAxis / dtInstant;
                }
            }

            // Match C++ lines 124-126: Return if no poses yet
            if (_timedPoseQueue.Count == 0)
            {
                return;
            }

            // Match C++ lines 127-129: Compute linear velocity in tracking frame
            var linearVelocityInTrackingFrameAtNewestOdometryTime = new Vector3(
                odometryPoseDelta.Translation.X / odometryTimeDelta,
                odometryPoseDelta.Translation.Y / odometryTimeDelta,
                odometryPoseDelta.Translation.Z / odometryTimeDelta);

            // Match C++ lines 130-133: Compute orientation at newest odometry time
            // FIX: _odometryImuTracker is now accessed within the same lock scope
            if (_odometryImuTracker != null)
            {
                var newestTimedPose = _timedPoseQueue[^1];
                var orientationAtNewestOdometryTime =
                    newestTimedPose.Pose.Rotation *
                    ExtrapolateRotation(odometryDataNewest.Time, _odometryImuTracker);

                // Match C++ lines 134-136: Transform to global frame
                _linearVelocityFromOdometry = Vector3.Transform(
                    linearVelocityInTrackingFrameAtNewestOdometryTime,
                    orientationAtNewestOdometryTime);
            }
            else
            {
                // Fallback: use pose rotation directly without IMU extrapolation
                var newestTimedPose = _timedPoseQueue[^1];
                _linearVelocityFromOdometry = Vector3.Transform(
                    linearVelocityInTrackingFrameAtNewestOdometryTime,
                    newestTimedPose.Pose.Rotation);
            }
        }
    }

    /// <summary>
    /// Match C++: ExtrapolatePose (pose_extrapolator.cc:195-208)
    /// </summary>
    public Rigid3d ExtrapolatePose(long time)
    {
        if (_timedPoseQueue.Count == 0)
        {
            return Rigid3d.Identity;
        }

        // Match C++ line 196-197
        var newestTimedPose = _timedPoseQueue[^1];
        if (time < newestTimedPose.Time)
        {
            var timeDiffMs = (newestTimedPose.Time - time) / TimeSpan.TicksPerMillisecond;
            throw new ArgumentException($"time ({time}) must be >= newest pose time ({newestTimedPose.Time}), diff={timeDiffMs}ms", nameof(time));
        }

        // Match C++ line 198: if (cached_extrapolated_pose_.time != time)
        if (_cachedExtrapolatedPose.HasValue && _cachedExtrapolatedPose.Value.Time == time)
        {
            return _cachedExtrapolatedPose.Value.Pose;
        }

        // Match C++ lines 199-200: Compute translation
        var translation = ExtrapolateTranslation(time) + newestTimedPose.Pose.Translation;

        // Match C++ lines 201-203: Compute rotation
        Quaternion rotation;
        lock (_dataLock)
        {
            if (_extrapolationImuTracker == null)
            {
                rotation = newestTimedPose.Pose.Rotation;
            }
            else if (_imuData.Count == 0)
            {
                // No IMU: prefer trajectory-based rotation from odometry
                var odomRotation = ExtrapolateRotationFromOdometry(time);
                rotation = odomRotation.HasValue
                    ? newestTimedPose.Pose.Rotation * odomRotation.Value
                    : newestTimedPose.Pose.Rotation * ExtrapolateRotation(time, _extrapolationImuTracker);
            }
            else
            {
                rotation = newestTimedPose.Pose.Rotation *
                           ExtrapolateRotation(time, _extrapolationImuTracker);
            }
        }

        // Match C++ lines 204-205: Cache result
        _cachedExtrapolatedPose = new TimedPose(time, new Rigid3d(translation, rotation));
        return _cachedExtrapolatedPose.Value.Pose;
    }

    /// <summary>
    /// Match C++: ExtrapolatePose_filter (pose_extrapolator.cc:146-193)
    /// Returns filtered pose with low-pass filter to reduce jitter during direction changes.
    /// </summary>
    public Rigid3d ExtrapolatePoseFilter(long time)
    {
        if (_timedPoseQueue.Count == 0)
        {
            return Rigid3d.Identity;
        }

        var newestTimedPose = _timedPoseQueue[^1];
        if (time < newestTimedPose.Time)
        {
            var timeDiffMs = (newestTimedPose.Time - time) / TimeSpan.TicksPerMillisecond;
            throw new ArgumentException($"time ({time}) must be >= newest pose time ({newestTimedPose.Time}), diff={timeDiffMs}ms", nameof(time));
        }

        // Match C++ line 149: if (cached_extrapolated_pose_.time != time)
        if (!_cachedExtrapolatedPose.HasValue || _cachedExtrapolatedPose.Value.Time != time)
        {
            // Match C++ lines 150-156: Compute translation and rotation, update cached_extrapolated_pose_
            var translation = ExtrapolateTranslation(time) + newestTimedPose.Pose.Translation;

            Quaternion rotation;
            lock (_dataLock)
            {
                if (_extrapolationImuTracker == null || _imuTracker == null)
                {
                    rotation = newestTimedPose.Pose.Rotation;
                }
                else if (_imuData.Count == 0)
                {
                    // No IMU: prefer trajectory-based rotation from odometry
                    var odomRotation = ExtrapolateRotationFromOdometry(time);
                    rotation = odomRotation.HasValue
                        ? newestTimedPose.Pose.Rotation * odomRotation.Value
                        : newestTimedPose.Pose.Rotation * ExtrapolateRotation(time, _extrapolationImuTracker);
                }
                else
                {
                    rotation = newestTimedPose.Pose.Rotation *
                               ExtrapolateRotation(time, _extrapolationImuTracker);
                }
            }

            _cachedExtrapolatedPose = new TimedPose(time, new Rigid3d(translation, rotation));

            // Match C++ lines 159-190: Update cached_extrapolated_pose_filter
            var extrapolationDeltaFilterTime = _cachedExtrapolatedPoseFilter.HasValue
                ? (time - _cachedExtrapolatedPoseFilter.Value.Time) / 10_000_000.0
                : double.PositiveInfinity;

            Vector3 translationFilter;

            if (extrapolationDeltaFilterTime < 0.1 && _cachedExtrapolatedPoseFilter.HasValue)
            {
                // Match C++ lines 163-171: Apply low-pass filter
                Vector3 linearVelocity;
                lock (_dataLock)
                {
                    linearVelocity = _odometryData.Count < 2
                        ? _linearVelocityFromPoses
                        : _linearVelocityFromOdometry;
                }

                translationFilter = _cachedExtrapolatedPoseFilter.Value.Pose.Translation +
                    new Vector3(
                        extrapolationDeltaFilterTime * linearVelocity.X,
                        extrapolationDeltaFilterTime * linearVelocity.Y,
                        extrapolationDeltaFilterTime * linearVelocity.Z);

                // Match C++ lines 173-180: Check delta and blend
                var deltaTrans = translationFilter - _cachedExtrapolatedPose.Value.Pose.Translation;

                if (deltaTrans.Length() < 0.03)
                {
                    translationFilter = 0.7 * translationFilter + 0.3 * _cachedExtrapolatedPose.Value.Pose.Translation;
                }
                else
                {
                    translationFilter = _cachedExtrapolatedPose.Value.Pose.Translation;
                }
            }
            else
            {
                // Match C++ lines 187-189: No filter, use extrapolated pose directly
                translationFilter = _cachedExtrapolatedPose.Value.Pose.Translation;
            }

            // Match C++ lines 182-183, 188-189: Cache filtered pose
            _cachedExtrapolatedPoseFilter = new TimedPose(time,
                new Rigid3d(translationFilter, _cachedExtrapolatedPose.Value.Pose.Rotation));
        }

        // Match C++ line 192: return cached_extrapolated_pose_filter.pose
        return _cachedExtrapolatedPoseFilter!.Value.Pose;
    }

    /// <summary>
    /// Match C++: ExtrapolatePosesWithGravity (pose_extrapolator.cc:306-320)
    /// </summary>
    public ExtrapolationResult ExtrapolatePosesWithGravity(List<long> times)
    {
        var previousPoses = new List<Rigid3f>();

        for (int i = 0; i < times.Count - 1; i++)
        {
            var pose = ExtrapolatePose(times[i]);
            previousPoses.Add(new Rigid3f(
                new Vector3(pose.Translation.X, pose.Translation.Y, pose.Translation.Z),
                pose.Rotation));
        }

        var currentPose = ExtrapolatePose(times[^1]);

        Vector3 currentVelocity;
        lock (_dataLock)
        {
            currentVelocity = _odometryData.Count < 2
                ? _linearVelocityFromPoses
                : _linearVelocityFromOdometry;
        }

        return new ExtrapolationResult
        {
            PreviousPoses = previousPoses,
            CurrentPose = currentPose,
            CurrentVelocity = currentVelocity,
            GravityFromTracking = EstimateGravityOrientation(times[^1])
        };
    }

    /// <summary>
    /// Match C++: EstimateGravityOrientation (pose_extrapolator.cc:210-215)
    /// </summary>
    public Quaternion EstimateGravityOrientation(long time)
    {
        // Match C++ line 212: ImuTracker imu_tracker = *imu_tracker_;
        // C++ assumes imu_tracker_ is initialized (via AddPose or InitializeWithImu)
        if (_imuTracker == null)
        {
            throw new InvalidOperationException("ImuTracker not initialized. Call AddPose or InitializeWithImu first.");
        }

        var imuTracker = new ImuTracker(_imuTracker);

        // Match C++ line 213: AdvanceImuTracker(time, &imu_tracker);
        lock (_dataLock)
        {
            AdvanceImuTracker(time, imuTracker);
        }

        // Match C++ line 214: return imu_tracker.orientation();
        return imuTracker.Orientation;
    }

    /// <summary>
    /// Match C++: TrimImuData (pose_extrapolator.cc:243-248)
    /// </summary>
    private void TrimImuData()
    {
        while (_imuData.Count > 1 && _timedPoseQueue.Count > 0 &&
               _imuData[1].Time <= _timedPoseQueue[^1].Time)
        {
            _imuData.RemoveAt(0);
        }
    }

    /// <summary>
    /// Match C++: TrimOdometryData (pose_extrapolator.cc:250-255)
    /// </summary>
    private void TrimOdometryData()
    {
        while (_odometryData.Count > 2 && _timedPoseQueue.Count > 0 &&
               _odometryData[1].Time <= _timedPoseQueue[^1].Time)
        {
            _odometryData.RemoveAt(0);
        }
    }

    /// <summary>
    /// Match C++: UpdateVelocitiesFromPoses (pose_extrapolator.cc:217-241)
    /// </summary>
    private void UpdateVelocitiesFromPoses()
    {
        if (_timedPoseQueue.Count < 2)
        {
            return;
        }

        var newestTimedPose = _timedPoseQueue[^1];
        var newestTime = newestTimedPose.Time;
        var oldestTimedPose = _timedPoseQueue[0];
        var oldestTime = oldestTimedPose.Time;

        var queueDelta = (newestTime - oldestTime) / 10_000_000.0;
        var requiredQueueDelta = poseQueueDuration / 10_000_000.0;

        if (queueDelta < requiredQueueDelta)
        {
            return;
        }

        var newestPose = newestTimedPose.Pose;
        var oldestPose = oldestTimedPose.Pose;
        var translationDelta = newestPose.Translation - oldestPose.Translation;

        _linearVelocityFromPoses = new Vector3(
            translationDelta.X / queueDelta,
            translationDelta.Y / queueDelta,
            translationDelta.Z / queueDelta);

        var rotationDelta = Quaternion.Inverse(oldestPose.Rotation) * newestPose.Rotation;
        var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(rotationDelta);

        _angularVelocityFromPoses = new Vector3(
            angleAxis.X / queueDelta,
            angleAxis.Y / queueDelta,
            angleAxis.Z / queueDelta);
    }

    /// <summary>
    /// Match C++: AdvanceImuTracker (pose_extrapolator.cc:257-286)
    /// </summary>
    private void AdvanceImuTracker(long time, ImuTracker imuTracker)
    {
        if (time < imuTracker.Time)
        {
            var timeDiffMs = (imuTracker.Time - time) / TimeSpan.TicksPerMillisecond;
            throw new ArgumentException($"time ({time}) must be >= imuTracker time ({imuTracker.Time}), diff={timeDiffMs}ms", nameof(time));
        }

        if (_imuData.Count == 0 || time < _imuData[0].Time)
        {
            imuTracker.Advance(time);
            imuTracker.AddImuLinearAccelerationObservation(Vector3.UnitZ);
            var angularVel = _odometryData.Count < 2
                ? _angularVelocityFromPoses
                : _angularVelocityFromOdometry;
            imuTracker.AddImuAngularVelocityObservation(angularVel);
            return;
        }

        if (imuTracker.Time < _imuData[0].Time)
        {
            imuTracker.Advance(_imuData[0].Time);
        }

        int startIndex = 0;
        for (int i = 0; i < _imuData.Count; i++)
        {
            if (_imuData[i].Time >= imuTracker.Time)
            {
                startIndex = i;
                break;
            }
        }

        for (int i = startIndex; i < _imuData.Count && _imuData[i].Time < time; i++)
        {
            if (_imuData[i].Time >= imuTracker.Time)
            {
                imuTracker.Advance(_imuData[i].Time);
                imuTracker.AddImuLinearAccelerationObservation(_imuData[i].LinearAcceleration);
                imuTracker.AddImuAngularVelocityObservation(_imuData[i].AngularVelocity);
            }
        }

        if (time >= imuTracker.Time)
        {
            imuTracker.Advance(time);
        }
    }

    /// <summary>
    /// Match C++: ExtrapolateRotation (pose_extrapolator.cc:288-294)
    /// </summary>
    private Quaternion ExtrapolateRotation(long time, ImuTracker imuTracker)
    {
        // Match C++ line 290: CHECK_GE(time, imu_tracker->time());
        if (time < imuTracker.Time)
        {
            var timeDiffMs = (imuTracker.Time - time) / TimeSpan.TicksPerMillisecond;
            throw new ArgumentException($"time ({time}) must be >= imuTracker time ({imuTracker.Time}), diff={timeDiffMs}ms", nameof(time));
        }

        // Match C++ line 291: AdvanceImuTracker(time, imu_tracker);
        AdvanceImuTracker(time, imuTracker);

        // Match C++ lines 292-293: return last_orientation.inverse() * imu_tracker->orientation();
        var lastOrientation = _imuTracker!.Orientation;
        return Quaternion.Inverse(lastOrientation) * imuTracker.Orientation;
    }

    /// <summary>
    /// Find the index of the latest odometry entry with Time &lt;= requested time.
    /// Returns -1 if no suitable entry found.
    /// Must be called under _dataLock.
    /// </summary>
    private int FindOdometryIndexBeforeTime(long time)
    {
        for (int i = _odometryData.Count - 1; i >= 0; i--)
        {
            if (_odometryData[i].Time <= time)
                return i;
        }
        return -1;
    }

    /// <summary>
    /// Extrapolate rotation using the actual odometry trajectory, mirroring ExtrapolateTranslation.
    /// Returns the relative rotation delta (compatible with ExtrapolateRotation return value).
    /// Returns null when no odometry data is available, signaling fallback to ImuTracker path.
    /// Must be called under _dataLock.
    /// </summary>
    private Quaternion? ExtrapolateRotationFromOdometry(long time)
    {
        if (_odometryData.Count < 2 || !_odometryAtLastPose.HasValue)
        {
            return null;
        }

        var odomRef = _odometryAtLastPose.Value;

        // Find latest odometry entry at or before requested time
        var idx = FindOdometryIndexBeforeTime(time);

        Quaternion deltaRotation;
        long lastCoveredTime;

        if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
        {
            // Exact rotation delta from odometry trajectory
            var odomAtTime = _odometryData[idx];
            deltaRotation = Quaternion.Inverse(odomRef.Pose.Rotation) * odomAtTime.Pose.Rotation;
            lastCoveredTime = odomAtTime.Time;
        }
        else
        {
            deltaRotation = Quaternion.Identity;
            lastCoveredTime = _timedPoseQueue[^1].Time;
        }

        // Instantaneous angular velocity extrapolation for the small gap after last odometry sample
        if (time > lastCoveredTime)
        {
            var dtRemaining = (time - lastCoveredTime) / 10_000_000.0;
            var tailAngleAxis = _instantAngularVelocityFromOdometry * dtRemaining;
            var tailRotation = TransformOperations.AngleAxisVectorToRotationQuaternion(tailAngleAxis);
            deltaRotation = Quaternion.Normalize(deltaRotation * tailRotation);
        }

        return deltaRotation;
    }

    /// <summary>
    /// Extrapolate translation using the actual odometry trajectory for exact displacement,
    /// with constant-velocity extrapolation only for the small gap after the last odometry sample.
    /// Falls back to pose-based constant velocity when no odometry data is available.
    /// </summary>
    private Vector3 ExtrapolateTranslation(long time)
    {
        var newestTimedPose = _timedPoseQueue[^1];

        lock (_dataLock)
        {
            if (_odometryData.Count < 2 || !_odometryAtLastPose.HasValue)
            {
                // No odometry: fall back to constant-velocity from poses
                var dtFallback = (time - newestTimedPose.Time) / 10_000_000.0;
                return new Vector3(
                    dtFallback * _linearVelocityFromPoses.X,
                    dtFallback * _linearVelocityFromPoses.Y,
                    dtFallback * _linearVelocityFromPoses.Z);
            }

            var odomRef = _odometryAtLastPose.Value;

            // Find latest odometry entry at or before requested time
            var idx = FindOdometryIndexBeforeTime(time);

            Vector3 displacementGlobal;
            long lastCoveredTime;

            if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
            {
                // Compute exact displacement from odometry trajectory (in odom frame)
                var odomAtTime = _odometryData[idx];
                var displacementOdom = odomAtTime.Pose.Translation - odomRef.Pose.Translation;

                // Transform odom-frame displacement to global frame
                displacementGlobal = Vector3.Transform(displacementOdom, _odomToGlobalRotation);
                lastCoveredTime = odomAtTime.Time;
            }
            else
            {
                // No odometry data between reference and requested time
                displacementGlobal = Vector3.Zero;
                lastCoveredTime = newestTimedPose.Time;
            }

            // Constant-velocity extrapolation for the small gap after last odometry sample
            if (time > lastCoveredTime)
            {
                var dtRemaining = (time - lastCoveredTime) / 10_000_000.0;
                displacementGlobal += new Vector3(
                    dtRemaining * _linearVelocityFromOdometry.X,
                    dtRemaining * _linearVelocityFromOdometry.Y,
                    dtRemaining * _linearVelocityFromOdometry.Z);
            }

            return displacementGlobal;
        }
    }
}