using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.Text;
using System.Threading;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using FundamentalLib;
using MyParking.Shared;

namespace MultiWheelC
{
    /// <summary>
    /// 从C层测试界面启动定时的Detour静态定位与轮组反馈诊断记录。
    /// </summary>
    [MovementTest(name = "诊断：Detour静态定位记录")]
    public sealed class DetourStaticDiagnosticTest : MovementTest
    {
        private sealed class DiagnosticSample
        {
            public double ElapsedSeconds;
            public string LocalTimestamp;
            public bool DetourReadSucceeded;
            public double DetourCallDurationMilliseconds;
            public long? DetourTickRaw;
            public string DetourTimestamp;
            public bool DetourTimestampValid;
            public double? DetourDataAgeMilliseconds;
            public double? DetourLStep;
            public double? DetourXMillimeters;
            public double? DetourYMillimeters;
            public double? DetourYawDegrees;
            public double? LocalDeltaMilliseconds;
            public double? DetourTickDeltaMilliseconds;
            public double? DeltaXMillimeters;
            public double? DeltaYMillimeters;
            public double? DeltaYawDegrees;
            public double? DeltaPositionMillimeters;
            public bool IsRepeatedTick;
            public bool IsRepeatedPose;
            public bool IsOutOfOrderTick;
            public bool WheelReadSucceeded;
            public double? WheelBodyVxMetersPerSecond;
            public double? WheelBodyVyMetersPerSecond;
            public double? WheelBodyOmegaDegreesPerSecond;
            public double? ActualSteerLeftFrontDegrees;
            public double? ActualSteerLeftRearDegrees;
            public double? ActualSteerRightFrontDegrees;
            public double? ActualSteerRightRearDegrees;
            public double? ActualSpeedLeftFrontMetersPerSecond;
            public double? ActualSpeedLeftRearMetersPerSecond;
            public double? ActualSpeedRightFrontMetersPerSecond;
            public double? ActualSpeedRightRearMetersPerSecond;
            public string FailureReason;
        }

        private readonly object _sampleSyncRoot = new object();
        private readonly List<DiagnosticSample> _samples =
            new List<DiagnosticSample>();
        private readonly Stopwatch _clock = new Stopwatch();

        private MultiWheelChassis _chassis;
        private Thread _samplingThread;
        private volatile bool _sampling;
        private int _testRunning;
        private int _stopRequested;
        private int _sessionId;
        private bool _hasPreviousDetourSample;
        private double _previousElapsedSeconds;
        private long _previousDetourTick;
        private double _previousDetourXMillimeters;
        private double _previousDetourYMillimeters;
        private double _previousDetourYawDegrees;

        /// <summary>
        /// 获取或设置自动结束前的记录时长，单位为min。
        /// </summary>
        public double DurationMinutes = 20.0;

        /// <summary>
        /// 获取或设置本机主动读取Detour的周期，单位为ms。
        /// </summary>
        public int SampleIntervalMilliseconds = 50;

        /// <summary>
        /// 获取最近一次静态诊断CSV的完整路径。
        /// </summary>
        public string SavedFilePath { get; private set; } =
            string.Empty;

        /// <summary>
        /// 停车后开始静态采样，并在到达设定时长时自动保存CSV。
        /// </summary>
        public override void Test()
        {
            if (Interlocked.CompareExchange(
                    ref _testRunning,
                    1,
                    0) != 0)
            {
                Console.WriteLine("Detour静态诊断已经在运行。");
                return;
            }

            var samplingStarted = false;
            var completedAutomatically = false;
            Exception testFailure = null;

            try
            {
                ValidateSettings();
                _chassis =
                    PilotDefinition.Chassis as MultiWheelChassis;
                if (_chassis == null)
                {
                    throw new InvalidOperationException(
                        "当前底盘不是MultiWheelChassis，无法读取四轮反馈。");
                }

                var sessionId = ResetSession();
                _chassis.PredefinedDriveStop();
                _sampling = true;
                _clock.Restart();
                samplingStarted = true;

                _samplingThread = new Thread(
                    () => SamplingLoop(sessionId))
                {
                    IsBackground = true,
                    Name = "DetourStaticDiagnostic"
                };
                _samplingThread.Start();

                Console.WriteLine(
                    $"Detour静态诊断开始：时长={DurationMinutes:F1}min，" +
                    $"主动读取周期={SampleIntervalMilliseconds}ms，" +
                    "车辆必须保持静止。");
                Hedingben.ToastText(
                    $"Detour静态诊断开始，预计{DurationMinutes:F1}分钟后自动结束。");

                var durationSeconds = DurationMinutes * 60.0;
                while (Volatile.Read(ref _stopRequested) == 0 &&
                       _clock.Elapsed.TotalSeconds < durationSeconds)
                {
                    Thread.Sleep(100);
                }

                completedAutomatically =
                    Volatile.Read(ref _stopRequested) == 0;
            }
            catch (Exception exception)
            {
                testFailure = exception;
                Console.WriteLine(
                    "Detour静态诊断失败：" +
                    exception.Message);
            }
            finally
            {
                _sampling = false;
                _chassis?.PredefinedDriveStop();

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

                _clock.Stop();

                if (samplingStarted)
                {
                    try
                    {
                        SaveCsvAndReport(
                            completedAutomatically,
                            testFailure);
                    }
                    catch (Exception exception)
                    {
                        Console.WriteLine(
                            "Detour静态诊断CSV保存失败：" +
                            exception.Message);
                        Hedingben.ToastText(
                            "Detour静态诊断CSV保存失败：" +
                            exception.Message);
                    }
                }

                _samplingThread = null;
                _chassis = null;
                Interlocked.Exchange(ref _testRunning, 0);
            }
        }

        /// <summary>
        /// 请求提前停止采样；测试线程随后保存已经采集的数据。
        /// </summary>
        public override void TestStop()
        {
            Interlocked.Exchange(ref _stopRequested, 1);
            _sampling = false;
            _chassis?.PredefinedDriveStop();
        }

        /// <summary>
        /// 清除上一次测试的样本、时间基准和输出路径。
        /// </summary>
        private int ResetSession()
        {
            lock (_sampleSyncRoot)
            {
                _samples.Clear();
            }

            Interlocked.Exchange(ref _stopRequested, 0);
            _hasPreviousDetourSample = false;
            _previousElapsedSeconds = 0.0;
            _previousDetourTick = 0;
            _previousDetourXMillimeters = 0.0;
            _previousDetourYMillimeters = 0.0;
            _previousDetourYawDegrees = 0.0;
            SavedFilePath = string.Empty;
            return Interlocked.Increment(ref _sessionId);
        }

        /// <summary>
        /// 检查测试时长和主动采样周期是否适合执行。
        /// </summary>
        private void ValidateSettings()
        {
            NumericGuard.EnsureFinitePositive(
                DurationMinutes,
                nameof(DurationMinutes));

            if (SampleIntervalMilliseconds < 20 ||
                SampleIntervalMilliseconds > 5000)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(SampleIntervalMilliseconds),
                    "Detour主动读取周期必须在20ms到5000ms之间。");
            }
        }

        /// <summary>
        /// 按设定周期持续采样，直至测试到时或收到停止请求。
        /// </summary>
        private void SamplingLoop(int sessionId)
        {
            while (_sampling &&
                   sessionId == Volatile.Read(ref _sessionId))
            {
                CaptureSample(sessionId);
                Thread.Sleep(SampleIntervalMilliseconds);
            }
        }

        /// <summary>
        /// 采集一帧原始Detour定位、接口耗时和四轮实际反馈。
        /// </summary>
        private void CaptureSample(int sessionId)
        {
            var sample = new DiagnosticSample
            {
                ElapsedSeconds = _clock.Elapsed.TotalSeconds,
                LocalTimestamp =
                    DateTimeOffset.Now.ToString(
                        "O",
                        CultureInfo.InvariantCulture),
                FailureReason = string.Empty
            };

            CaptureDetour(sample, sessionId);

            if (sessionId != Volatile.Read(ref _sessionId))
            {
                return;
            }

            CaptureWheelFeedback(sample);

            if (!_sampling ||
                sessionId != Volatile.Read(ref _sessionId))
            {
                return;
            }

            lock (_sampleSyncRoot)
            {
                _samples.Add(sample);
            }
        }

        /// <summary>
        /// 读取Detour原始字段并计算与上一成功读取之间的时间和位姿差。
        /// </summary>
        private void CaptureDetour(
            DiagnosticSample sample,
            int sessionId)
        {
            var callClock = Stopwatch.StartNew();

            try
            {
                var location =
                    DetourInterface.getCartLocation();
                callClock.Stop();

                sample.DetourCallDurationMilliseconds =
                    callClock.Elapsed.TotalMilliseconds;
                sample.DetourReadSucceeded = true;
                sample.DetourTickRaw = Convert.ToInt64(
                    location.tick,
                    CultureInfo.InvariantCulture);
                sample.DetourLStep = Convert.ToDouble(
                    location.l_step,
                    CultureInfo.InvariantCulture);
                sample.DetourXMillimeters = Convert.ToDouble(
                    location.x,
                    CultureInfo.InvariantCulture);
                sample.DetourYMillimeters = Convert.ToDouble(
                    location.y,
                    CultureInfo.InvariantCulture);
                sample.DetourYawDegrees = Convert.ToDouble(
                    location.th,
                    CultureInfo.InvariantCulture);

                if (sessionId != Volatile.Read(ref _sessionId))
                {
                    return;
                }

                CaptureDetourTimestamp(sample);
                CaptureDetourDelta(sample);
            }
            catch (Exception exception)
            {
                callClock.Stop();
                sample.DetourCallDurationMilliseconds =
                    callClock.Elapsed.TotalMilliseconds;
                AppendFailure(
                    sample,
                    "Detour读取失败：" +
                    exception.Message);
            }
        }

        /// <summary>
        /// 将Detour原始tick按.NET DateTime ticks解释并记录数据年龄。
        /// </summary>
        private static void CaptureDetourTimestamp(
            DiagnosticSample sample)
        {
            try
            {
                var detourTime = new DateTime(
                    sample.DetourTickRaw.Value,
                    DateTimeKind.Local);
                sample.DetourTimestamp =
                    detourTime.ToString(
                        "O",
                        CultureInfo.InvariantCulture);
                sample.DetourDataAgeMilliseconds =
                    (DateTime.Now - detourTime)
                    .TotalMilliseconds;
                sample.DetourTimestampValid = true;
            }
            catch (ArgumentOutOfRangeException)
            {
                sample.DetourTimestamp = string.Empty;
            }
        }

        /// <summary>
        /// 计算Detour帧间差并更新下一帧使用的原始基准。
        /// </summary>
        private void CaptureDetourDelta(
            DiagnosticSample sample)
        {
            var tick = sample.DetourTickRaw.Value;
            var xMillimeters = sample.DetourXMillimeters.Value;
            var yMillimeters = sample.DetourYMillimeters.Value;
            var yawDegrees = sample.DetourYawDegrees.Value;

            if (_hasPreviousDetourSample)
            {
                sample.LocalDeltaMilliseconds =
                    (sample.ElapsedSeconds -
                     _previousElapsedSeconds) * 1000.0;
                sample.DetourTickDeltaMilliseconds =
                    (tick - _previousDetourTick) /
                    (double)TimeSpan.TicksPerMillisecond;
                sample.DeltaXMillimeters =
                    xMillimeters - _previousDetourXMillimeters;
                sample.DeltaYMillimeters =
                    yMillimeters - _previousDetourYMillimeters;
                sample.DeltaYawDegrees =
                    AngleMath.ShortestDifferenceDegrees(
                        yawDegrees,
                        _previousDetourYawDegrees);
                sample.DeltaPositionMillimeters = Math.Sqrt(
                    sample.DeltaXMillimeters.Value *
                    sample.DeltaXMillimeters.Value +
                    sample.DeltaYMillimeters.Value *
                    sample.DeltaYMillimeters.Value);
                sample.IsRepeatedTick =
                    tick == _previousDetourTick;
                sample.IsOutOfOrderTick =
                    tick < _previousDetourTick;
                sample.IsRepeatedPose =
                    sample.DeltaPositionMillimeters.Value <= 1e-6 &&
                    Math.Abs(sample.DeltaYawDegrees.Value) <= 1e-9;
            }

            _hasPreviousDetourSample = true;
            _previousElapsedSeconds = sample.ElapsedSeconds;
            _previousDetourTick = tick;
            _previousDetourXMillimeters = xMillimeters;
            _previousDetourYMillimeters = yMillimeters;
            _previousDetourYawDegrees = yawDegrees;
        }

        /// <summary>
        /// 读取底盘反算速度与按物理安装位置识别的四轮实际反馈。
        /// </summary>
        private void CaptureWheelFeedback(
            DiagnosticSample sample)
        {
            try
            {
                var carSpeed = _chassis.GetCarSpeed(true);
                sample.WheelBodyVxMetersPerSecond = carSpeed.Vx;
                sample.WheelBodyVyMetersPerSecond = carSpeed.Vy;
                // CommonUsage的CarSpeed.Vw以deg/s表达。
                sample.WheelBodyOmegaDegreesPerSecond = carSpeed.Vw;

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

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

                if (leftFront == null || leftRear == null ||
                    rightFront == null || rightRear == null)
                {
                    throw new InvalidOperationException(
                        "未能按物理安装位置识别四个舵轮。");
                }

                sample.ActualSteerLeftFrontDegrees =
                    leftFront.ReadAngle();
                sample.ActualSteerLeftRearDegrees =
                    leftRear.ReadAngle();
                sample.ActualSteerRightFrontDegrees =
                    rightFront.ReadAngle();
                sample.ActualSteerRightRearDegrees =
                    rightRear.ReadAngle();
                sample.ActualSpeedLeftFrontMetersPerSecond =
                    leftFront.ReadSpeed();
                sample.ActualSpeedLeftRearMetersPerSecond =
                    leftRear.ReadSpeed();
                sample.ActualSpeedRightFrontMetersPerSecond =
                    rightFront.ReadSpeed();
                sample.ActualSpeedRightRearMetersPerSecond =
                    rightRear.ReadSpeed();
                sample.WheelReadSucceeded = true;
            }
            catch (Exception exception)
            {
                AppendFailure(
                    sample,
                    "四轮反馈读取失败：" +
                    exception.Message);
            }
        }

        /// <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;
        }

        /// <summary>
        /// 追加本帧诊断失败原因且保留先前错误信息。
        /// </summary>
        private static void AppendFailure(
            DiagnosticSample sample,
            string reason)
        {
            sample.FailureReason =
                string.IsNullOrWhiteSpace(sample.FailureReason)
                    ? reason
                    : sample.FailureReason + "；" + reason;
        }

        /// <summary>
        /// 保存采样快照并输出自动结束或手动停止后的摘要提示。
        /// </summary>
        private void SaveCsvAndReport(
            bool completedAutomatically,
            Exception testFailure)
        {
            List<DiagnosticSample> snapshot;
            lock (_sampleSyncRoot)
            {
                snapshot =
                    new List<DiagnosticSample>(_samples);
            }

            var outputDirectory = Path.Combine(
                AppContext.BaseDirectory,
                "DetourStaticDiagnostics");
            Directory.CreateDirectory(outputDirectory);
            SavedFilePath = Path.Combine(
                outputDirectory,
                $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
                "DetourStaticDiagnostic.csv");

            using (var writer = new StreamWriter(
                       SavedFilePath,
                       false,
                       new UTF8Encoding(true)))
            {
                WriteCsvRow(writer,
                    "ElapsedSeconds", "LocalTimestamp",
                    "DetourReadSucceeded", "DetourCallDurationMilliseconds",
                    "DetourTickRaw", "DetourTimestamp",
                    "DetourTimestampValid", "DetourDataAgeMilliseconds",
                    "DetourLStep", "DetourXMillimeters",
                    "DetourYMillimeters", "DetourYawDegrees",
                    "LocalDeltaMilliseconds", "DetourTickDeltaMilliseconds",
                    "DeltaXMillimeters", "DeltaYMillimeters",
                    "DeltaYawDegrees", "DeltaPositionMillimeters",
                    "IsRepeatedTick", "IsRepeatedPose", "IsOutOfOrderTick",
                    "WheelReadSucceeded", "WheelBodyVxMetersPerSecond",
                    "WheelBodyVyMetersPerSecond",
                    "WheelBodyOmegaDegreesPerSecond",
                    "ActualSteerLeftFrontDegrees",
                    "ActualSteerLeftRearDegrees",
                    "ActualSteerRightFrontDegrees",
                    "ActualSteerRightRearDegrees",
                    "ActualSpeedLeftFrontMetersPerSecond",
                    "ActualSpeedLeftRearMetersPerSecond",
                    "ActualSpeedRightFrontMetersPerSecond",
                    "ActualSpeedRightRearMetersPerSecond",
                    "FailureReason");

                foreach (var sample in snapshot)
                {
                    WriteCsvRow(writer,
                        sample.ElapsedSeconds, sample.LocalTimestamp,
                        sample.DetourReadSucceeded,
                        sample.DetourCallDurationMilliseconds,
                        sample.DetourTickRaw, sample.DetourTimestamp,
                        sample.DetourTimestampValid,
                        sample.DetourDataAgeMilliseconds,
                        sample.DetourLStep, sample.DetourXMillimeters,
                        sample.DetourYMillimeters, sample.DetourYawDegrees,
                        sample.LocalDeltaMilliseconds,
                        sample.DetourTickDeltaMilliseconds,
                        sample.DeltaXMillimeters, sample.DeltaYMillimeters,
                        sample.DeltaYawDegrees,
                        sample.DeltaPositionMillimeters,
                        sample.IsRepeatedTick, sample.IsRepeatedPose,
                        sample.IsOutOfOrderTick,
                        sample.WheelReadSucceeded,
                        sample.WheelBodyVxMetersPerSecond,
                        sample.WheelBodyVyMetersPerSecond,
                        sample.WheelBodyOmegaDegreesPerSecond,
                        sample.ActualSteerLeftFrontDegrees,
                        sample.ActualSteerLeftRearDegrees,
                        sample.ActualSteerRightFrontDegrees,
                        sample.ActualSteerRightRearDegrees,
                        sample.ActualSpeedLeftFrontMetersPerSecond,
                        sample.ActualSpeedLeftRearMetersPerSecond,
                        sample.ActualSpeedRightFrontMetersPerSecond,
                        sample.ActualSpeedRightRearMetersPerSecond,
                        sample.FailureReason);
                }
            }

            var successfulSamples = 0;
            var maximumPositionStepMillimeters = 0.0;
            var maximumAbsoluteYawStepDegrees = 0.0;
            foreach (var sample in snapshot)
            {
                if (sample.DetourReadSucceeded)
                {
                    successfulSamples++;
                }

                maximumPositionStepMillimeters = Math.Max(
                    maximumPositionStepMillimeters,
                    sample.DeltaPositionMillimeters ?? 0.0);
                maximumAbsoluteYawStepDegrees = Math.Max(
                    maximumAbsoluteYawStepDegrees,
                    Math.Abs(sample.DeltaYawDegrees ?? 0.0));
            }

            var completionReason = testFailure != null
                ? "因异常提前结束"
                : completedAutomatically
                    ? "到达设定时长，已自动结束"
                    : "收到手动停止请求";
            var message =
                $"Detour静态诊断{completionReason}；" +
                $"样本={snapshot.Count}，有效Detour样本={successfulSamples}，" +
                $"最大位置阶跃={maximumPositionStepMillimeters:F2}mm，" +
                $"最大航向阶跃={maximumAbsoluteYawStepDegrees:F3}°；" +
                $"CSV={SavedFilePath}";

            Console.WriteLine(message);
            Hedingben.ToastText(message);
        }

        /// <summary>
        /// 使用InvariantCulture格式化并转义一行CSV字段。
        /// </summary>
        private static void WriteCsvRow(
            TextWriter writer,
            params object[] values)
        {
            var fields = new string[values.Length];
            for (var index = 0; index < values.Length; index++)
            {
                fields[index] = FormatCsvValue(values[index]);
            }

            writer.WriteLine(string.Join(",", fields));
        }

        /// <summary>
        /// 将单个值转换为区域无关且符合CSV转义规则的文本。
        /// </summary>
        private static string FormatCsvValue(object value)
        {
            if (value == null)
            {
                return string.Empty;
            }

            string text;
            if (value is bool boolean)
            {
                text = boolean ? "1" : "0";
            }
            else if (value is IFormattable formattable)
            {
                text = formattable.ToString(
                    null,
                    CultureInfo.InvariantCulture);
            }
            else
            {
                text = value.ToString();
            }

            if (text.IndexOfAny(
                    new[] { ',', '"', '\r', '\n' }) < 0)
            {
                return text;
            }

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