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 MyParking.Shared;

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

    // C层实验工具：统一采集并保存轨迹跟踪实验数据。
    public sealed class TrackingExperimentRecorder
    {
        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 int _sampleIntervalMs;

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

        private readonly object _sampleSyncRoot =
            new object();

        private readonly object _commandSyncRoot =
            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;

        public TrackingExperimentRecorder(
            string controllerName,
            string trajectoryName,
            int trialNumber,
            Vector2 referenceStart,
            Vector2 referenceEnd,
            float referenceSpeed,
            float referenceAngularSpeed = 0f,
            int sampleIntervalMs = 50,
            float referenceMotionFrameYawDegrees = 0f)
        {
            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;
            _sampleIntervalMs = sampleIntervalMs;
        }

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

        // 启动后台采样线程。
        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;
            }
        }

        // 停止采样并将本次实验保存为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();

                Console.WriteLine(
                    $"轨迹实验数据已保存：{SavedFilePath}");
            }
            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;

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

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

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

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

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

            var outputDirectory = Path.Combine(
                AppContext.BaseDirectory,
                "TrackingExperiments");

            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");

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

        // 将文件名中的非法字符替换为下划线。
        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);
        }

        // 对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("\"", "\"\"") +
                "\"";
        }
    }
}