using System;
using System.Diagnostics;
using System.Globalization;
using ClumsyCore.Interfaces;
using MyParking.Shared;

namespace MultiWheelC.StateEstimation
{
    /// <summary>
    /// 读取Detour位姿，并将可能发生重定位的原始世界坐标转换为当前任务使用的连续坐标。
    /// </summary>
    public sealed class DetourVehicleStateProvider
        : IVehicleStateProvider
    {
        private readonly struct DetourObservation
        {
            public DetourObservation(
                Pose2D poseInDetour,
                long tickRaw,
                double localizationStep)
            {
                PoseInDetour = poseInDetour;
                TickRaw = tickRaw;
                LocalizationStep = localizationStep;
            }

            public Pose2D PoseInDetour { get; }
            public long TickRaw { get; }
            public double LocalizationStep { get; }
        }

        public const double DefaultMaximumLinearSpeedMetersPerSecond =
            1.20;
        public const double DefaultMaximumAngularSpeedRadiansPerSecond =
            Math.PI / 4.0;
        public const double DefaultPositionJumpMarginMeters =
            0.03;
        public const double DefaultHeadingJumpMarginRadians =
            5.0 * Math.PI / 180.0;
        public const double DefaultVelocityPositionResidualMeters =
            0.04;
        public const double DefaultVelocityHeadingResidualRadians =
            5.0 * Math.PI / 180.0;
        public const double DefaultStationaryConfirmationSeconds =
            0.35;
        public const int DefaultHeadingOutlierConfirmationFrameCount =
            3;
        public const double
            DefaultHeadingOutlierPredictionTimeoutSeconds = 0.30;
        public const int DefaultJumpConfirmationFrameCount = 3;
        public const double DefaultJumpConfirmationTimeoutSeconds =
            0.60;
        public const double DefaultMaximumAutomaticFrameShiftMeters =
            0.15;
        public const double
            DefaultMaximumAutomaticHeadingShiftRadians =
                5.0 * Math.PI / 180.0;

        private const double MillimetersPerMeter = 1000.0;
        private const double PositionEqualityToleranceMeters = 1e-9;
        private const double HeadingEqualityToleranceRadians = 1e-8;
        private const double
            InPlaceRotationLinearSpeedThresholdMetersPerSecond = 0.03;
        private const double
            InPlaceRotationAngularSpeedThresholdRadiansPerSecond =
                Math.PI / 180.0;

        private readonly object _syncRoot = new object();
        private readonly Stopwatch _clock = Stopwatch.StartNew();
        private readonly VelocityEstimator2D _velocityEstimator;
        private readonly double _maximumLinearSpeedMetersPerSecond;
        private readonly double _maximumAngularSpeedRadiansPerSecond;
        private readonly double _positionJumpMarginMeters;
        private readonly double _headingJumpMarginRadians;
        private readonly double _velocityPositionResidualMeters;
        private readonly double _velocityHeadingResidualRadians;
        private readonly double _stationaryConfirmationSeconds;
        private readonly int _headingOutlierConfirmationFrameCount;
        private readonly double
            _headingOutlierPredictionTimeoutSeconds;
        private readonly int _jumpConfirmationFrameCount;
        private readonly double _jumpConfirmationTimeoutSeconds;
        private readonly double _maximumAutomaticFrameShiftMeters;
        private readonly double
            _maximumAutomaticHeadingShiftRadians;

        private Pose2D _acceptedPoseInControl;
        private double _acceptedTimestampSeconds;
        private VehicleState _latestState;
        private bool _stationaryHoldActive;

        private bool _hasObservedDetourFrame;
        private Pose2D _lastObservedPoseInDetour;
        private long _lastObservedTickRaw;
        private bool _hasDetourSourceFrameInterval;
        private double _lastDetourSourceFrameIntervalSeconds;
        private Pose2D _controlFromDetour = Pose2D.Identity;

        private bool _hasMotionPrediction;
        private Pose2D _predictedPoseInControl;
        private double _predictionTimestampSeconds;
        private Twist2D _latestWheelTwistInBody = Twist2D.Zero;
        private bool _latestWheelVelocityValid;

        private bool _jumpCandidateActive;
        private Pose2D _candidateControlFromDetour;
        private int _candidateConsistentFrameCount;
        private double _candidateStartedTimestampSeconds;
        private double _candidateShiftDistanceMeters;
        private double _candidateShiftHeadingRadians;
        private bool _candidateAutomaticShiftForbidden;
        private bool _candidateEligibleForAutomaticShift;
        private bool _postRotationPositionRecoveryActive;
        private string _lastJumpCandidateTriggerReason = string.Empty;

        private bool _hasInnovationDiagnostics;
        private double _lastPositionInnovationMeters;
        private double _lastAllowedPositionInnovationMeters;
        private double _lastHeadingInnovationRadians;
        private double _lastAllowedHeadingInnovationRadians;

        private bool _hasReliableHeadingObservation;
        private double _latestReliableHeadingInDetourRadians;
        private bool _headingOutlierCandidateActive;
        private int _headingOutlierConsecutiveFrameCount;
        private double _headingOutlierStartedTimestampSeconds;
        private string _headingOutlierCandidateReason = string.Empty;

        /// <summary>
        /// 创建使用停车机器人默认边界和坐标连续化参数的Detour状态源。
        /// </summary>
        public DetourVehicleStateProvider()
            : this(
                new VelocityEstimator2D(),
                DefaultMaximumLinearSpeedMetersPerSecond,
                DefaultMaximumAngularSpeedRadiansPerSecond,
                DefaultPositionJumpMarginMeters,
                DefaultHeadingJumpMarginRadians,
                DefaultVelocityPositionResidualMeters,
                DefaultVelocityHeadingResidualRadians,
                DefaultStationaryConfirmationSeconds,
                DefaultHeadingOutlierConfirmationFrameCount,
                DefaultHeadingOutlierPredictionTimeoutSeconds,
                DefaultJumpConfirmationFrameCount,
                DefaultJumpConfirmationTimeoutSeconds,
                DefaultMaximumAutomaticFrameShiftMeters,
                DefaultMaximumAutomaticHeadingShiftRadians)
        {
        }

        /// <summary>
        /// 创建使用指定物理边界和既有跳变检测参数的Detour状态源，
        /// 坐标连续化参数使用停车机器人默认值。
        /// </summary>
        public DetourVehicleStateProvider(
            VelocityEstimator2D velocityEstimator,
            double maximumLinearSpeedMetersPerSecond,
            double maximumAngularSpeedRadiansPerSecond,
            double positionJumpMarginMeters,
            double headingJumpMarginRadians,
            double velocityPositionResidualMeters,
            double velocityHeadingResidualRadians,
            double stationaryConfirmationSeconds)
            : this(
                velocityEstimator,
                maximumLinearSpeedMetersPerSecond,
                maximumAngularSpeedRadiansPerSecond,
                positionJumpMarginMeters,
                headingJumpMarginRadians,
                velocityPositionResidualMeters,
                velocityHeadingResidualRadians,
                stationaryConfirmationSeconds,
                DefaultHeadingOutlierConfirmationFrameCount,
                DefaultHeadingOutlierPredictionTimeoutSeconds,
                DefaultJumpConfirmationFrameCount,
                DefaultJumpConfirmationTimeoutSeconds,
                DefaultMaximumAutomaticFrameShiftMeters,
                DefaultMaximumAutomaticHeadingShiftRadians)
        {
        }

        /// <summary>
        /// 创建使用指定物理边界、候选确认和自动坐标连续化参数的Detour状态源。
        /// </summary>
        public DetourVehicleStateProvider(
            VelocityEstimator2D velocityEstimator,
            double maximumLinearSpeedMetersPerSecond,
            double maximumAngularSpeedRadiansPerSecond,
            double positionJumpMarginMeters,
            double headingJumpMarginRadians,
            double velocityPositionResidualMeters,
            double velocityHeadingResidualRadians,
            double stationaryConfirmationSeconds,
            int headingOutlierConfirmationFrameCount,
            double headingOutlierPredictionTimeoutSeconds,
            int jumpConfirmationFrameCount,
            double jumpConfirmationTimeoutSeconds,
            double maximumAutomaticFrameShiftMeters,
            double maximumAutomaticHeadingShiftRadians)
        {
            _velocityEstimator = velocityEstimator ??
                throw new ArgumentNullException(
                    nameof(velocityEstimator));

            NumericGuard.EnsureFinitePositive(
                maximumLinearSpeedMetersPerSecond,
                nameof(maximumLinearSpeedMetersPerSecond));
            NumericGuard.EnsureFinitePositive(
                maximumAngularSpeedRadiansPerSecond,
                nameof(maximumAngularSpeedRadiansPerSecond));
            NumericGuard.EnsureFiniteNonNegative(
                positionJumpMarginMeters,
                nameof(positionJumpMarginMeters));
            NumericGuard.EnsureFiniteNonNegative(
                headingJumpMarginRadians,
                nameof(headingJumpMarginRadians));
            NumericGuard.EnsureFinitePositive(
                velocityPositionResidualMeters,
                nameof(velocityPositionResidualMeters));
            NumericGuard.EnsureFinitePositive(
                velocityHeadingResidualRadians,
                nameof(velocityHeadingResidualRadians));
            NumericGuard.EnsureFinitePositive(
                stationaryConfirmationSeconds,
                nameof(stationaryConfirmationSeconds));
            NumericGuard.EnsureFinitePositive(
                headingOutlierPredictionTimeoutSeconds,
                nameof(headingOutlierPredictionTimeoutSeconds));
            NumericGuard.EnsureFinitePositive(
                jumpConfirmationTimeoutSeconds,
                nameof(jumpConfirmationTimeoutSeconds));
            NumericGuard.EnsureFinitePositive(
                maximumAutomaticFrameShiftMeters,
                nameof(maximumAutomaticFrameShiftMeters));
            NumericGuard.EnsureFinitePositive(
                maximumAutomaticHeadingShiftRadians,
                nameof(maximumAutomaticHeadingShiftRadians));

            if (jumpConfirmationFrameCount < 2)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(jumpConfirmationFrameCount),
                    "Detour跳变至少需要两个新帧确认。");
            }

            if (headingOutlierConfirmationFrameCount < 2)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(headingOutlierConfirmationFrameCount),
                    "Detour航向异常至少需要两个新帧确认。");
            }

            _maximumLinearSpeedMetersPerSecond =
                maximumLinearSpeedMetersPerSecond;
            _maximumAngularSpeedRadiansPerSecond =
                maximumAngularSpeedRadiansPerSecond;
            _positionJumpMarginMeters = positionJumpMarginMeters;
            _headingJumpMarginRadians = headingJumpMarginRadians;
            _velocityPositionResidualMeters =
                velocityPositionResidualMeters;
            _velocityHeadingResidualRadians =
                velocityHeadingResidualRadians;
            _stationaryConfirmationSeconds =
                stationaryConfirmationSeconds;
            _headingOutlierConfirmationFrameCount =
                headingOutlierConfirmationFrameCount;
            _headingOutlierPredictionTimeoutSeconds =
                headingOutlierPredictionTimeoutSeconds;
            _jumpConfirmationFrameCount =
                jumpConfirmationFrameCount;
            _jumpConfirmationTimeoutSeconds =
                jumpConfirmationTimeoutSeconds;
            _maximumAutomaticFrameShiftMeters =
                maximumAutomaticFrameShiftMeters;
            _maximumAutomaticHeadingShiftRadians =
                maximumAutomaticHeadingShiftRadians;
        }

        /// <summary>
        /// 获取最近一次状态不可用或正在使用短时预测的原因。
        /// </summary>
        public string LastFailureReason { get; private set; } =
            string.Empty;

        /// <summary>
        /// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
        /// </summary>
        public string LastHeadingFailureReason { get; private set; } =
            string.Empty;

        /// <summary>
        /// 获取最近一次读取到的Detour源时间戳原值。
        /// </summary>
        public long? LastDetourTickRaw { get; private set; }

        /// <summary>
        /// 获取最近一次Detour l_step原值；其精确定义仍由Detour接口文档确认。
        /// </summary>
        public double? LastDetourLocalizationStep { get; private set; }

        /// <summary>
        /// 获取当前是否正在确认疑似Detour坐标跳变。
        /// </summary>
        public bool IsJumpCandidateActive => _jumpCandidateActive;

        /// <summary>
        /// 获取本状态源生命周期内已自动连续化的Detour坐标偏移次数。
        /// </summary>
        public int AutomaticFrameShiftCount { get; private set; }

        /// <summary>
        /// 获取最近一次Detour读取对应的跳变确认和数据新鲜度诊断。
        /// </summary>
        internal bool TryGetLatestDiagnostics(
            out bool jumpCandidateActive,
            out int jumpCandidateConsistentFrameCount,
            out double estimatedShiftDistanceMeters,
            out double estimatedShiftHeadingRadians,
            out int automaticFrameShiftCount,
            out string stateStatusReason,
            out double detourDataAgeMilliseconds)
        {
            lock (_syncRoot)
            {
                jumpCandidateActive = _jumpCandidateActive;
                jumpCandidateConsistentFrameCount =
                    _candidateConsistentFrameCount;
                estimatedShiftDistanceMeters =
                    _candidateShiftDistanceMeters;
                estimatedShiftHeadingRadians =
                    _candidateShiftHeadingRadians;
                automaticFrameShiftCount =
                    AutomaticFrameShiftCount;
                stateStatusReason =
                    GetDiagnosticStateStatusReason();

                if (!LastDetourTickRaw.HasValue)
                {
                    detourDataAgeMilliseconds = 0.0;
                    return false;
                }

                detourDataAgeMilliseconds =
                    (DateTime.Now.Ticks -
                     LastDetourTickRaw.Value) /
                    (double)TimeSpan.TicksPerMillisecond;
                return true;
            }
        }

        /// <summary>
        /// 获取最近一次Detour源帧、轮速预测、创新门限、候选原因和状态诊断。
        /// </summary>
        internal bool TryGetLatestDiagnostics(
            out bool jumpCandidateActive,
            out int jumpCandidateConsistentFrameCount,
            out double estimatedShiftDistanceMeters,
            out double estimatedShiftHeadingRadians,
            out int automaticFrameShiftCount,
            out double sourceFrameIntervalSeconds,
            out double motionPredictionTimestampSeconds,
            out bool hasInnovationDiagnostics,
            out double positionInnovationMeters,
            out double allowedPositionInnovationMeters,
            out double headingInnovationRadians,
            out double allowedHeadingInnovationRadians,
            out string jumpCandidateTriggerReason,
            out string stateStatus,
            out string stateStatusReason,
            out double detourDataAgeMilliseconds)
        {
            lock (_syncRoot)
            {
                jumpCandidateActive = _jumpCandidateActive;
                jumpCandidateConsistentFrameCount =
                    _candidateConsistentFrameCount;
                estimatedShiftDistanceMeters =
                    _candidateShiftDistanceMeters;
                estimatedShiftHeadingRadians =
                    _candidateShiftHeadingRadians;
                automaticFrameShiftCount =
                    AutomaticFrameShiftCount;
                sourceFrameIntervalSeconds =
                    _hasDetourSourceFrameInterval
                        ? _lastDetourSourceFrameIntervalSeconds
                        : 0.0;
                motionPredictionTimestampSeconds =
                    _hasMotionPrediction
                        ? _predictionTimestampSeconds
                        : 0.0;
                hasInnovationDiagnostics =
                    _hasInnovationDiagnostics;
                positionInnovationMeters =
                    _lastPositionInnovationMeters;
                allowedPositionInnovationMeters =
                    _lastAllowedPositionInnovationMeters;
                headingInnovationRadians =
                    _lastHeadingInnovationRadians;
                allowedHeadingInnovationRadians =
                    _lastAllowedHeadingInnovationRadians;
                jumpCandidateTriggerReason =
                    _lastJumpCandidateTriggerReason;
                stateStatus = GetDiagnosticStateStatus();
                stateStatusReason =
                    GetDiagnosticStateStatusReason();

                if (!LastDetourTickRaw.HasValue)
                {
                    detourDataAgeMilliseconds = 0.0;
                    return false;
                }

                detourDataAgeMilliseconds =
                    (DateTime.Now.Ticks -
                     LastDetourTickRaw.Value) /
                    (double)TimeSpan.TicksPerMillisecond;
                return true;
            }
        }

        /// <summary>
        /// 尝试读取Detour，并输出当前任务坐标系中的连续车辆状态。
        /// </summary>
        public bool TryGetState(out VehicleState state)
        {
            lock (_syncRoot)
            {
                try
                {
                    var observation = ReadDetourObservation();
                    var timestampSeconds =
                        _clock.Elapsed.TotalSeconds;
                    AdvanceMotionPrediction(timestampSeconds);

                    LastDetourTickRaw = observation.TickRaw;
                    LastDetourLocalizationStep =
                        observation.LocalizationStep;

                    if (!_hasObservedDetourFrame)
                    {
                        SetLastObservedDetourFrame(observation);
                        AcceptReliableHeadingObservation(observation);
                        state = AcceptPoseAfterReset(
                            observation.PoseInDetour,
                            timestampSeconds);
                        LastFailureReason = string.Empty;
                        return true;
                    }

                    if (observation.TickRaw < _lastObservedTickRaw)
                    {
                        InvalidateHeadingObservation(
                            "Detour源时间戳发生倒退，航向暂不可用。");
                        state = default;
                        LastFailureReason =
                            "Detour源时间戳发生倒退，车辆状态暂不可用。";
                        return false;
                    }

                    if (observation.TickRaw == _lastObservedTickRaw)
                    {
                        return HandleRepeatedDetourFrame(
                            timestampSeconds,
                            out state);
                    }

                    var sourceDeltaTimeSeconds =
                        (observation.TickRaw -
                         _lastObservedTickRaw) /
                        (double)TimeSpan.TicksPerSecond;
                    var previousPoseInDetour =
                        _lastObservedPoseInDetour;
                    SetLastObservedDetourFrame(observation);

                    if (!NumericGuard.IsFinite(
                            sourceDeltaTimeSeconds) ||
                        sourceDeltaTimeSeconds <= 0.0)
                    {
                        InvalidateHeadingObservation(
                            "Detour新帧时间间隔无效，航向暂不可用。");
                        state = default;
                        LastFailureReason =
                            "Detour新帧时间间隔无效，车辆状态暂不可用。";
                        return false;
                    }

                    _lastDetourSourceFrameIntervalSeconds =
                        sourceDeltaTimeSeconds;
                    _hasDetourSourceFrameInterval = true;

                    var poseInControl =
                        FrameTransform2D.TransformPose(
                            _controlFromDetour,
                            observation.PoseInDetour);
                    var predictedPoseInControl =
                        GetPredictedPoseInControl();

                    var sourceHeadingPlausible =
                        IsHeadingMotionPlausible(
                            previousPoseInDetour,
                            observation.PoseInDetour,
                            sourceDeltaTimeSeconds);

                    if (_jumpCandidateActive)
                    {
                        IsPoseInnovationAbnormal(
                            poseInControl,
                            predictedPoseInControl,
                            sourceDeltaTimeSeconds);
                        UpdateHeadingObservation(
                            observation,
                            sourceHeadingPlausible,
                            timestampSeconds);
                        return HandleJumpCandidate(
                            observation,
                            poseInControl,
                            predictedPoseInControl,
                            sourceDeltaTimeSeconds,
                            timestampSeconds,
                            out state);
                    }

                    var sourceMotionPlausible =
                        IsMotionPlausible(
                            previousPoseInDetour,
                            observation.PoseInDetour,
                            sourceDeltaTimeSeconds);
                    var innovationAbnormal =
                        IsPoseInnovationAbnormal(
                            poseInControl,
                            predictedPoseInControl,
                            sourceDeltaTimeSeconds);
                    UpdateHeadingObservation(
                        observation,
                        sourceHeadingPlausible,
                        timestampSeconds);

                    if (!sourceMotionPlausible ||
                        innovationAbnormal)
                    {
                        BeginJumpCandidate(
                            observation.PoseInDetour,
                            poseInControl,
                            predictedPoseInControl,
                            timestampSeconds,
                            BuildJumpCandidateTriggerReason(
                                sourceMotionPlausible));

                        return ReturnCandidateState(
                            timestampSeconds,
                            out state);
                    }

                    if (ArePosesEquivalent(
                            poseInControl,
                            _acceptedPoseInControl))
                    {
                        state = HandleRepeatedPose(
                            timestampSeconds);
                        LastFailureReason = string.Empty;
                        return true;
                    }

                    state = _stationaryHoldActive
                        ? AcceptPoseAfterReset(
                            poseInControl,
                            timestampSeconds)
                        : AcceptContinuousPose(
                            poseInControl,
                            timestampSeconds);
                    LastFailureReason = string.Empty;
                    return true;
                }
                catch (Exception exception)
                {
                    InvalidateHeadingObservation(
                        "Detour航向读取失败：" +
                        exception.Message);
                    state = default;
                    LastFailureReason =
                        "Detour车辆状态读取失败：" +
                        exception.Message;
                    return false;
                }
            }
        }

        /// <summary>
        /// 尝试读取经过独立物理边界和创新校验的任务坐标系航向。
        /// 位置单独异常时仍可成功，供原地自转等只依赖航向的动作使用。
        /// </summary>
        public bool TryGetHeadingRadians(
            out double headingRadians)
        {
            lock (_syncRoot)
            {
                TryGetState(out _);
                return TryGetLatestReliableHeadingRadians(
                    out headingRadians);
            }
        }

        internal bool TryGetLatestReliableHeadingRadians(
            out double headingRadians)
        {
            lock (_syncRoot)
            {
                if (_headingOutlierCandidateActive)
                {
                    var candidateAgeSeconds =
                        _clock.Elapsed.TotalSeconds -
                        _headingOutlierStartedTimestampSeconds;
                    if (candidateAgeSeconds >=
                        _headingOutlierPredictionTimeoutSeconds)
                    {
                        InvalidateHeadingObservation(
                            "Detour航向异常短时预测超过" +
                            _headingOutlierPredictionTimeoutSeconds
                                .ToString("F2", CultureInfo.InvariantCulture) +
                            "s，航向暂不可用。");
                    }
                    else if (_hasReliableHeadingObservation &&
                             _latestWheelVelocityValid &&
                             _hasMotionPrediction)
                    {
                        headingRadians =
                            GetPredictedPoseInControl()
                                .YawRadians;
                        LastHeadingFailureReason = string.Empty;
                        return true;
                    }
                    else
                    {
                        InvalidateHeadingObservation(
                            "Detour航向异常期间缺少有效轮组角速度预测，航向暂不可用。");
                    }
                }

                if (!_hasReliableHeadingObservation)
                {
                    headingRadians = 0.0;
                    if (string.IsNullOrWhiteSpace(
                            LastHeadingFailureReason))
                    {
                        LastHeadingFailureReason =
                            string.IsNullOrWhiteSpace(
                                LastFailureReason)
                                ? "Detour当前没有可靠航向观测。"
                                : LastFailureReason;
                    }

                    return false;
                }

                headingRadians = AngleMath.NormalizeRadians(
                    _controlFromDetour.YawRadians +
                    _latestReliableHeadingInDetourRadians);
                LastHeadingFailureReason = string.Empty;
                return true;
            }
        }

        /// <summary>
        /// 原地自转已经停车后，允许现有候选按普通有限偏移规则重新确认。
        /// 自转期间不会调用该入口，因此不会在旋转中改写任务坐标系。
        /// </summary>
        public void BeginPostRotationPositionRecovery()
        {
            lock (_syncRoot)
            {
                if (!_jumpCandidateActive)
                {
                    return;
                }

                _candidateAutomaticShiftForbidden = false;
                _postRotationPositionRecoveryActive = true;
                _candidateEligibleForAutomaticShift =
                    _candidateShiftDistanceMeters <=
                        _maximumAutomaticFrameShiftMeters &&
                    _candidateShiftHeadingRadians <=
                        _maximumAutomaticHeadingShiftRadians;
                _candidateConsistentFrameCount = 0;
                _candidateStartedTimestampSeconds =
                    _clock.Elapsed.TotalSeconds;
                _lastJumpCandidateTriggerReason =
                    AppendDiagnosticToken(
                        _lastJumpCandidateTriggerReason,
                        "PostRotationPositionRecovery");
            }
        }

        /// <summary>
        /// 由轮组状态源在读取Detour前提供最新滤波车体速度和角速度，用于短时间位姿预测。
        /// </summary>
        internal void UpdateWheelVelocityEstimate(
            double bodyVxMetersPerSecond,
            double bodyVyMetersPerSecond,
            double bodyOmegaRadiansPerSecond,
            bool velocityEstimateValid)
        {
            NumericGuard.EnsureFinite(
                bodyVxMetersPerSecond,
                nameof(bodyVxMetersPerSecond));
            NumericGuard.EnsureFinite(
                bodyVyMetersPerSecond,
                nameof(bodyVyMetersPerSecond));
            NumericGuard.EnsureFinite(
                bodyOmegaRadiansPerSecond,
                nameof(bodyOmegaRadiansPerSecond));

            lock (_syncRoot)
            {
                var timestampSeconds =
                    _clock.Elapsed.TotalSeconds;
                AdvanceMotionPrediction(timestampSeconds);

                _latestWheelTwistInBody = new Twist2D(
                    bodyVxMetersPerSecond,
                    bodyVyMetersPerSecond,
                    bodyOmegaRadiansPerSecond);
                _latestWheelVelocityValid =
                    velocityEstimateValid;
            }
        }

        /// <summary>
        /// 清除Detour历史、任务坐标偏移、候选跳变和速度预测状态。
        /// </summary>
        public void Reset()
        {
            lock (_syncRoot)
            {
                _velocityEstimator.Reset();
                _acceptedPoseInControl = Pose2D.Identity;
                _acceptedTimestampSeconds = 0.0;
                _latestState = default;
                _stationaryHoldActive = false;

                _hasObservedDetourFrame = false;
                _lastObservedPoseInDetour = Pose2D.Identity;
                _lastObservedTickRaw = 0L;
                _hasDetourSourceFrameInterval = false;
                _lastDetourSourceFrameIntervalSeconds = 0.0;
                _controlFromDetour = Pose2D.Identity;

                _hasMotionPrediction = false;
                _predictedPoseInControl = Pose2D.Identity;
                _predictionTimestampSeconds = 0.0;
                _latestWheelTwistInBody = Twist2D.Zero;
                _latestWheelVelocityValid = false;

                _hasInnovationDiagnostics = false;
                _lastPositionInnovationMeters = 0.0;
                _lastAllowedPositionInnovationMeters = 0.0;
                _lastHeadingInnovationRadians = 0.0;
                _lastAllowedHeadingInnovationRadians = 0.0;
                _lastJumpCandidateTriggerReason = string.Empty;

                _hasReliableHeadingObservation = false;
                _latestReliableHeadingInDetourRadians = 0.0;
                ClearHeadingOutlierCandidate();

                ClearJumpCandidate();
                AutomaticFrameShiftCount = 0;
                LastDetourTickRaw = null;
                LastDetourLocalizationStep = null;
                LastFailureReason = string.Empty;
                LastHeadingFailureReason = string.Empty;
            }
        }

        private bool HandleRepeatedDetourFrame(
            double timestampSeconds,
            out VehicleState state)
        {
            if (_jumpCandidateActive)
            {
                return ReturnCandidateState(
                    timestampSeconds,
                    out state);
            }

            state = HandleRepeatedPose(timestampSeconds);
            LastFailureReason = string.Empty;
            return true;
        }

        private bool HandleJumpCandidate(
            DetourObservation observation,
            Pose2D poseUsingCurrentTransform,
            Pose2D predictedPoseInControl,
            double sourceDeltaTimeSeconds,
            double timestampSeconds,
            out VehicleState state)
        {
            if (!IsPoseInnovationAbnormal(
                    poseUsingCurrentTransform,
                    predictedPoseInControl,
                    sourceDeltaTimeSeconds))
            {
                ClearJumpCandidate();
                state = AcceptPoseAfterReset(
                    poseUsingCurrentTransform,
                    timestampSeconds);
                LastFailureReason = string.Empty;
                return true;
            }

            if (timestampSeconds -
                    _candidateStartedTimestampSeconds >
                _jumpConfirmationTimeoutSeconds)
            {
                return ReturnCandidateState(
                    timestampSeconds,
                    out state);
            }

            var poseUsingCandidateTransform =
                FrameTransform2D.TransformPose(
                    _candidateControlFromDetour,
                    observation.PoseInDetour);
            var candidateIsConsistent =
                !IsPoseInnovationAbnormal(
                    poseUsingCandidateTransform,
                    predictedPoseInControl,
                    sourceDeltaTimeSeconds,
                    recordDiagnostics: false);

            if (candidateIsConsistent)
            {
                _candidateConsistentFrameCount++;

                if (_candidateEligibleForAutomaticShift &&
                    _candidateConsistentFrameCount >=
                    _jumpConfirmationFrameCount)
                {
                    _controlFromDetour =
                        FrameTransform2D.Compose(
                            predictedPoseInControl,
                            FrameTransform2D.Inverse(
                                observation.PoseInDetour));
                    ClearJumpCandidate();
                    AutomaticFrameShiftCount++;
                    state = AcceptPoseAfterReset(
                        predictedPoseInControl,
                        timestampSeconds);
                    LastFailureReason = string.Empty;
                    return true;
                }
            }
            else
            {
                // 首个异常帧可能只是离群值。后续异常帧若属于另一稳定分支，
                // 以新分支重新累计连续帧，但保留总确认超时起点。
                var candidateStartedTimestampSeconds =
                    _candidateStartedTimestampSeconds;
                BeginJumpCandidate(
                    observation.PoseInDetour,
                    poseUsingCurrentTransform,
                    predictedPoseInControl,
                    timestampSeconds,
                    AppendDiagnosticToken(
                        BuildJumpCandidateTriggerReason(
                            sourceMotionPlausible: true),
                        "CandidateBranchChanged"),
                    preserveAutomaticShiftRestriction: true);
                _candidateStartedTimestampSeconds =
                    candidateStartedTimestampSeconds;
            }

            return ReturnCandidateState(
                timestampSeconds,
                out state);
        }

        private void BeginJumpCandidate(
            Pose2D poseInDetour,
            Pose2D poseUsingCurrentTransform,
            Pose2D predictedPoseInControl,
            double timestampSeconds,
            string triggerReason,
            bool preserveAutomaticShiftRestriction = false)
        {
            var automaticShiftAlreadyForbidden =
                preserveAutomaticShiftRestriction &&
                _candidateAutomaticShiftForbidden;

            GetPoseDifference(
                poseUsingCurrentTransform,
                predictedPoseInControl,
                out _candidateShiftDistanceMeters,
                out _candidateShiftHeadingRadians);

            _candidateControlFromDetour =
                FrameTransform2D.Compose(
                    predictedPoseInControl,
                    FrameTransform2D.Inverse(
                        poseInDetour));
            _candidateConsistentFrameCount = 1;
            _candidateStartedTimestampSeconds =
                timestampSeconds;
            _candidateAutomaticShiftForbidden =
                automaticShiftAlreadyForbidden ||
                (!_postRotationPositionRecoveryActive &&
                 IsInPlaceRotationFromWheelFeedback());
            _lastJumpCandidateTriggerReason =
                _candidateAutomaticShiftForbidden
                    ? AppendDiagnosticToken(
                        triggerReason,
                        "AutomaticShiftForbiddenDuringInPlaceRotation")
                    : triggerReason;
            _candidateEligibleForAutomaticShift =
                _candidateShiftDistanceMeters <=
                    _maximumAutomaticFrameShiftMeters &&
                _candidateShiftHeadingRadians <=
                    _maximumAutomaticHeadingShiftRadians &&
                !_candidateAutomaticShiftForbidden;
            _jumpCandidateActive = true;
        }

        private bool ReturnCandidateState(
            double timestampSeconds,
            out VehicleState state)
        {
            var candidateAgeSeconds =
                timestampSeconds -
                _candidateStartedTimestampSeconds;

            if (candidateAgeSeconds >
                _jumpConfirmationTimeoutSeconds)
            {
                state = default;
                LastFailureReason = _candidateEligibleForAutomaticShift
                    ? "Detour疑似坐标跳变未能在" +
                      $"{_jumpConfirmationTimeoutSeconds:F2}s内确认，" +
                      "车辆状态已置为不可用。"
                    : "Detour位姿偏移在短时确认后仍未恢复，且不能安全地自动连续化：平移" +
                      $"{_candidateShiftDistanceMeters:F3}m，航向" +
                      $"{AngleMath.RadiansToDegrees(_candidateShiftHeadingRadians):F2}°。" +
                      "车辆状态已置为不可用，应停车并重新定位或规划。";
                return false;
            }

            state = CreatePredictedState(timestampSeconds);
            LastFailureReason = _candidateEligibleForAutomaticShift
                ? "Detour疑似坐标跳变正在确认，当前使用轮组速度短时预测位姿。"
                : "Detour位姿暂不连续且当前运动状态禁止自动坐标修正，" +
                  "正在使用轮组速度进行短时恢复确认。";
            return true;
        }

        /// <summary>
        /// 判断轮组反馈是否表明车辆正在近似原地自转；此时禁止自动改写任务坐标系。
        /// </summary>
        private bool IsInPlaceRotationFromWheelFeedback()
        {
            if (!_latestWheelVelocityValid)
            {
                return false;
            }

            var linearSpeedMetersPerSecond = Math.Sqrt(
                _latestWheelTwistInBody.VxMetersPerSecond *
                _latestWheelTwistInBody.VxMetersPerSecond +
                _latestWheelTwistInBody.VyMetersPerSecond *
                _latestWheelTwistInBody.VyMetersPerSecond);

            return linearSpeedMetersPerSecond <=
                       InPlaceRotationLinearSpeedThresholdMetersPerSecond &&
                   Math.Abs(
                       _latestWheelTwistInBody
                           .OmegaRadiansPerSecond) >=
                       InPlaceRotationAngularSpeedThresholdRadiansPerSecond;
        }

        private void ClearJumpCandidate()
        {
            _jumpCandidateActive = false;
            _candidateControlFromDetour = Pose2D.Identity;
            _candidateConsistentFrameCount = 0;
            _candidateStartedTimestampSeconds = 0.0;
            _candidateShiftDistanceMeters = 0.0;
            _candidateShiftHeadingRadians = 0.0;
            _candidateAutomaticShiftForbidden = false;
            _candidateEligibleForAutomaticShift = false;
            _postRotationPositionRecoveryActive = false;
        }

        private static DetourObservation ReadDetourObservation()
        {
            var location = DetourInterface.getCartLocation();
            var xMillimeters = Convert.ToDouble(
                location.x,
                CultureInfo.InvariantCulture);
            var yMillimeters = Convert.ToDouble(
                location.y,
                CultureInfo.InvariantCulture);
            var yawDegrees = Convert.ToDouble(
                location.th,
                CultureInfo.InvariantCulture);
            var tickRaw = Convert.ToInt64(
                location.tick,
                CultureInfo.InvariantCulture);
            var localizationStep = Convert.ToDouble(
                location.l_step,
                CultureInfo.InvariantCulture);

            NumericGuard.EnsureFinite(
                xMillimeters,
                "DetourX");
            NumericGuard.EnsureFinite(
                yMillimeters,
                "DetourY");
            NumericGuard.EnsureFinite(
                yawDegrees,
                "DetourTheta");
            NumericGuard.EnsureFinite(
                localizationStep,
                "DetourLStep");

            if (tickRaw <= 0L)
            {
                throw new InvalidOperationException(
                    "Detour源时间戳必须为正整数。");
            }

            return new DetourObservation(
                new Pose2D(
                    xMillimeters / MillimetersPerMeter,
                    yMillimeters / MillimetersPerMeter,
                    AngleMath.NormalizeRadians(
                        AngleMath.DegreesToRadians(
                            yawDegrees))),
                tickRaw,
                localizationStep);
        }

        private void SetLastObservedDetourFrame(
            DetourObservation observation)
        {
            _lastObservedPoseInDetour =
                observation.PoseInDetour;
            _lastObservedTickRaw = observation.TickRaw;
            _hasObservedDetourFrame = true;
        }

        private VehicleState AcceptContinuousPose(
            Pose2D poseInControl,
            double timestampSeconds)
        {
            _latestState = _velocityEstimator.Update(
                poseInControl,
                timestampSeconds);
            _acceptedPoseInControl = poseInControl;
            _acceptedTimestampSeconds = timestampSeconds;
            _stationaryHoldActive = false;
            ResetMotionPrediction(
                poseInControl,
                timestampSeconds);
            return _latestState;
        }

        private VehicleState AcceptPoseAfterReset(
            Pose2D poseInControl,
            double timestampSeconds)
        {
            _latestState = _velocityEstimator.Reset(
                poseInControl,
                timestampSeconds);
            _acceptedPoseInControl = poseInControl;
            _acceptedTimestampSeconds = timestampSeconds;
            _stationaryHoldActive = false;
            ResetMotionPrediction(
                poseInControl,
                timestampSeconds);
            return _latestState;
        }

        private VehicleState HandleRepeatedPose(
            double timestampSeconds)
        {
            var unchangedSeconds =
                timestampSeconds -
                _acceptedTimestampSeconds;

            if (!_stationaryHoldActive &&
                unchangedSeconds >=
                _stationaryConfirmationSeconds)
            {
                _latestState = new VehicleState(
                    timestampSeconds,
                    _acceptedPoseInControl,
                    Twist2D.Zero,
                    true);
                _stationaryHoldActive = true;
                ResetMotionPrediction(
                    _acceptedPoseInControl,
                    timestampSeconds);
            }

            return _latestState;
        }

        private VehicleState CreatePredictedState(
            double timestampSeconds)
        {
            var predictedPose = GetPredictedPoseInControl();
            var twistInBody = _latestWheelVelocityValid
                ? _latestWheelTwistInBody
                : Twist2D.Zero;
            var twistInControl =
                FrameTransform2D.TransformTwistAtSamePoint(
                    predictedPose,
                    twistInBody);
            _latestState = new VehicleState(
                timestampSeconds,
                predictedPose,
                twistInControl,
                _latestWheelVelocityValid);
            return _latestState;
        }

        private void ResetMotionPrediction(
            Pose2D poseInControl,
            double timestampSeconds)
        {
            _predictedPoseInControl = poseInControl;
            _predictionTimestampSeconds = timestampSeconds;
            _hasMotionPrediction = true;
        }

        private void AdvanceMotionPrediction(
            double timestampSeconds)
        {
            if (!_hasMotionPrediction)
            {
                return;
            }

            var deltaTimeSeconds =
                timestampSeconds -
                _predictionTimestampSeconds;
            if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
                deltaTimeSeconds <= 0.0)
            {
                _predictionTimestampSeconds = timestampSeconds;
                return;
            }

            if (_latestWheelVelocityValid)
            {
                _predictedPoseInControl = IntegrateBodyTwist(
                    _predictedPoseInControl,
                    _latestWheelTwistInBody,
                    deltaTimeSeconds);
            }

            _predictionTimestampSeconds = timestampSeconds;
        }

        private Pose2D GetPredictedPoseInControl()
        {
            return _hasMotionPrediction
                ? _predictedPoseInControl
                : _acceptedPoseInControl;
        }

        private static Pose2D IntegrateBodyTwist(
            Pose2D startPoseInControl,
            Twist2D twistInBody,
            double deltaTimeSeconds)
        {
            var middleYawRadians =
                startPoseInControl.YawRadians +
                0.5 * twistInBody.OmegaRadiansPerSecond *
                deltaTimeSeconds;
            var cos = Math.Cos(middleYawRadians);
            var sin = Math.Sin(middleYawRadians);
            var velocityXInControl =
                cos * twistInBody.VxMetersPerSecond -
                sin * twistInBody.VyMetersPerSecond;
            var velocityYInControl =
                sin * twistInBody.VxMetersPerSecond +
                cos * twistInBody.VyMetersPerSecond;

            return new Pose2D(
                startPoseInControl.XMeters +
                velocityXInControl * deltaTimeSeconds,
                startPoseInControl.YMeters +
                velocityYInControl * deltaTimeSeconds,
                AngleMath.NormalizeRadians(
                    startPoseInControl.YawRadians +
                    twistInBody.OmegaRadiansPerSecond *
                    deltaTimeSeconds));
        }

        private bool IsMotionPlausible(
            Pose2D startPose,
            Pose2D endPose,
            double deltaTimeSeconds)
        {
            if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
                deltaTimeSeconds <= 0.0)
            {
                return false;
            }

            GetPoseDifference(
                startPose,
                endPose,
                out var displacementMeters,
                out var headingChangeRadians);

            var maximumDisplacementMeters =
                _maximumLinearSpeedMetersPerSecond *
                deltaTimeSeconds +
                _positionJumpMarginMeters;
            var maximumHeadingChangeRadians =
                GetMaximumHeadingChangeRadians(
                    deltaTimeSeconds);

            return displacementMeters <=
                       maximumDisplacementMeters &&
                   headingChangeRadians <=
                        maximumHeadingChangeRadians;
        }

        private bool IsHeadingMotionPlausible(
            Pose2D startPose,
            Pose2D endPose,
            double deltaTimeSeconds)
        {
            if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
                deltaTimeSeconds <= 0.0)
            {
                return false;
            }

            var headingChangeRadians = Math.Abs(
                AngleMath.ShortestDifferenceRadians(
                    endPose.YawRadians,
                    startPose.YawRadians));
            return headingChangeRadians <=
                   GetMaximumHeadingChangeRadians(
                       deltaTimeSeconds);
        }

        private double GetMaximumHeadingChangeRadians(
            double deltaTimeSeconds)
        {
            var measuredAngularSpeedRadiansPerSecond =
                _latestWheelVelocityValid
                    ? Math.Abs(
                        _latestWheelTwistInBody
                            .OmegaRadiansPerSecond)
                    : 0.0;
            var plausibleAngularSpeedRadiansPerSecond =
                Math.Max(
                    _maximumAngularSpeedRadiansPerSecond,
                    measuredAngularSpeedRadiansPerSecond);
            return plausibleAngularSpeedRadiansPerSecond *
                   deltaTimeSeconds +
                   _headingJumpMarginRadians;
        }

        private bool IsPoseInnovationAbnormal(
            Pose2D observedPoseInControl,
            Pose2D predictedPoseInControl,
            double sourceDeltaTimeSeconds,
            bool recordDiagnostics = true)
        {
            GetPoseDifference(
                observedPoseInControl,
                predictedPoseInControl,
                out var positionResidualMeters,
                out var headingResidualRadians);

            // 轮速预测与Detour帧的采样时刻可能相差一个源周期。
            // 航向残差限值随实际角速度和Detour帧间隔增加，避免正常自转
            // 被固定角度阈值误判；静止和低速时仍保留原来的严格限值。
            var headingTimingAllowanceRadians =
                _latestWheelVelocityValid
                    ? Math.Abs(
                        _latestWheelTwistInBody
                            .OmegaRadiansPerSecond) *
                      sourceDeltaTimeSeconds
                    : 0.0;
            var maximumHeadingResidualRadians =
                _velocityHeadingResidualRadians +
                headingTimingAllowanceRadians;

            if (recordDiagnostics)
            {
                _hasInnovationDiagnostics = true;
                _lastPositionInnovationMeters =
                    positionResidualMeters;
                _lastAllowedPositionInnovationMeters =
                    _velocityPositionResidualMeters;
                _lastHeadingInnovationRadians =
                    headingResidualRadians;
                _lastAllowedHeadingInnovationRadians =
                    maximumHeadingResidualRadians;
            }

            return positionResidualMeters >
                       _velocityPositionResidualMeters ||
                   headingResidualRadians >
                       maximumHeadingResidualRadians;
        }

        /// <summary>
        /// 接受健康Detour航向，或将单帧异常转入轮组Vw短时预测确认。
        /// </summary>
        private void UpdateHeadingObservation(
            DetourObservation observation,
            bool sourceHeadingPlausible,
            double timestampSeconds)
        {
            var headingInnovationPlausible =
                !_hasInnovationDiagnostics ||
                _lastHeadingInnovationRadians <=
                    _lastAllowedHeadingInnovationRadians;

            if (sourceHeadingPlausible &&
                headingInnovationPlausible)
            {
                AcceptReliableHeadingObservation(observation);
                return;
            }

            var reason = !sourceHeadingPlausible
                ? "Detour单帧航向变化超出物理边界。"
                : "Detour航向创新超过当前动态允许值。";
            BeginOrContinueHeadingOutlierCandidate(
                timestampSeconds,
                reason);
        }

        /// <summary>
        /// 接受Detour航向并清除尚未确认的航向异常候选。
        /// </summary>
        private void AcceptReliableHeadingObservation(
            DetourObservation observation)
        {
            ClearHeadingOutlierCandidate();
            _latestReliableHeadingInDetourRadians =
                observation.PoseInDetour.YawRadians;
            _hasReliableHeadingObservation = true;
            LastHeadingFailureReason = string.Empty;
        }

        /// <summary>
        /// 终止短时航向预测并将独立航向标记为不可用。
        /// </summary>
        private void InvalidateHeadingObservation(
            string reason)
        {
            ClearHeadingOutlierCandidate();
            _hasReliableHeadingObservation = false;
            LastHeadingFailureReason = reason;
        }

        /// <summary>
        /// 对连续异常新帧计数，并在有限时间内保留轮组预测航向。
        /// </summary>
        private void BeginOrContinueHeadingOutlierCandidate(
            double timestampSeconds,
            string reason)
        {
            if (!_hasReliableHeadingObservation ||
                !_latestWheelVelocityValid ||
                !_hasMotionPrediction)
            {
                InvalidateHeadingObservation(
                    reason +
                    " 当前缺少可靠航向或轮组角速度预测，无法短时降级。");
                return;
            }

            if (!_headingOutlierCandidateActive)
            {
                _headingOutlierCandidateActive = true;
                _headingOutlierConsecutiveFrameCount = 1;
                _headingOutlierStartedTimestampSeconds =
                    timestampSeconds;
            }
            else
            {
                _headingOutlierConsecutiveFrameCount++;
            }

            _headingOutlierCandidateReason =
                "Detour航向异常正在确认(" +
                _headingOutlierConsecutiveFrameCount +
                "/" +
                _headingOutlierConfirmationFrameCount +
                ")，当前使用轮组Vw短时预测。原因：" +
                reason;
            LastHeadingFailureReason = string.Empty;

            if (_headingOutlierConsecutiveFrameCount >=
                _headingOutlierConfirmationFrameCount ||
                timestampSeconds -
                    _headingOutlierStartedTimestampSeconds >=
                _headingOutlierPredictionTimeoutSeconds)
            {
                InvalidateHeadingObservation(
                    "Detour航向连续异常达到确认条件，航向暂不可用。最后原因：" +
                    reason);
            }
        }

        /// <summary>
        /// 清除航向异常候选的帧数、起始时刻和诊断原因。
        /// </summary>
        private void ClearHeadingOutlierCandidate()
        {
            _headingOutlierCandidateActive = false;
            _headingOutlierConsecutiveFrameCount = 0;
            _headingOutlierStartedTimestampSeconds = 0.0;
            _headingOutlierCandidateReason = string.Empty;
        }

        private string BuildJumpCandidateTriggerReason(
            bool sourceMotionPlausible)
        {
            var reason = sourceMotionPlausible
                ? string.Empty
                : "SourceMotionOutsidePhysicalBoundary";

            if (_hasInnovationDiagnostics &&
                _lastPositionInnovationMeters >
                _lastAllowedPositionInnovationMeters)
            {
                reason = AppendDiagnosticToken(
                    reason,
                    "PositionInnovationExceeded");
            }

            if (_hasInnovationDiagnostics &&
                _lastHeadingInnovationRadians >
                _lastAllowedHeadingInnovationRadians)
            {
                reason = AppendDiagnosticToken(
                    reason,
                    "HeadingInnovationExceeded");
            }

            return string.IsNullOrWhiteSpace(reason)
                ? "PoseInnovationAbnormal"
                : reason;
        }

        private string GetDiagnosticStateStatus()
        {
            if (!LastDetourTickRaw.HasValue)
            {
                return "Uninitialized";
            }

            if (_jumpCandidateActive)
            {
                var candidateAgeSeconds =
                    _clock.Elapsed.TotalSeconds -
                    _candidateStartedTimestampSeconds;
                return candidateAgeSeconds >
                       _jumpConfirmationTimeoutSeconds
                    ? _hasReliableHeadingObservation
                        ? "PositionUnavailableHeadingAvailable"
                        : "Unavailable"
                    : "JumpCandidatePrediction";
            }

            if (_headingOutlierCandidateActive)
            {
                return "HeadingOutlierPrediction";
            }

            if (!_hasReliableHeadingObservation)
            {
                return "HeadingUnavailable";
            }

            return string.IsNullOrWhiteSpace(LastFailureReason)
                ? "Healthy"
                : "Unavailable";
        }

        /// <summary>
        /// 合并完整位姿状态原因和航向短时预测原因供CSV诊断使用。
        /// </summary>
        private string GetDiagnosticStateStatusReason()
        {
            if (!_headingOutlierCandidateActive)
            {
                return LastFailureReason;
            }

            return string.IsNullOrWhiteSpace(LastFailureReason)
                ? _headingOutlierCandidateReason
                : LastFailureReason + " " +
                  _headingOutlierCandidateReason;
        }

        private static string AppendDiagnosticToken(
            string existing,
            string token)
        {
            return string.IsNullOrWhiteSpace(existing)
                ? token
                : existing + "|" + token;
        }

        private static void GetPoseDifference(
            Pose2D firstPose,
            Pose2D secondPose,
            out double positionDifferenceMeters,
            out double headingDifferenceRadians)
        {
            var deltaX =
                firstPose.XMeters - secondPose.XMeters;
            var deltaY =
                firstPose.YMeters - secondPose.YMeters;
            positionDifferenceMeters = Math.Sqrt(
                deltaX * deltaX +
                deltaY * deltaY);
            headingDifferenceRadians = Math.Abs(
                AngleMath.ShortestDifferenceRadians(
                    firstPose.YawRadians,
                    secondPose.YawRadians));
        }

        private static bool ArePosesEquivalent(
            Pose2D firstPose,
            Pose2D secondPose)
        {
            return Math.Abs(
                       firstPose.XMeters -
                       secondPose.XMeters) <=
                   PositionEqualityToleranceMeters &&
                   Math.Abs(
                       firstPose.YMeters -
                       secondPose.YMeters) <=
                   PositionEqualityToleranceMeters &&
                   Math.Abs(
                       AngleMath.ShortestDifferenceRadians(
                           firstPose.YawRadians,
                           secondPose.YawRadians)) <=
                   HeadingEqualityToleranceRadians;
        }
    }
}