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;

        // 车体速度单位为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的数据来源产生歧义。
        public bool HasVelocityDiagnostics;
        public double DetourEstimatedBodyVxMetersPerSecond;
        public bool DetourVelocityEstimateValid;
        public double WheelFeedbackRawBodyVxMetersPerSecond;
        public double WheelFeedbackFilteredBodyVxMetersPerSecond;
        public bool WheelFeedbackVelocityEstimateValid;

        // 四舵轮机械角使用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 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 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)
        {
            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;
        }

        // 保存成功后的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,
            bool wheelFeedbackVelocityEstimateValid)
        {
            lock (_stateSyncRoot)
            {
                _detourEstimatedBodyVxMetersPerSecond =
                    detourEstimatedBodyVxMetersPerSecond;
                _detourVelocityEstimateValid =
                    detourVelocityEstimateValid;
                _wheelFeedbackRawBodyVxMetersPerSecond =
                    wheelFeedbackRawBodyVxMetersPerSecond;
                _wheelFeedbackFilteredBodyVxMetersPerSecond =
                    wheelFeedbackFilteredBodyVxMetersPerSecond;
                _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();

                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;
                bool wheelFeedbackVelocityEstimateValid;
                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;
                    wheelFeedbackVelocityEstimateValid =
                        _wheelFeedbackVelocityEstimateValid;
                    hasGcpCommand = _hasGcpCommand;
                    requestedFrontGcpAngleRadians =
                        _requestedFrontGcpAngleRadians;
                    requestedRearGcpAngleRadians =
                        _requestedRearGcpAngleRadians;
                    sentFrontGcpAngleRadians =
                        _sentFrontGcpAngleRadians;
                    sentRearGcpAngleRadians =
                        _sentRearGcpAngleRadians;
                    frontGcpRateLimitActive =
                        _frontGcpRateLimitActive;
                    rearGcpRateLimitActive =
                        _rearGcpRateLimitActive;
                    commandFrontGcpAngleRadians =
                        _commandFrontGcpAngleRadians;
                    commandRearGcpAngleRadians =
                        _commandRearGcpAngleRadians;
                }

                var sample = new TrackingSample
                {
                    ElapsedSeconds =
                        _stopwatch.Elapsed.TotalSeconds,
                    DetourX = location.x,
                    DetourY = location.y,
                    DetourTheta = location.th,
                    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,
                    WheelFeedbackVelocityEstimateValid =
                        wheelFeedbackVelocityEstimateValid,
                    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," +
                    "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," +
                    "WheelFeedbackVelocityEstimateValid," +
                    "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),
                        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),
                        sample.HasVelocityDiagnostics
                            ? sample.WheelFeedbackVelocityEstimateValid
                                ? "1"
                                : "0"
                            : string.Empty,
                        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("\"", "\"\"") +
                "\"";
        }
    }
}