using System;
using System.Collections.Concurrent;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.Text;
using System.Threading;

namespace MedullaAdapter
{
    /// <summary>
    /// 在后台保存驱动器CAN速度事件和底盘周期快照，避免文件IO阻塞CAN回调。
    /// </summary>
    internal sealed class WheelSpeedDiagnosticLogger : IDisposable
    {
        private readonly struct LogRecord
        {
            public LogRecord(bool isCanEvent, string line)
            {
                IsCanEvent = isCanEvent;
                Line = line;
            }

            public bool IsCanEvent { get; }

            public string Line { get; }
        }

        private const int MaximumQueuedRecords = 100000;
        private const double SnapshotIntervalMilliseconds = 20.0;
        private readonly ConcurrentQueue<LogRecord> _records = new();
        private readonly AutoResetEvent _recordsAvailable = new(false);
        private readonly object _lifecycleLock = new();
        private Stopwatch _stopwatch;
        private Thread _writerThread;
        private StreamWriter _canWriter;
        private StreamWriter _snapshotWriter;
        private volatile bool _isRunning;
        private int _queuedRecordCount;
        private long _receiveSequence;
        private long _droppedRecordCount;
        private double _lastSnapshotMilliseconds = double.NegativeInfinity;

        public bool IsRunning => _isRunning;

        public string CanLogPath { get; private set; } = "";

        public string SnapshotLogPath { get; private set; } = "";

        /// <summary>
        /// 创建本次诊断的两个CSV文件并启动后台写入线程。
        /// </summary>
        public void Start(string directory, int carNumber)
        {
            lock (_lifecycleLock)
            {
                if (_isRunning)
                    return;

                if (string.IsNullOrWhiteSpace(directory))
                    throw new ArgumentException(
                        "轮速诊断目录不能为空。",
                        nameof(directory));

                Directory.CreateDirectory(directory);

                var filePrefix =
                    $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_Car{carNumber}";

                CanLogPath = Path.Combine(
                    directory,
                    $"{filePrefix}_can.csv");

                SnapshotLogPath = Path.Combine(
                    directory,
                    $"{filePrefix}_snapshot.csv");

                _canWriter = CreateWriter(CanLogPath);
                _snapshotWriter = CreateWriter(SnapshotLogPath);

                _canWriter.WriteLine(
                    "ElapsedMs,ReceiveSequence,CanId,MotorName,RawRpm,SpeedMps");

                _snapshotWriter.WriteLine(
                    "ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
                    "DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
                    "PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
                    "CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," +
                    "PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," +
                    "ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," +
                    "ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," +
                    "TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," +
                    "ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear," +
                    "ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear");

                while (_records.TryDequeue(out _))
                {
                }

                _queuedRecordCount = 0;
                _receiveSequence = 0;
                _droppedRecordCount = 0;
                _lastSnapshotMilliseconds =
                    double.NegativeInfinity;
                _stopwatch = Stopwatch.StartNew();
                _isRunning = true;

                _writerThread = new Thread(WriterLoop)
                {
                    IsBackground = true,
                    Name = "WheelSpeedDiagnosticWriter"
                };
                _writerThread.Start();
            }
        }

        /// <summary>
        /// 停止记录并等待队列中的诊断数据写入磁盘。
        /// </summary>
        public void Stop()
        {
            Thread writerThread;

            lock (_lifecycleLock)
            {
                if (!_isRunning &&
                    _writerThread == null)
                    return;

                _isRunning = false;
                writerThread = _writerThread;
                _recordsAvailable.Set();
            }

            writerThread?.Join(3000);

            lock (_lifecycleLock)
            {
                _canWriter?.Flush();
                _snapshotWriter?.Flush();
                _canWriter?.Dispose();
                _snapshotWriter?.Dispose();
                _canWriter = null;
                _snapshotWriter = null;
                _writerThread = null;
                _stopwatch?.Stop();
            }
        }

        /// <summary>
        /// 将一帧驱动器速度反馈加入内存队列，不在CAN回调中执行文件写入。
        /// </summary>
        public void RecordCanFeedback(
            ushort canId,
            string motorName,
            float rawRpm,
            float speedMetersPerSecond)
        {
            if (!_isRunning)
                return;

            var elapsedMilliseconds =
                _stopwatch.Elapsed.TotalMilliseconds;
            var receiveSequence =
                Interlocked.Increment(
                    ref _receiveSequence);

            var line = string.Join(
                ",",
                Format(elapsedMilliseconds),
                receiveSequence.ToString(
                    CultureInfo.InvariantCulture),
                $"0x{canId:X3}",
                motorName,
                Format(rawRpm),
                Format(speedMetersPerSecond));

            Enqueue(new LogRecord(
                isCanEvent: true,
                line));
        }

        /// <summary>
        /// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。
        /// </summary>
        public void RecordSnapshot(
            DiverCartDefinition cart)
        {
            if (!_isRunning || cart == null)
                return;

            var elapsedMilliseconds =
                _stopwatch.Elapsed.TotalMilliseconds;

            if (elapsedMilliseconds -
                _lastSnapshotMilliseconds <
                SnapshotIntervalMilliseconds)
            {
                return;
            }

            _lastSnapshotMilliseconds =
                elapsedMilliseconds;

            var line = string.Join(
                ",",
                Format(elapsedMilliseconds),
                cart.CarNum.ToString(
                    CultureInfo.InvariantCulture),
                Format((int)cart.TransmitterControlMode),
                Format(cart.ManualMode),
                Format(cart.SendThresSpeed),
                Format(cart.DiffSteerKp),
                Format(cart.DiffSteerKi),
                Format(cart.DiffSteerKd),
                Format(cart.DiffSteerMaxI),
                Format(cart.DiffSteerDeadZone),
                Format(cart.DiffSteerThresh),
                Format(cart.DiffSteerSpeedAcc),
                Format(cart.DiffSteerOutputLeftFront),
                Format(cart.DiffSteerOutputLeftRear),
                Format(cart.DiffSteerOutputRightFront),
                Format(cart.DiffSteerOutputRightRear),
                Format(cart.SpeedLeftFrontLeft),
                Format(cart.SpeedLeftFrontRight),
                Format(cart.SpeedLeftRearLeft),
                Format(cart.SpeedLeftRearRight),
                Format(cart.SpeedRightFrontLeft),
                Format(cart.SpeedRightFrontRight),
                Format(cart.SpeedRightRearLeft),
                Format(cart.SpeedRightRearRight),
                Format(cart.SpeedLFL),
                Format(cart.SpeedLFR),
                Format(cart.SpeedLRL),
                Format(cart.SpeedLRR),
                Format(cart.SpeedRFL),
                Format(cart.SpeedRFR),
                Format(cart.SpeedRRL),
                Format(cart.SpeedRRR),
                Format(cart.ActualSpeedLeftFrontLeft),
                Format(cart.ActualSpeedLeftFrontRight),
                Format(cart.ActualSpeedLeftRearLeft),
                Format(cart.ActualSpeedLeftRearRight),
                Format(cart.ActualSpeedRightFrontLeft),
                Format(cart.ActualSpeedRightFrontRight),
                Format(cart.ActualSpeedRightRearLeft),
                Format(cart.ActualSpeedRightRearRight),
                Format(cart.ActualSpeedLeftFront),
                Format(cart.ActualSpeedLeftRear),
                Format(cart.ActualSpeedRightFront),
                Format(cart.ActualSpeedRightRear),
                Format(cart.ThLeftFront),
                Format(cart.ThLeftRear),
                Format(cart.ThRightFront),
                Format(cart.ThRightRear),
                Format(cart.ActualThLeftFront),
                Format(cart.ActualThLeftRear),
                Format(cart.ActualThRightFront),
                Format(cart.ActualThRightRear),
                Format(cart.ThLeftFront - cart.ActualThLeftFront),
                Format(cart.ThLeftRear - cart.ActualThLeftRear),
                Format(cart.ThRightFront - cart.ActualThRightFront),
                Format(cart.ThRightRear - cart.ActualThRightRear));

            Enqueue(new LogRecord(
                isCanEvent: false,
                line));
        }

        private static StreamWriter CreateWriter(
            string path)
        {
            return new StreamWriter(
                path,
                append: false,
                new UTF8Encoding(
                    encoderShouldEmitUTF8Identifier: true),
                bufferSize: 64 * 1024);
        }

        private void Enqueue(LogRecord record)
        {
            var queuedCount =
                Interlocked.Increment(
                    ref _queuedRecordCount);

            if (queuedCount >
                MaximumQueuedRecords)
            {
                Interlocked.Decrement(
                    ref _queuedRecordCount);
                Interlocked.Increment(
                    ref _droppedRecordCount);
                return;
            }

            _records.Enqueue(record);
            _recordsAvailable.Set();
        }

        private void WriterLoop()
        {
            var lastFlushTime = DateTime.UtcNow;

            try
            {
                while (_isRunning ||
                       !_records.IsEmpty)
                {
                    var wroteAnyRecord = false;

                    while (_records.TryDequeue(
                               out var record))
                    {
                        Interlocked.Decrement(
                            ref _queuedRecordCount);

                        if (record.IsCanEvent)
                            _canWriter.WriteLine(record.Line);
                        else
                            _snapshotWriter.WriteLine(record.Line);

                        wroteAnyRecord = true;
                    }

                    var shouldFlush =
                        wroteAnyRecord &&
                        (DateTime.UtcNow -
                         lastFlushTime)
                        .TotalMilliseconds >= 500.0;

                    if (shouldFlush)
                    {
                        _canWriter.Flush();
                        _snapshotWriter.Flush();
                        lastFlushTime = DateTime.UtcNow;
                    }

                    if (!wroteAnyRecord)
                        _recordsAvailable.WaitOne(100);
                }

                var dropped =
                    Interlocked.Read(
                        ref _droppedRecordCount);

                if (dropped > 0)
                {
                    _canWriter.WriteLine(
                        $"# DroppedRecords={dropped}");
                    _snapshotWriter.WriteLine(
                        $"# DroppedRecords={dropped}");
                }

                _canWriter.Flush();
                _snapshotWriter.Flush();
            }
            catch (Exception ex)
            {
                // 后台日志失败不能终止车辆控制线程。
                Console.WriteLine(
                    "轮速诊断后台写入失败：" +
                    ex.Message);
            }
        }

        private static string Format(
            double value)
        {
            return value.ToString(
                "0.######",
                CultureInfo.InvariantCulture);
        }

        public void Dispose()
        {
            Stop();
            _recordsAvailable.Dispose();
        }
    }
}