ParkingRobot/MultiWheelC/Experiments/TrackingExperimentRecorder.cs

using ClumsyCore.Interfaces;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.Numerics;
using System.Text;
using System.Threading;
using CommonUsage.Chassis;
using MyParking.Shared;
using MultiWheelC.Control.Allocation;
using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;

namespace MultiWheelC
{
    // C层实验数据：保存一个采样时刻的定位与控制命令。
    public sealed class TrackingSample
    {
        public double ElapsedSeconds;

        // Detour位置单位为mm，航向单位为deg。
        public double DetourX;
        public double DetourY;
        public double DetourTheta;
        public long DetourTickRaw;
        public double DetourLStep;

        // Detour状态估计内部诊断；偏移量仅在跳变候选有效时有意义。
        public bool HasDetourStateDiagnostics;
        public bool DetourJumpCandidateActive;
        public int DetourJumpCandidateConsistentFrameCount;
        public double DetourEstimatedShiftDistanceMeters;
        public double DetourEstimatedShiftHeadingRadians;
        public int DetourAutomaticFrameShiftCount;
        public string DetourStateStatusReason;
        public double DetourDataAgeMilliseconds;
        public double DetourSourceFrameIntervalMilliseconds;
        public double DetourMotionPredictionTimestampSeconds;
        public bool HasDetourInnovationDiagnostics;
        public double DetourPositionInnovationMeters;
        public double DetourAllowedPositionInnovationMeters;
        public double DetourHeadingInnovationRadians;
        public double DetourAllowedHeadingInnovationRadians;
        public string DetourLastJumpTriggerReason;
        public string DetourStateStatus;

        // 车体速度单位为m/s，角速度统一使用rad/s。
        public float CommandSpeed;
        public float CommandVx;
        public float CommandVy;
        public float CommandAngularSpeed;

        // 新版状态估计统一使用SI单位；无有效控制器状态时HasProcessedState为false。
        public bool HasProcessedState;
        public double StateTimestampSeconds;
        public double StateXmeters;
        public double StateYMeters;
        public double StateYawRadians;
        public double StateWorldVxMetersPerSecond;
        public double StateWorldVyMetersPerSecond;
        public double StateBodyVxMetersPerSecond;
        public double StateBodyVyMetersPerSecond;
        public double StateAngularSpeedRadiansPerSecond;
        public bool StateVelocityEstimateValid;
        public bool HasControlReference;
        public double ControlReferenceArcLengthMeters;
        public double ControlReferenceSpeedMetersPerSecond;
        public double ControlLateralErrorMeters;
        public double ControlHeadingErrorRadians;
        public double ControlDistanceToTrajectoryMeters;
        public double ControlRemainingDistanceMeters;
        public double CurvaturePreviewDistanceMeters;
        public double FeedforwardCurvaturePerMeter;

        // 并列保存Detour速度与轮速解算速度，避免StateBodyVx/Vy的数据来源产生歧义。
        public bool HasVelocityDiagnostics;
        public double DetourEstimatedBodyVxMetersPerSecond;
        public bool DetourVelocityEstimateValid;
        public double WheelFeedbackRawBodyVxMetersPerSecond;
        public double WheelFeedbackFilteredBodyVxMetersPerSecond;
        public double WheelFeedbackRawBodyVyMetersPerSecond;
        public double WheelFeedbackFilteredBodyVyMetersPerSecond;
        public bool WheelFeedbackVelocityEstimateValid;
        public bool HasWheelFeedbackAngularVelocityDiagnostics;
        public double WheelFeedbackRawBodyOmegaRadiansPerSecond;
        public double WheelFeedbackFilteredBodyOmegaRadiansPerSecond;
        public double WheelFeedbackSampleTimestampSeconds;

        // 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
        public bool HasSteeringDiagnostics;
        public double TargetSteerLeftFrontDegrees;
        public double TargetSteerLeftRearDegrees;
        public double TargetSteerRightFrontDegrees;
        public double TargetSteerRightRearDegrees;
        public double ActualSteerLeftFrontDegrees;
        public double ActualSteerLeftRearDegrees;
        public double ActualSteerRightFrontDegrees;
        public double ActualSteerRightRearDegrees;
        public bool HasGcpCommand;
        public double RequestedFrontGcpAngleRadians;
        public double RequestedRearGcpAngleRadians;
        public double SentFrontGcpAngleRadians;
        public double SentRearGcpAngleRadians;
        public bool FrontGcpRateLimitActive;
        public bool RearGcpRateLimitActive;
        // 兼容既有分析脚本：Command角仍表示限速后实际发送角。
        public double CommandFrontGcpAngleRadians;
        public double CommandRearGcpAngleRadians;
    }

    // C层实验工具：统一采集并保存轨迹跟踪实验数据。
    public sealed class TrackingExperimentRecorder
    {
        /// <summary>
        /// 保存一次控制周期计时及其所属组合运动段索引。
        /// </summary>
        private readonly struct ControlCycleTimingRecord
        {
            public ControlCycleTimingRecord(
                double recorderElapsedSeconds,
                int motionSegmentIndex,
                ParkingControlCycleTiming timing,
                GcpMotionCommand? requestedCommand,
                GcpMotionCommand? sentCommand)
            {
                RecorderElapsedSeconds =
                    recorderElapsedSeconds;
                MotionSegmentIndex = motionSegmentIndex;
                Timing = timing;
                RequestedCommand = requestedCommand;
                SentCommand = sentCommand;
            }

            public double RecorderElapsedSeconds { get; }
            public int MotionSegmentIndex { get; }
            public ParkingControlCycleTiming Timing { get; }
            public GcpMotionCommand? RequestedCommand { get; }
            public GcpMotionCommand? SentCommand { get; }
        }

        private const double GcpAngleComparisonToleranceRadians =
            1e-9;

        private readonly string _controllerName;
        private readonly string _trajectoryName;
        private readonly int _trialNumber;
        private readonly Vector2 _referenceStart;
        private readonly Vector2 _referenceEnd;
        private readonly float _referenceSpeed;
        private readonly float _referenceAngularSpeed;
        private readonly float _referenceMotionFrameYawDegrees;
        private readonly float _referenceAccelerationMetersPerSecondSquared;
        private readonly float _referenceDecelerationMetersPerSecondSquared;
        private readonly int _sampleIntervalMs;
        private readonly MultiWheelChassis _diagnosticChassis;
        private readonly WheelFeedbackVehicleStateProvider
            _diagnosticStateProvider;

        private readonly List<TrackingSample> _samples =
            new List<TrackingSample>();

        private readonly List<ControlCycleTimingRecord>
            _controlCycleTimings =
                new List<ControlCycleTimingRecord>();

        private readonly object _sampleSyncRoot =
            new object();

        private readonly object _controlCycleTimingSyncRoot =
            new object();

        private readonly object _commandSyncRoot =
            new object();

        private readonly object _stateSyncRoot =
            new object();

        private readonly Stopwatch _stopwatch =
            new Stopwatch();

        private Thread _worker;
        private volatile bool _running;
        private int _started;
        private int _saved;

        private bool _hasExternalCommand;
        private float _externalCommandSpeed;
        private float _externalCommandVx;
        private float _externalCommandVy;
        private float _externalCommandAngularSpeed;
        private VehicleState? _latestProcessedState;
        private bool _hasControlReference;
        private double _controlReferenceArcLengthMeters;
        private double _controlReferenceSpeedMetersPerSecond;
        private double _controlLateralErrorMeters;
        private double _controlHeadingErrorRadians;
        private double _controlDistanceToTrajectoryMeters;
        private double _controlRemainingDistanceMeters;
        private double _curvaturePreviewDistanceMeters;
        private double _feedforwardCurvaturePerMeter;
        private bool _hasVelocityDiagnostics;
        private double _detourEstimatedBodyVxMetersPerSecond;
        private bool _detourVelocityEstimateValid;
        private double _wheelFeedbackRawBodyVxMetersPerSecond;
        private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
        private double _wheelFeedbackRawBodyVyMetersPerSecond;
        private double _wheelFeedbackFilteredBodyVyMetersPerSecond;
        private bool _wheelFeedbackVelocityEstimateValid;
        private bool _hasGcpCommand;
        private double _requestedFrontGcpAngleRadians;
        private double _requestedRearGcpAngleRadians;
        private double _sentFrontGcpAngleRadians;
        private double _sentRearGcpAngleRadians;
        private bool _frontGcpRateLimitActive;
        private bool _rearGcpRateLimitActive;
        private double _commandFrontGcpAngleRadians;
        private double _commandRearGcpAngleRadians;

        public TrackingExperimentRecorder(
            string controllerName,
            string trajectoryName,
            int trialNumber,
            Vector2 referenceStart,
            Vector2 referenceEnd,
            float referenceSpeed,
            float referenceAngularSpeed = 0f,
            int sampleIntervalMs = 50,
            float referenceMotionFrameYawDegrees = 0f,
            float referenceAccelerationMetersPerSecondSquared = 0f,
            float referenceDecelerationMetersPerSecondSquared = 0f,
            MultiWheelChassis diagnosticChassis = null,
            WheelFeedbackVehicleStateProvider
                diagnosticStateProvider = null)
        {
            if (string.IsNullOrWhiteSpace(controllerName))
                throw new ArgumentException(
                    "控制器名称不能为空。",
                    nameof(controllerName));

            if (string.IsNullOrWhiteSpace(trajectoryName))
                throw new ArgumentException(
                    "轨迹名称不能为空。",
                    nameof(trajectoryName));

            if (sampleIntervalMs <= 0)
                throw new ArgumentOutOfRangeException(
                    nameof(sampleIntervalMs),
                    "采样周期必须大于零。");

            _controllerName = controllerName;
            _trajectoryName = trajectoryName;
            _trialNumber = trialNumber;
            _referenceStart = referenceStart;
            _referenceEnd = referenceEnd;
            _referenceSpeed = referenceSpeed;
            _referenceAngularSpeed = referenceAngularSpeed;
            _referenceMotionFrameYawDegrees =
                referenceMotionFrameYawDegrees;
            _referenceAccelerationMetersPerSecondSquared =
                referenceAccelerationMetersPerSecondSquared;
            _referenceDecelerationMetersPerSecondSquared =
                referenceDecelerationMetersPerSecondSquared;
            _sampleIntervalMs = sampleIntervalMs;
            _diagnosticChassis = diagnosticChassis;
            _diagnosticStateProvider = diagnosticStateProvider;
        }

        // 保存成功后的CSV绝对路径；尚未保存时为空。
        public string SavedFilePath { get; private set; }

        /// <summary>
        /// 获取逐控制周期计时CSV的绝对路径；本次实验没有计时数据时为空。
        /// </summary>
        public string SavedTimingFilePath { get; private set; }

        /// <summary>
        /// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。
        /// </summary>
        public static string DefaultOutputDirectory =>
            Path.Combine(
                AppContext.BaseDirectory,
                "TrackingExperiments");

        // 启动后台采样线程。
        public void Start()
        {
            if (Interlocked.Exchange(ref _started, 1) != 0)
                return;

            _stopwatch.Restart();
            _running = true;

            // 立即保存起点静止状态，避免第一帧被后台线程延迟。
            CaptureSample();

            _worker = new Thread(SamplingLoop)
            {
                IsBackground = true,
                Name = "TrackingExperimentRecorder"
            };
            _worker.Start();
        }

        // 供Stanley/LQR控制器主动写入本周期最终速度命令。
        // 调用后优先记录该命令，不再使用底盘反解值。
        public void UpdateCommand(
            float commandSpeed,
            float commandAngularSpeed)
        {
            lock (_commandSyncRoot)
            {
                _externalCommandSpeed = commandSpeed;
                _externalCommandVx = commandSpeed;
                _externalCommandVy = 0f;
                _externalCommandAngularSpeed =
                    commandAngularSpeed;
                _hasExternalCommand = true;
            }
        }

        // 供全向、蟹行和曲线控制器写入完整车体速度命令。
        public void UpdateBodyCommand(
            float commandVx,
            float commandVy,
            float commandAngularSpeed)
        {
            lock (_commandSyncRoot)
            {
                _externalCommandVx = commandVx;
                _externalCommandVy = commandVy;
                _externalCommandSpeed =
                    (float)Math.Sqrt(
                        commandVx * commandVx +
                        commandVy * commandVy);
                _externalCommandAngularSpeed =
                    commandAngularSpeed;
                _hasExternalCommand = true;
            }
        }

        /// <summary>
        /// 将一个真实控制周期的分阶段耗时追加到内存，实验结束后统一保存。
        /// </summary>
        public void RecordControlCycleTiming(
            ParkingControlCycleTiming timing,
            int motionSegmentIndex = -1,
            GcpMotionCommand? requestedCommand = null,
            GcpMotionCommand? sentCommand = null)
        {
            var record = new ControlCycleTimingRecord(
                _stopwatch.Elapsed.TotalSeconds,
                motionSegmentIndex,
                timing,
                requestedCommand,
                sentCommand);

            lock (_controlCycleTimingSyncRoot)
            {
                _controlCycleTimings.Add(record);
            }
        }

        /// <summary>
        /// 保存新版控制器本周期实际使用的校验后车辆状态，供后台采样线程写入CSV。
        /// </summary>
        public void UpdateProcessedState(VehicleState state)
        {
            lock (_stateSyncRoot)
            {
                _latestProcessedState = state;
            }
        }

        /// <summary>
        /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波平面速度。
        /// </summary>
        public void UpdateVelocityDiagnostics(
            double detourEstimatedBodyVxMetersPerSecond,
            bool detourVelocityEstimateValid,
            double wheelFeedbackRawBodyVxMetersPerSecond,
            double wheelFeedbackFilteredBodyVxMetersPerSecond,
            double wheelFeedbackRawBodyVyMetersPerSecond,
            double wheelFeedbackFilteredBodyVyMetersPerSecond,
            bool wheelFeedbackVelocityEstimateValid)
        {
            lock (_stateSyncRoot)
            {
                _detourEstimatedBodyVxMetersPerSecond =
                    detourEstimatedBodyVxMetersPerSecond;
                _detourVelocityEstimateValid =
                    detourVelocityEstimateValid;
                _wheelFeedbackRawBodyVxMetersPerSecond =
                    wheelFeedbackRawBodyVxMetersPerSecond;
                _wheelFeedbackFilteredBodyVxMetersPerSecond =
                    wheelFeedbackFilteredBodyVxMetersPerSecond;
                _wheelFeedbackRawBodyVyMetersPerSecond =
                    wheelFeedbackRawBodyVyMetersPerSecond;
                _wheelFeedbackFilteredBodyVyMetersPerSecond =
                    wheelFeedbackFilteredBodyVyMetersPerSecond;
                _wheelFeedbackVelocityEstimateValid =
                    wheelFeedbackVelocityEstimateValid;
                _hasVelocityDiagnostics = true;
            }
        }

        /// <summary>
        /// 保存本周期经过GCP角速度限制前后的前后虚拟控制点转角。
        /// </summary>
        public void UpdateGcpCommand(
            double requestedFrontGcpAngleRadians,
            double requestedRearGcpAngleRadians,
            double sentFrontGcpAngleRadians,
            double sentRearGcpAngleRadians)
        {
            lock (_stateSyncRoot)
            {
                _requestedFrontGcpAngleRadians =
                    requestedFrontGcpAngleRadians;
                _requestedRearGcpAngleRadians =
                    requestedRearGcpAngleRadians;
                _sentFrontGcpAngleRadians =
                    sentFrontGcpAngleRadians;
                _sentRearGcpAngleRadians =
                    sentRearGcpAngleRadians;
                _frontGcpRateLimitActive =
                    IsGcpRateLimitActive(
                        requestedFrontGcpAngleRadians,
                        sentFrontGcpAngleRadians);
                _rearGcpRateLimitActive =
                    IsGcpRateLimitActive(
                        requestedRearGcpAngleRadians,
                        sentRearGcpAngleRadians);
                // 保留原字段语义，避免既有绘图脚本失效。
                _commandFrontGcpAngleRadians =
                    sentFrontGcpAngleRadians;
                _commandRearGcpAngleRadians =
                    sentRearGcpAngleRadians;
                _hasGcpCommand = true;
            }
        }

        /// <summary>
        /// 清除上一轨迹段的GCP命令，避免停车或原地自转阶段沿用旧角度。
        /// </summary>
        public void ClearGcpCommand()
        {
            lock (_stateSyncRoot)
            {
                _hasGcpCommand = false;
            }
        }

        /// <summary>
        /// 保存新版控制器本周期实际使用的轨迹投影、误差和参考速度。
        /// </summary>
        public void UpdateControlReference(
            double arcLengthMeters,
            double referenceSpeedMetersPerSecond,
            double lateralErrorMeters,
            double headingErrorRadians,
            double distanceToTrajectoryMeters,
            double remainingDistanceMeters,
            double curvaturePreviewDistanceMeters,
            double feedforwardCurvaturePerMeter)
        {
            lock (_stateSyncRoot)
            {
                _controlReferenceArcLengthMeters =
                    arcLengthMeters;
                _controlReferenceSpeedMetersPerSecond =
                    referenceSpeedMetersPerSecond;
                _controlLateralErrorMeters =
                    lateralErrorMeters;
                _controlHeadingErrorRadians =
                    headingErrorRadians;
                _controlDistanceToTrajectoryMeters =
                    distanceToTrajectoryMeters;
                _controlRemainingDistanceMeters =
                    remainingDistanceMeters;
                _curvaturePreviewDistanceMeters =
                    curvaturePreviewDistanceMeters;
                _feedforwardCurvaturePerMeter =
                    feedforwardCurvaturePerMeter;
                _hasControlReference = true;
            }
        }

        /// <summary>
        /// 清除上一轨迹段参考量，避免停车或原地自转期间沿用已经结束的投影结果。
        /// </summary>
        public void ClearControlReference()
        {
            lock (_stateSyncRoot)
            {
                _hasControlReference = false;
            }
        }

        // 停止采样并将本次实验保存为CSV；重复调用只保存一次。
        public void StopAndSave()
        {
            if (Volatile.Read(ref _started) == 0)
                return;

            if (Interlocked.Exchange(ref _saved, 1) != 0)
                return;

            try
            {
                _running = false;

                if (_worker != null &&
                    _worker != Thread.CurrentThread)
                {
                    _worker.Join(
                        Math.Max(1000, _sampleIntervalMs * 4));
                }

                // 保存停止时刻的最后一帧。
                CaptureSample();
                _stopwatch.Stop();
                SaveCsv();
                SaveControlCycleTimingCsv();

                Console.WriteLine(
                    $"轨迹实验数据已保存：{SavedFilePath}");
                if (!string.IsNullOrWhiteSpace(
                        SavedTimingFilePath))
                {
                    Console.WriteLine(
                        "控制周期计时数据已保存：" +
                        SavedTimingFilePath);
                }
            }
            catch
            {
                // 保存失败后允许调用者再次尝试。
                Interlocked.Exchange(ref _saved, 0);
                throw;
            }
        }

        // 按固定周期采集Detour位姿和控制命令。
        private void SamplingLoop()
        {
            while (_running)
            {
                Thread.Sleep(_sampleIntervalMs);

                if (!_running)
                    break;

                CaptureSample();
            }
        }

        // 采集一帧Detour位姿和控制命令。
        private void CaptureSample()
        {
            try
            {
                var location =
                    DetourInterface.getCartLocation();

                var hasDetourStateDiagnostics = false;
                var detourJumpCandidateActive = false;
                var detourJumpCandidateConsistentFrameCount = 0;
                var detourEstimatedShiftDistanceMeters = 0.0;
                var detourEstimatedShiftHeadingRadians = 0.0;
                var detourAutomaticFrameShiftCount = 0;
                var detourStateStatusReason = string.Empty;
                var detourDataAgeMilliseconds = 0.0;
                var detourSourceFrameIntervalSeconds = 0.0;
                var detourMotionPredictionTimestampSeconds = 0.0;
                var hasDetourInnovationDiagnostics = false;
                var detourPositionInnovationMeters = 0.0;
                var detourAllowedPositionInnovationMeters = 0.0;
                var detourHeadingInnovationRadians = 0.0;
                var detourAllowedHeadingInnovationRadians = 0.0;
                var detourLastJumpTriggerReason = string.Empty;
                var detourStateStatus = string.Empty;

                if (_diagnosticStateProvider != null)
                {
                    hasDetourStateDiagnostics =
                        _diagnosticStateProvider
                            .TryGetLatestDetourDiagnostics(
                                out detourJumpCandidateActive,
                                out detourJumpCandidateConsistentFrameCount,
                                out detourEstimatedShiftDistanceMeters,
                                out detourEstimatedShiftHeadingRadians,
                                out detourAutomaticFrameShiftCount,
                                out detourSourceFrameIntervalSeconds,
                                out detourMotionPredictionTimestampSeconds,
                                out hasDetourInnovationDiagnostics,
                                out detourPositionInnovationMeters,
                                out detourAllowedPositionInnovationMeters,
                                out detourHeadingInnovationRadians,
                                out detourAllowedHeadingInnovationRadians,
                                out detourLastJumpTriggerReason,
                                out detourStateStatus,
                                out detourStateStatusReason,
                                out detourDataAgeMilliseconds);
                }

                float commandSpeed;
                float commandVx;
                float commandVy;
                float commandAngularSpeed;
                VehicleState? processedState;
                bool hasControlReference;
                double controlReferenceArcLengthMeters;
                double controlReferenceSpeedMetersPerSecond;
                double controlLateralErrorMeters;
                double controlHeadingErrorRadians;
                double controlDistanceToTrajectoryMeters;
                double controlRemainingDistanceMeters;
                double curvaturePreviewDistanceMeters;
                double feedforwardCurvaturePerMeter;
                bool hasVelocityDiagnostics;
                double detourEstimatedBodyVxMetersPerSecond;
                bool detourVelocityEstimateValid;
                double wheelFeedbackRawBodyVxMetersPerSecond;
                double wheelFeedbackFilteredBodyVxMetersPerSecond;
                double wheelFeedbackRawBodyVyMetersPerSecond;
                double wheelFeedbackFilteredBodyVyMetersPerSecond;
                bool wheelFeedbackVelocityEstimateValid;
                var hasWheelFeedbackAngularVelocityDiagnostics = false;
                var wheelFeedbackRawBodyOmegaRadiansPerSecond = 0.0;
                var wheelFeedbackFilteredBodyOmegaRadiansPerSecond = 0.0;
                var wheelFeedbackSampleTimestampSeconds = 0.0;
                bool hasGcpCommand;
                double requestedFrontGcpAngleRadians;
                double requestedRearGcpAngleRadians;
                double sentFrontGcpAngleRadians;
                double sentRearGcpAngleRadians;
                bool frontGcpRateLimitActive;
                bool rearGcpRateLimitActive;
                double commandFrontGcpAngleRadians;
                double commandRearGcpAngleRadians;

                lock (_commandSyncRoot)
                {
                    if (_hasExternalCommand)
                    {
                        commandSpeed =
                            _externalCommandSpeed;
                        commandVx =
                            _externalCommandVx;
                        commandVy =
                            _externalCommandVy;
                        commandAngularSpeed =
                            _externalCommandAngularSpeed;
                    }
                    else
                    {
                        var command =
                            PilotDefinition.Chassis
                                .GetCarSpeed(false);

                        commandVx = command.Vx;
                        commandVy = command.Vy;
                        // CommonUsage.GetCarSpeed().Vw的单位为deg/s，
                        // 记录器内部统一转换为rad/s。
                        commandAngularSpeed =
                            (float)AngleMath.DegreesToRadians(
                                command.Vw);
                        commandSpeed = (float)Math.Sqrt(
                            commandVx * commandVx +
                            commandVy * commandVy);
                    }
                }

                lock (_stateSyncRoot)
                {
                    processedState =
                        _latestProcessedState;
                    hasControlReference =
                        _hasControlReference;
                    controlReferenceArcLengthMeters =
                        _controlReferenceArcLengthMeters;
                    controlReferenceSpeedMetersPerSecond =
                        _controlReferenceSpeedMetersPerSecond;
                    controlLateralErrorMeters =
                        _controlLateralErrorMeters;
                    controlHeadingErrorRadians =
                        _controlHeadingErrorRadians;
                    controlDistanceToTrajectoryMeters =
                        _controlDistanceToTrajectoryMeters;
                    controlRemainingDistanceMeters =
                        _controlRemainingDistanceMeters;
                    curvaturePreviewDistanceMeters =
                        _curvaturePreviewDistanceMeters;
                    feedforwardCurvaturePerMeter =
                        _feedforwardCurvaturePerMeter;
                    hasVelocityDiagnostics =
                        _hasVelocityDiagnostics;
                    detourEstimatedBodyVxMetersPerSecond =
                        _detourEstimatedBodyVxMetersPerSecond;
                    detourVelocityEstimateValid =
                        _detourVelocityEstimateValid;
                    wheelFeedbackRawBodyVxMetersPerSecond =
                        _wheelFeedbackRawBodyVxMetersPerSecond;
                    wheelFeedbackFilteredBodyVxMetersPerSecond =
                        _wheelFeedbackFilteredBodyVxMetersPerSecond;
                    wheelFeedbackRawBodyVyMetersPerSecond =
                        _wheelFeedbackRawBodyVyMetersPerSecond;
                    wheelFeedbackFilteredBodyVyMetersPerSecond =
                        _wheelFeedbackFilteredBodyVyMetersPerSecond;
                    wheelFeedbackVelocityEstimateValid =
                        _wheelFeedbackVelocityEstimateValid;
                    hasGcpCommand = _hasGcpCommand;
                    requestedFrontGcpAngleRadians =
                        _requestedFrontGcpAngleRadians;
                    requestedRearGcpAngleRadians =
                        _requestedRearGcpAngleRadians;
                    sentFrontGcpAngleRadians =
                        _sentFrontGcpAngleRadians;
                    sentRearGcpAngleRadians =
                        _sentRearGcpAngleRadians;
                    frontGcpRateLimitActive =
                        _frontGcpRateLimitActive;
                    rearGcpRateLimitActive =
                        _rearGcpRateLimitActive;
                    commandFrontGcpAngleRadians =
                        _commandFrontGcpAngleRadians;
                    commandRearGcpAngleRadians =
                        _commandRearGcpAngleRadians;
                }

                // 直接从状态源读取最新完整轮组诊断，使原地自转等没有
                // 控制周期回调的实验也能记录原始/滤波Vw及其采样时间。
                if (_diagnosticStateProvider != null &&
                    _diagnosticStateProvider
                        .TryGetLatestVelocityDiagnostics(
                            out var directDetourBodyVx,
                            out var directDetourVelocityValid,
                            out var directRawWheelBodyVx,
                            out var directFilteredWheelBodyVx,
                            out var directRawWheelBodyVy,
                            out var directFilteredWheelBodyVy,
                            out var directRawWheelBodyOmega,
                            out var directFilteredWheelBodyOmega,
                            out var directWheelSampleTimestampSeconds,
                            out var directWheelVelocityValid))
                {
                    hasVelocityDiagnostics = true;
                    detourEstimatedBodyVxMetersPerSecond =
                        directDetourBodyVx;
                    detourVelocityEstimateValid =
                        directDetourVelocityValid;
                    wheelFeedbackRawBodyVxMetersPerSecond =
                        directRawWheelBodyVx;
                    wheelFeedbackFilteredBodyVxMetersPerSecond =
                        directFilteredWheelBodyVx;
                    wheelFeedbackRawBodyVyMetersPerSecond =
                        directRawWheelBodyVy;
                    wheelFeedbackFilteredBodyVyMetersPerSecond =
                        directFilteredWheelBodyVy;
                    wheelFeedbackRawBodyOmegaRadiansPerSecond =
                        directRawWheelBodyOmega;
                    wheelFeedbackFilteredBodyOmegaRadiansPerSecond =
                        directFilteredWheelBodyOmega;
                    wheelFeedbackSampleTimestampSeconds =
                        directWheelSampleTimestampSeconds;
                    wheelFeedbackVelocityEstimateValid =
                        directWheelVelocityValid;
                    hasWheelFeedbackAngularVelocityDiagnostics = true;
                }

                var sample = new TrackingSample
                {
                    ElapsedSeconds =
                        _stopwatch.Elapsed.TotalSeconds,
                    DetourX = location.x,
                    DetourY = location.y,
                    DetourTheta = location.th,
                    DetourTickRaw = Convert.ToInt64(
                        location.tick,
                        CultureInfo.InvariantCulture),
                    DetourLStep = Convert.ToDouble(
                        location.l_step,
                        CultureInfo.InvariantCulture),
                    HasDetourStateDiagnostics =
                        hasDetourStateDiagnostics,
                    DetourJumpCandidateActive =
                        detourJumpCandidateActive,
                    DetourJumpCandidateConsistentFrameCount =
                        detourJumpCandidateConsistentFrameCount,
                    DetourEstimatedShiftDistanceMeters =
                        detourEstimatedShiftDistanceMeters,
                    DetourEstimatedShiftHeadingRadians =
                        detourEstimatedShiftHeadingRadians,
                    DetourAutomaticFrameShiftCount =
                        detourAutomaticFrameShiftCount,
                    DetourStateStatusReason =
                        detourStateStatusReason,
                    DetourDataAgeMilliseconds =
                        detourDataAgeMilliseconds,
                    DetourSourceFrameIntervalMilliseconds =
                        detourSourceFrameIntervalSeconds * 1000.0,
                    DetourMotionPredictionTimestampSeconds =
                        detourMotionPredictionTimestampSeconds,
                    HasDetourInnovationDiagnostics =
                        hasDetourInnovationDiagnostics,
                    DetourPositionInnovationMeters =
                        detourPositionInnovationMeters,
                    DetourAllowedPositionInnovationMeters =
                        detourAllowedPositionInnovationMeters,
                    DetourHeadingInnovationRadians =
                        detourHeadingInnovationRadians,
                    DetourAllowedHeadingInnovationRadians =
                        detourAllowedHeadingInnovationRadians,
                    DetourLastJumpTriggerReason =
                        detourLastJumpTriggerReason,
                    DetourStateStatus = detourStateStatus,
                    CommandSpeed = commandSpeed,
                    CommandVx = commandVx,
                    CommandVy = commandVy,
                    CommandAngularSpeed =
                        commandAngularSpeed,
                    HasProcessedState =
                        processedState.HasValue,
                    HasControlReference =
                        hasControlReference,
                    ControlReferenceArcLengthMeters =
                        controlReferenceArcLengthMeters,
                    ControlReferenceSpeedMetersPerSecond =
                        controlReferenceSpeedMetersPerSecond,
                    ControlLateralErrorMeters =
                        controlLateralErrorMeters,
                    ControlHeadingErrorRadians =
                        controlHeadingErrorRadians,
                    ControlDistanceToTrajectoryMeters =
                        controlDistanceToTrajectoryMeters,
                    ControlRemainingDistanceMeters =
                        controlRemainingDistanceMeters,
                    CurvaturePreviewDistanceMeters =
                        curvaturePreviewDistanceMeters,
                    FeedforwardCurvaturePerMeter =
                        feedforwardCurvaturePerMeter,
                    HasVelocityDiagnostics =
                        hasVelocityDiagnostics,
                    DetourEstimatedBodyVxMetersPerSecond =
                        detourEstimatedBodyVxMetersPerSecond,
                    DetourVelocityEstimateValid =
                        detourVelocityEstimateValid,
                    WheelFeedbackRawBodyVxMetersPerSecond =
                        wheelFeedbackRawBodyVxMetersPerSecond,
                    WheelFeedbackFilteredBodyVxMetersPerSecond =
                        wheelFeedbackFilteredBodyVxMetersPerSecond,
                    WheelFeedbackRawBodyVyMetersPerSecond =
                        wheelFeedbackRawBodyVyMetersPerSecond,
                    WheelFeedbackFilteredBodyVyMetersPerSecond =
                        wheelFeedbackFilteredBodyVyMetersPerSecond,
                    WheelFeedbackVelocityEstimateValid =
                        wheelFeedbackVelocityEstimateValid,
                    HasWheelFeedbackAngularVelocityDiagnostics =
                        hasWheelFeedbackAngularVelocityDiagnostics,
                    WheelFeedbackRawBodyOmegaRadiansPerSecond =
                        wheelFeedbackRawBodyOmegaRadiansPerSecond,
                    WheelFeedbackFilteredBodyOmegaRadiansPerSecond =
                        wheelFeedbackFilteredBodyOmegaRadiansPerSecond,
                    WheelFeedbackSampleTimestampSeconds =
                        wheelFeedbackSampleTimestampSeconds,
                    HasGcpCommand = hasGcpCommand,
                    RequestedFrontGcpAngleRadians =
                        requestedFrontGcpAngleRadians,
                    RequestedRearGcpAngleRadians =
                        requestedRearGcpAngleRadians,
                    SentFrontGcpAngleRadians =
                        sentFrontGcpAngleRadians,
                    SentRearGcpAngleRadians =
                        sentRearGcpAngleRadians,
                    FrontGcpRateLimitActive =
                        frontGcpRateLimitActive,
                    RearGcpRateLimitActive =
                        rearGcpRateLimitActive,
                    CommandFrontGcpAngleRadians =
                        commandFrontGcpAngleRadians,
                    CommandRearGcpAngleRadians =
                        commandRearGcpAngleRadians
                };

                CaptureSteeringDiagnostics(sample);

                if (processedState.HasValue)
                {
                    var state = processedState.Value;
                    sample.StateTimestampSeconds =
                        state.SampleTimestampSeconds;
                    sample.StateXmeters =
                        state.PoseInWorld.XMeters;
                    sample.StateYMeters =
                        state.PoseInWorld.YMeters;
                    sample.StateYawRadians =
                        state.PoseInWorld.YawRadians;
                    sample.StateWorldVxMetersPerSecond =
                        state.TwistInWorld.VxMetersPerSecond;
                    sample.StateWorldVyMetersPerSecond =
                        state.TwistInWorld.VyMetersPerSecond;
                    sample.StateBodyVxMetersPerSecond =
                        state.TwistInBody.VxMetersPerSecond;
                    sample.StateBodyVyMetersPerSecond =
                        state.TwistInBody.VyMetersPerSecond;
                    sample.StateAngularSpeedRadiansPerSecond =
                        state.TwistInBody.OmegaRadiansPerSecond;
                    sample.StateVelocityEstimateValid =
                        state.HasValidVelocityEstimate;
                }

                lock (_sampleSyncRoot)
                {
                    _samples.Add(sample);
                }
            }
            catch (Exception ex)
            {
                // 单帧读取失败不应终止车辆控制或整个记录线程。
                Console.WriteLine(
                    $"轨迹实验采样失败：{ex.Message}");
            }
        }

        /// <summary>
        /// 按舵轮物理安装位置记录四轮目标角和实际反馈角。
        /// </summary>
        private void CaptureSteeringDiagnostics(
            TrackingSample sample)
        {
            if (_diagnosticChassis == null)
            {
                return;
            }

#pragma warning disable CS0612, CS0618
            var wheels = _diagnosticChassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618

            var leftFront = FindWheel(
                wheels,
                requireFront: true,
                requireLeft: true);
            var leftRear = FindWheel(
                wheels,
                requireFront: false,
                requireLeft: true);
            var rightFront = FindWheel(
                wheels,
                requireFront: true,
                requireLeft: false);
            var rightRear = FindWheel(
                wheels,
                requireFront: false,
                requireLeft: false);

            if (leftFront == null ||
                leftRear == null ||
                rightFront == null ||
                rightRear == null)
            {
                return;
            }

            sample.HasSteeringDiagnostics = true;
            sample.TargetSteerLeftFrontDegrees =
                leftFront.GetSendAngle();
            sample.TargetSteerLeftRearDegrees =
                leftRear.GetSendAngle();
            sample.TargetSteerRightFrontDegrees =
                rightFront.GetSendAngle();
            sample.TargetSteerRightRearDegrees =
                rightRear.GetSendAngle();
            sample.ActualSteerLeftFrontDegrees =
                leftFront.ReadAngle();
            sample.ActualSteerLeftRearDegrees =
                leftRear.ReadAngle();
            sample.ActualSteerRightFrontDegrees =
                rightFront.ReadAngle();
            sample.ActualSteerRightRearDegrees =
                rightRear.ReadAngle();
        }

        /// <summary>
        /// 根据车体X向前、Y向左的物理坐标查找指定象限中的舵轮。
        /// </summary>
        private static SteerWheel FindWheel(
            IReadOnlyList<SteerWheel> wheels,
            bool requireFront,
            bool requireLeft)
        {
            foreach (var wheel in wheels)
            {
                var isFront =
                    wheel.PhysicalPosition.X >= 0f;
                var isLeft =
                    wheel.PhysicalPosition.Y >= 0f;

                if (isFront == requireFront &&
                    isLeft == requireLeft)
                {
                    return wheel;
                }
            }

            return null;
        }

        // 将内存中的采样数据写入CSV。
        private void SaveCsv()
        {
            List<TrackingSample> snapshot;

            lock (_sampleSyncRoot)
            {
                snapshot =
                    new List<TrackingSample>(_samples);
            }

            var outputDirectory =
                DefaultOutputDirectory;

            Directory.CreateDirectory(outputDirectory);

            var fileName =
                $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
                $"{SanitizeFileName(_controllerName)}_" +
                $"{SanitizeFileName(_trajectoryName)}_" +
                $"Trial{_trialNumber}.csv";

            SavedFilePath = Path.Combine(
                outputDirectory,
                fileName);

            using (var writer = new StreamWriter(
                SavedFilePath,
                false,
                new UTF8Encoding(true)))
            {
                writer.WriteLine(
                    "ElapsedSeconds," +
                    "ControllerName," +
                    "TrajectoryName," +
                    "TrialNumber," +
                    "DetourX," +
                    "DetourY," +
                    "DetourTheta," +
                    "DetourTickRaw," +
                    "DetourLStep," +
                    "HasDetourStateDiagnostics," +
                    "DetourJumpCandidateActive," +
                    "DetourJumpCandidateConsistentFrameCount," +
                    "DetourEstimatedShiftDistanceMeters," +
                    "DetourEstimatedShiftHeadingRadians," +
                    "DetourAutomaticFrameShiftCount," +
                    "DetourStateStatusReason," +
                    "DetourDataAgeMilliseconds," +
                    "DetourSourceFrameIntervalMilliseconds," +
                    "DetourMotionPredictionTimestampSeconds," +
                    "HasDetourInnovationDiagnostics," +
                    "DetourPositionInnovationMeters," +
                    "DetourAllowedPositionInnovationMeters," +
                    "DetourHeadingInnovationRadians," +
                    "DetourAllowedHeadingInnovationRadians," +
                    "DetourLastJumpTriggerReason," +
                    "DetourStateStatus," +
                    "CommandSpeed," +
                    // 保留旧列（deg/s）供历史Python脚本兼容。
                    "CommandAngularSpeed," +
                    "CommandAngularSpeedRadPerSecond," +
                    "CommandVx," +
                    "CommandVy," +
                    "ReferenceStartX," +
                    "ReferenceStartY," +
                    "ReferenceEndX," +
                    "ReferenceEndY," +
                    "ReferenceSpeed," +
                    "ReferenceAngularSpeedRadPerSecond," +
                    "ReferenceMotionFrameYawDegrees," +
                    "ReferenceAccelerationMetersPerSecondSquared," +
                    "ReferenceDecelerationMetersPerSecondSquared," +
                    "HasProcessedState," +
                    "StateTimestampSeconds," +
                    "StateXMeters," +
                    "StateYMeters," +
                    "StateYawRadians," +
                    "StateWorldVxMetersPerSecond," +
                    "StateWorldVyMetersPerSecond," +
                    "StateBodyVxMetersPerSecond," +
                    "StateBodyVyMetersPerSecond," +
                    "StateAngularSpeedRadiansPerSecond," +
                    "StateVelocityEstimateValid," +
                    "HasControlReference," +
                    "ControlReferenceArcLengthMeters," +
                    "ControlReferenceSpeedMetersPerSecond," +
                    "ControlLateralErrorMeters," +
                    "ControlHeadingErrorRadians," +
                    "ControlDistanceToTrajectoryMeters," +
                    "ControlRemainingDistanceMeters," +
                    "CurvaturePreviewDistanceMeters," +
                    "FeedforwardCurvaturePerMeter," +
                    "HasVelocityDiagnostics," +
                    "DetourEstimatedBodyVxMetersPerSecond," +
                    "DetourVelocityEstimateValid," +
                    "WheelFeedbackRawBodyVxMetersPerSecond," +
                    "WheelFeedbackFilteredBodyVxMetersPerSecond," +
                    "WheelFeedbackRawBodyVyMetersPerSecond," +
                    "WheelFeedbackFilteredBodyVyMetersPerSecond," +
                    "WheelFeedbackVelocityEstimateValid," +
                    "HasWheelFeedbackAngularVelocityDiagnostics," +
                    "WheelFeedbackRawBodyOmegaRadiansPerSecond," +
                    "WheelFeedbackFilteredBodyOmegaRadiansPerSecond," +
                    "WheelFeedbackSampleTimestampSeconds," +
                    "HasSteeringDiagnostics," +
                    "TargetSteerLeftFrontDegrees," +
                    "TargetSteerLeftRearDegrees," +
                    "TargetSteerRightFrontDegrees," +
                    "TargetSteerRightRearDegrees," +
                    "ActualSteerLeftFrontDegrees," +
                    "ActualSteerLeftRearDegrees," +
                    "ActualSteerRightFrontDegrees," +
                    "ActualSteerRightRearDegrees," +
                    "HasGcpCommand," +
                    "CommandFrontGcpAngleRadians," +
                    "CommandRearGcpAngleRadians," +
                    "RequestedFrontGcpAngleRadians," +
                    "RequestedRearGcpAngleRadians," +
                    "SentFrontGcpAngleRadians," +
                    "SentRearGcpAngleRadians," +
                    "FrontGcpRateLimitActive," +
                    "RearGcpRateLimitActive");

                foreach (var sample in snapshot)
                {
                    writer.WriteLine(string.Join(
                        ",",
                        Format(sample.ElapsedSeconds),
                        EscapeCsv(_controllerName),
                        EscapeCsv(_trajectoryName),
                        _trialNumber.ToString(
                            CultureInfo.InvariantCulture),
                        Format(sample.DetourX),
                        Format(sample.DetourY),
                        Format(sample.DetourTheta),
                        sample.DetourTickRaw.ToString(
                            CultureInfo.InvariantCulture),
                        Format(sample.DetourLStep),
                        sample.HasDetourStateDiagnostics
                            ? "1"
                            : "0",
                        FormatOptionalBoolean(
                            sample.HasDetourStateDiagnostics,
                            sample.DetourJumpCandidateActive),
                        sample.HasDetourStateDiagnostics
                            ? sample
                                .DetourJumpCandidateConsistentFrameCount
                                .ToString(
                                    CultureInfo.InvariantCulture)
                            : string.Empty,
                        FormatOptional(
                            sample.HasDetourStateDiagnostics &&
                            sample.DetourJumpCandidateActive,
                            sample.DetourEstimatedShiftDistanceMeters),
                        FormatOptional(
                            sample.HasDetourStateDiagnostics &&
                            sample.DetourJumpCandidateActive,
                            sample.DetourEstimatedShiftHeadingRadians),
                        sample.HasDetourStateDiagnostics
                            ? sample.DetourAutomaticFrameShiftCount
                                .ToString(
                                    CultureInfo.InvariantCulture)
                            : string.Empty,
                        sample.HasDetourStateDiagnostics
                            ? EscapeCsv(
                                sample.DetourStateStatusReason)
                            : string.Empty,
                        FormatOptional(
                            sample.HasDetourStateDiagnostics,
                            sample.DetourDataAgeMilliseconds),
                        FormatOptional(
                            sample.HasDetourStateDiagnostics,
                            sample.DetourSourceFrameIntervalMilliseconds),
                        FormatOptional(
                            sample.HasDetourStateDiagnostics,
                            sample.DetourMotionPredictionTimestampSeconds),
                        FormatOptionalBoolean(
                            sample.HasDetourStateDiagnostics,
                            sample.HasDetourInnovationDiagnostics),
                        FormatOptional(
                            sample.HasDetourStateDiagnostics &&
                            sample.HasDetourInnovationDiagnostics,
                            sample.DetourPositionInnovationMeters),
                        FormatOptional(
                            sample.HasDetourStateDiagnostics &&
                            sample.HasDetourInnovationDiagnostics,
                            sample.DetourAllowedPositionInnovationMeters),
                        FormatOptional(
                            sample.HasDetourStateDiagnostics &&
                            sample.HasDetourInnovationDiagnostics,
                            sample.DetourHeadingInnovationRadians),
                        FormatOptional(
                            sample.HasDetourStateDiagnostics &&
                            sample.HasDetourInnovationDiagnostics,
                            sample.DetourAllowedHeadingInnovationRadians),
                        sample.HasDetourStateDiagnostics
                            ? EscapeCsv(
                                sample.DetourLastJumpTriggerReason)
                            : string.Empty,
                        sample.HasDetourStateDiagnostics
                            ? EscapeCsv(sample.DetourStateStatus)
                            : string.Empty,
                        Format(sample.CommandSpeed),
                        Format(
                            AngleMath.RadiansToDegrees(
                                sample.CommandAngularSpeed)),
                        Format(sample.CommandAngularSpeed),
                        Format(sample.CommandVx),
                        Format(sample.CommandVy),
                        Format(_referenceStart.X),
                        Format(_referenceStart.Y),
                        Format(_referenceEnd.X),
                        Format(_referenceEnd.Y),
                        Format(_referenceSpeed),
                        Format(_referenceAngularSpeed),
                        Format(_referenceMotionFrameYawDegrees),
                        Format(
                            _referenceAccelerationMetersPerSecondSquared),
                        Format(
                            _referenceDecelerationMetersPerSecondSquared),
                        sample.HasProcessedState
                            ? "1"
                            : "0",
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateTimestampSeconds),
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateXmeters),
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateYMeters),
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateYawRadians),
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateWorldVxMetersPerSecond),
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateWorldVyMetersPerSecond),
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateBodyVxMetersPerSecond),
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateBodyVyMetersPerSecond),
                        FormatOptional(
                            sample.HasProcessedState,
                            sample.StateAngularSpeedRadiansPerSecond),
                        sample.HasProcessedState
                            ? sample.StateVelocityEstimateValid
                                ? "1"
                                : "0"
                            : string.Empty,
                        sample.HasControlReference
                            ? "1"
                            : "0",
                        FormatOptional(
                            sample.HasControlReference,
                            sample.ControlReferenceArcLengthMeters),
                        FormatOptional(
                            sample.HasControlReference,
                            sample.ControlReferenceSpeedMetersPerSecond),
                        FormatOptional(
                            sample.HasControlReference,
                            sample.ControlLateralErrorMeters),
                        FormatOptional(
                            sample.HasControlReference,
                            sample.ControlHeadingErrorRadians),
                        FormatOptional(
                            sample.HasControlReference,
                            sample.ControlDistanceToTrajectoryMeters),
                        FormatOptional(
                            sample.HasControlReference,
                            sample.ControlRemainingDistanceMeters),
                        FormatOptional(
                            sample.HasControlReference,
                            sample.CurvaturePreviewDistanceMeters),
                        FormatOptional(
                            sample.HasControlReference,
                            sample.FeedforwardCurvaturePerMeter),
                        sample.HasVelocityDiagnostics
                            ? "1"
                            : "0",
                        FormatOptional(
                            sample.HasVelocityDiagnostics,
                            sample.DetourEstimatedBodyVxMetersPerSecond),
                        sample.HasVelocityDiagnostics
                            ? sample.DetourVelocityEstimateValid
                                ? "1"
                                : "0"
                            : string.Empty,
                        FormatOptional(
                            sample.HasVelocityDiagnostics,
                            sample.WheelFeedbackRawBodyVxMetersPerSecond),
                        FormatOptional(
                            sample.HasVelocityDiagnostics,
                            sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
                        FormatOptional(
                            sample.HasVelocityDiagnostics,
                            sample.WheelFeedbackRawBodyVyMetersPerSecond),
                        FormatOptional(
                            sample.HasVelocityDiagnostics,
                            sample.WheelFeedbackFilteredBodyVyMetersPerSecond),
                        sample.HasVelocityDiagnostics
                            ? sample.WheelFeedbackVelocityEstimateValid
                                ? "1"
                                : "0"
                            : string.Empty,
                        FormatOptionalBoolean(
                            sample.HasVelocityDiagnostics,
                            sample.HasWheelFeedbackAngularVelocityDiagnostics),
                        FormatOptional(
                            sample.HasVelocityDiagnostics &&
                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
                            sample.WheelFeedbackRawBodyOmegaRadiansPerSecond),
                        FormatOptional(
                            sample.HasVelocityDiagnostics &&
                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
                            sample.WheelFeedbackFilteredBodyOmegaRadiansPerSecond),
                        FormatOptional(
                            sample.HasVelocityDiagnostics &&
                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
                            sample.WheelFeedbackSampleTimestampSeconds),
                        sample.HasSteeringDiagnostics
                            ? "1"
                            : "0",
                        FormatOptional(
                            sample.HasSteeringDiagnostics,
                            sample.TargetSteerLeftFrontDegrees),
                        FormatOptional(
                            sample.HasSteeringDiagnostics,
                            sample.TargetSteerLeftRearDegrees),
                        FormatOptional(
                            sample.HasSteeringDiagnostics,
                            sample.TargetSteerRightFrontDegrees),
                        FormatOptional(
                            sample.HasSteeringDiagnostics,
                            sample.TargetSteerRightRearDegrees),
                        FormatOptional(
                            sample.HasSteeringDiagnostics,
                            sample.ActualSteerLeftFrontDegrees),
                        FormatOptional(
                            sample.HasSteeringDiagnostics,
                            sample.ActualSteerLeftRearDegrees),
                        FormatOptional(
                            sample.HasSteeringDiagnostics,
                            sample.ActualSteerRightFrontDegrees),
                        FormatOptional(
                            sample.HasSteeringDiagnostics,
                            sample.ActualSteerRightRearDegrees),
                        sample.HasGcpCommand
                            ? "1"
                            : "0",
                        FormatOptional(
                            sample.HasGcpCommand,
                            sample.CommandFrontGcpAngleRadians),
                        FormatOptional(
                            sample.HasGcpCommand,
                            sample.CommandRearGcpAngleRadians),
                        FormatOptional(
                            sample.HasGcpCommand,
                            sample.RequestedFrontGcpAngleRadians),
                        FormatOptional(
                            sample.HasGcpCommand,
                            sample.RequestedRearGcpAngleRadians),
                        FormatOptional(
                            sample.HasGcpCommand,
                            sample.SentFrontGcpAngleRadians),
                        FormatOptional(
                            sample.HasGcpCommand,
                            sample.SentRearGcpAngleRadians),
                        FormatOptionalBoolean(
                            sample.HasGcpCommand,
                            sample.FrontGcpRateLimitActive),
                        FormatOptionalBoolean(
                            sample.HasGcpCommand,
                            sample.RearGcpRateLimitActive)));
                }
            }
        }

        /// <summary>
        /// 将逐控制周期的内存计时数据保存为独立CSV，不受后台采样周期限制。
        /// </summary>
        private void SaveControlCycleTimingCsv()
        {
            List<ControlCycleTimingRecord> snapshot;

            lock (_controlCycleTimingSyncRoot)
            {
                snapshot =
                    new List<ControlCycleTimingRecord>(
                        _controlCycleTimings);
            }

            if (snapshot.Count == 0)
            {
                SavedTimingFilePath = null;
                return;
            }

            SavedTimingFilePath = Path.Combine(
                Path.GetDirectoryName(SavedFilePath) ??
                    DefaultOutputDirectory,
                Path.GetFileNameWithoutExtension(
                    SavedFilePath) +
                "_timing.csv");

            using (var writer = new StreamWriter(
                SavedTimingFilePath,
                false,
                new UTF8Encoding(true)))
            {
                writer.WriteLine(
                    "ControlCycleEndElapsedSeconds," +
                    "ControllerName," +
                    "TrajectoryName," +
                    "TrialNumber," +
                    "MotionSegmentIndex," +
                    "ControlCycleIndex," +
                    "ControlCycleIntervalMs," +
                    "StateReadMs," +
                    "ProjectionMs," +
                    "ControllerComputeMs," +
                    "CommandSendMs," +
                    "OtherMs," +
                    "TotalCycleMs," +
                    "HasStateTimestamp," +
                    "StateTimestampSeconds," +
                    "StateTimestampChanged," +
                    "CycleResult," +
                    "HasGcpCommand," +
                    "RequestedFrontGcpAngleRadians," +
                    "RequestedRearGcpAngleRadians," +
                    "SentFrontGcpAngleRadians," +
                    "SentRearGcpAngleRadians," +
                    "FrontGcpRateLimitActive," +
                    "RearGcpRateLimitActive");

                foreach (var record in snapshot)
                {
                    var timing = record.Timing;
                    var measuredStageMilliseconds =
                        timing.StateReadMilliseconds +
                        timing.ProjectionMilliseconds +
                        timing.ControllerComputeMilliseconds +
                        timing.CommandSendMilliseconds;
                    var otherMilliseconds = Math.Max(
                        0.0,
                        timing.TotalCycleMilliseconds -
                        measuredStageMilliseconds);
                    var hasGcpCommand =
                        record.RequestedCommand.HasValue &&
                        record.SentCommand.HasValue;
                    var requestedCommand =
                        record.RequestedCommand.GetValueOrDefault();
                    var sentCommand =
                        record.SentCommand.GetValueOrDefault();
                    var frontRateLimitActive =
                        hasGcpCommand &&
                        IsGcpRateLimitActive(
                            requestedCommand.FrontAngleRadians,
                            sentCommand.FrontAngleRadians);
                    var rearRateLimitActive =
                        hasGcpCommand &&
                        IsGcpRateLimitActive(
                            requestedCommand.RearAngleRadians,
                            sentCommand.RearAngleRadians);

                    writer.WriteLine(string.Join(
                        ",",
                        Format(record.RecorderElapsedSeconds),
                        EscapeCsv(_controllerName),
                        EscapeCsv(_trajectoryName),
                        _trialNumber.ToString(
                            CultureInfo.InvariantCulture),
                        record.MotionSegmentIndex.ToString(
                            CultureInfo.InvariantCulture),
                        timing.CycleIndex.ToString(
                            CultureInfo.InvariantCulture),
                        Format(
                            timing.CycleIntervalMilliseconds),
                        Format(timing.StateReadMilliseconds),
                        Format(timing.ProjectionMilliseconds),
                        Format(
                            timing.ControllerComputeMilliseconds),
                        Format(timing.CommandSendMilliseconds),
                        Format(otherMilliseconds),
                        Format(timing.TotalCycleMilliseconds),
                        timing.HasStateTimestamp
                            ? "1"
                            : "0",
                        FormatOptional(
                            timing.HasStateTimestamp,
                            timing.StateTimestampSeconds),
                        timing.HasStateTimestamp
                            ? timing.StateTimestampChanged
                                ? "1"
                                : "0"
                            : string.Empty,
                        EscapeCsv(timing.Result.ToString()),
                        hasGcpCommand
                            ? "1"
                            : "0",
                        FormatOptional(
                            hasGcpCommand,
                            requestedCommand.FrontAngleRadians),
                        FormatOptional(
                            hasGcpCommand,
                            requestedCommand.RearAngleRadians),
                        FormatOptional(
                            hasGcpCommand,
                            sentCommand.FrontAngleRadians),
                        FormatOptional(
                            hasGcpCommand,
                            sentCommand.RearAngleRadians),
                        FormatOptionalBoolean(
                            hasGcpCommand,
                            frontRateLimitActive),
                        FormatOptionalBoolean(
                            hasGcpCommand,
                            rearRateLimitActive)));
                }
            }
        }

        // 将文件名中的非法字符替换为下划线。
        private static string SanitizeFileName(string value)
        {
            var result = value;

            foreach (var invalidCharacter in
                     Path.GetInvalidFileNameChars())
            {
                result = result.Replace(
                    invalidCharacter,
                    '_');
            }

            return result;
        }

        // 按固定小数格式输出数值，避免系统区域设置改变CSV格式。
        private static string Format(double value)
        {
            return value.ToString(
                "0.######",
                CultureInfo.InvariantCulture);
        }

        /// <summary>
        /// 在新版状态尚未产生时为空，否则按统一小数格式输出状态数值。
        /// </summary>
        private static string FormatOptional(
            bool hasValue,
            double value)
        {
            return hasValue
                ? Format(value)
                : string.Empty;
        }

        /// <summary>
        /// 在GCP命令有效时将布尔诊断输出为0或1，否则保持CSV空值。
        /// </summary>
        private static string FormatOptionalBoolean(
            bool hasValue,
            bool value)
        {
            return hasValue
                ? value
                    ? "1"
                    : "0"
                : string.Empty;
        }

        /// <summary>
        /// 判断GCP角速度限制是否实质改变了控制器请求角度。
        /// </summary>
        private static bool IsGcpRateLimitActive(
            double requestedAngleRadians,
            double sentAngleRadians)
        {
            return Math.Abs(
                       requestedAngleRadians -
                       sentAngleRadians) >
                   GcpAngleComparisonToleranceRadians;
        }

        // 对CSV文本字段进行引号和逗号转义。
        private static string EscapeCsv(string value)
        {
            if (value == null)
                return string.Empty;

            if (!value.Contains(",") &&
                !value.Contains("\"") &&
                !value.Contains("\r") &&
                !value.Contains("\n"))
            {
                return value;
            }

            return
                "\"" +
                value.Replace("\"", "\"\"") +
                "\"";
        }
    }
}