using System.Diagnostics;
using System.Net.Sockets;
using Microsoft.Extensions.Logging;

namespace Flyshot.Runtime.Fanuc.Protocol;

/// <summary>
/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供状态包驱动的命令发送与响应接收能力。
/// </summary>
public sealed class FanucJ519Client : IDisposable
{
    private readonly object _commandLock = new();
    private readonly object _responseLock = new();
    private readonly ILogger<FanucJ519Client>? _logger;
    /// <summary>
    /// 指示预装队列发出 LastData 结束帧后是否进入仅监控状态，而不是继续发送保姿命令。
    /// </summary>
    private readonly bool _endCommandStreamAfterQueueDrained;
    private UdpClient? _udpClient;
    private CancellationTokenSource? _cts;
    private Thread? _receiveThread;
    private FanucJ519Command? _currentCommand;
    private FanucJ519Command? _lastSentCommand;
    // 稠密轨迹执行时预装的命令队列，由机器人状态包节拍逐帧出队。
    private Queue<FanucJ519Command>? _commandQueue;
    private TaskCompletionSource? _commandQueueDrainedCompletion;
    private List<FanucJ519Command>? _commandHistoryForTests;
    private FanucJ519Response? _latestResponse;
    private long _slowSendCount;
    private long _maxReceiveToSendTicks;
    private uint _sequenceBufferSize;
    /// <summary>
    /// 在 StartMotion 前是否有待发送的新命令；如果为 true 则 StartMotion 不会清空当前目标，而是继续发送新目标。
    /// </summary>
    private bool _hasPendingCommandForStart;
    /// <summary>
    ///  标记命令流已通过 LastData 正常结束；该状态下仅接收状态包，不继续回发 hold 命令。
    /// </summary>
    private bool _monitorOnlyAfterLastData;
    private bool _motionStarted;
    private bool _disposed;

    /// <summary>
    /// 获取当前是否已创建 UDP 套接字。
    /// </summary>
    public bool IsConnected => _udpClient is not null;

    /// <summary>
    /// 获取正常队列结束时是否通过 LastData 结束命令流并保留状态监控。
    /// </summary>
    internal bool EndCommandStreamAfterQueueDrained => _endCommandStreamAfterQueueDrained;

    /// <summary>
    /// 初始化 FANUC J519 客户端。
    /// </summary>
    /// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
    /// <param name="endCommandStreamAfterQueueDrained">队列结束帧发出后是否停止命令流并保留状态监控。</param>
    public FanucJ519Client(
        ILogger<FanucJ519Client>? logger = null,
        bool endCommandStreamAfterQueueDrained = true)
    {
        _logger = logger;
        _endCommandStreamAfterQueueDrained = endCommandStreamAfterQueueDrained;
    }

    /// <summary>
    /// 建立到 FANUC 控制柜 UDP 60015 运动通道的连接并启动接收循环。
    /// </summary>
    /// <param name="ip">控制柜 IP 地址。</param>
    /// <param name="port">运动通道端口，默认 60015。</param>
    /// <param name="cancellationToken">取消令牌。</param>
    public async Task ConnectAsync(string ip, int port = 60015, CancellationToken cancellationToken = default)
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        if (string.IsNullOrWhiteSpace(ip))
        {
            throw new ArgumentException("IP 不能为空。", nameof(ip));
        }

        if (_udpClient is not null)
        {
            throw new InvalidOperationException("J519 通道已经连接，请先 Disconnect。");
        }

        _logger?.LogInformation("J519 ConnectAsync: {Ip}:{Port}", ip, port);

        _udpClient = new UdpClient();
        _udpClient.Connect(ip, port);
        ConfigureRealtimeSocket(_udpClient.Client);
        ConfigureProcessPriority();

        // 发送初始化包。
        await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
        _logger?.LogInformation("J519 初始化包已发送");

        _cts = new CancellationTokenSource();
        _receiveThread = new Thread(ReceiveLoop)
        {
            IsBackground = true,
            Name = "J519 UDP realtime loop",
            Priority = ThreadPriority.Highest
        };
        _receiveThread.Start();
    }

    /// <summary>
    /// 启动 J519 命令发送许可；实际发包由机器人状态包节拍驱动。
    /// </summary>
    public void StartMotion()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        if (_udpClient is null)
        {
            throw new InvalidOperationException("J519 通道未连接。");
        }

        lock (_commandLock)
        {
            _lastSentCommand = null;
            if (_motionStarted)
            {
                _logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
                return;
            }

            if (!_hasPendingCommandForStart)
            {
                _currentCommand = null;
                CompleteCommandQueueLocked();
            }

            _hasPendingCommandForStart = false;
            _monitorOnlyAfterLastData = false;
            _motionStarted = true;
            
        }

        _logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送");
    }

    /// <summary>
    /// 配置状态包驱动回发时附加到机器人 sequence 的前视缓冲深度。
    /// </summary>
    /// <param name="bufferSize">要附加到状态序号上的缓冲深度。</param>
    public void SetSequenceBufferSize(int bufferSize)
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        if (bufferSize < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(bufferSize), "J519 sequence buffer size 不能为负数。");
        }

        lock (_commandLock)
        {
            _sequenceBufferSize = (uint)bufferSize;
        }
    }

    /// <summary>
    /// 发送状态输出停止包并停止 J519 命令发送。
    /// </summary>
    public async Task StopMotionAsync(CancellationToken cancellationToken = default)
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        if (_udpClient is null)
        {
            return;
        }

        _logger?.LogInformation("J519 StopMotionAsync: 停止状态包驱动发送");
        lock (_commandLock)
        {
            _motionStarted = false;
            _hasPendingCommandForStart = false;
            _monitorOnlyAfterLastData = false;
            CompleteCommandQueueLocked();
        }

        // FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。
        await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
        _logger?.LogInformation("J519 StopMotionAsync: 状态输出停止包已发送");
    }

    /// <summary>
    /// 原子更新下一周期要发送的 J519 命令。
    /// </summary>
    /// <param name="command">新的 J519 命令。</param>
    public void UpdateCommand(FanucJ519Command command)
    {
        ArgumentNullException.ThrowIfNull(command);
        ObjectDisposedException.ThrowIf(_disposed, this);

        lock (_commandLock)
        {
            CompleteCommandQueueLocked();
            _currentCommand = command;
            _hasPendingCommandForStart = true;
            _monitorOnlyAfterLastData = false;
            _commandHistoryForTests?.Add(command);
        }

        if (_logger?.IsEnabled(LogLevel.Debug) == true)
        {
            //_logger.LogDebug(
            //    "J519 UpdateCommand: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
            //    command.TargetJoints,
            //    command.WriteIoMask,
            //    command.WriteIoValue);
        }
    }

    /// <summary>
    /// 装载一整段 J519 命令队列；后续每个可接收命令的机器人状态包会自动取出下一帧。
    /// </summary>
    /// <param name="commands">按执行顺序排列的 J519 命令列表，至少包含一帧。</param>
    public void LoadCommandQueue(IReadOnlyList<FanucJ519Command> commands)
    {
        ArgumentNullException.ThrowIfNull(commands);
        ObjectDisposedException.ThrowIf(_disposed, this);
        if (commands.Count == 0)
        {
            throw new ArgumentException("J519 命令队列至少需要包含一帧。", nameof(commands));
        }

        lock (_commandLock)
        {
            CompleteCommandQueueLocked();
            _commandQueue = new Queue<FanucJ519Command>(commands);
            // 队列耗尽后继续保持最后一帧目标，避免运动结束后回落到旧目标或空目标。
            _currentCommand = commands[^1];
            _hasPendingCommandForStart = true;
            _monitorOnlyAfterLastData = false;
            _motionStarted = true;
            _commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously);
            _commandHistoryForTests?.AddRange(commands);
        }
        _logger?.LogInformation("J519 命令队列已装载: count={Count}", commands.Count);
        _logger?.LogInformation("开始运动前向机器人发送的sequence={Sequence}",_lastSentCommand?.Sequence ?? 0);
    }

    /// <summary>
    /// 等待当前预装命令队列被状态包全部取出；无队列时立即完成。
    /// </summary>
    /// <param name="cancellationToken">取消令牌。</param>
    /// <returns>表示等待过程的任务。</returns>
    internal Task WaitForCommandQueueDrainedAsync(CancellationToken cancellationToken = default)
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        Task waitTask;
        lock (_commandLock)
        {
            waitTask = _commandQueueDrainedCompletion?.Task ?? Task.CompletedTask;
        }

        return waitTask.WaitAsync(cancellationToken);
    }

    /// 判断当前是否没有等待出队的命令；仅供单元测试断言。
    /// </summary>
    /// <returns>如果队列为空或尚未装载队列，则返回 true。</returns>
    internal bool IsCommandQueueDrainedForTests()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        lock (_commandLock)
        {
            return _commandQueue is null || _commandQueue.Count == 0;
        }
    }

    /// <summary>
    /// 打开命令历史记录，仅供单元测试验证运行时生成的命令序列。
    /// </summary>
    internal void EnableCommandHistoryForTests()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        lock (_commandLock)
        {
            _commandHistoryForTests = [];
        }
    }

    /// <summary>
    /// 获取测试记录的命令历史。
    /// </summary>
    /// <returns>命令历史快照。</returns>
    internal IReadOnlyList<FanucJ519Command> GetCommandHistoryForTests()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        lock (_commandLock)
        {
            return _commandHistoryForTests?.ToArray() ?? Array.Empty<FanucJ519Command>();
        }
    }

    /// <summary>
    /// 获取最近一次通过 UpdateCommand 设置的 J519 命令；供测试断言使用。
    /// </summary>
    /// <returns>当前 J519 命令或 null。</returns>
    internal FanucJ519Command? GetCurrentCommand()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        lock (_commandLock)
        {
            return _currentCommand;
        }
    }

    /// <summary>
    /// 获取最近一次解析的 J519 响应；若尚未收到任何响应则返回 null。
    /// </summary>
    /// <returns>最新 J519 响应或 null。</returns>
    public FanucJ519Response? GetLatestResponse()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        lock (_responseLock)
        {
            return _latestResponse;
        }
    }

    /// <summary>
    /// 断开 J519 通道并释放资源。
    /// </summary>
    public void Disconnect()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        _cts?.Cancel();

        try
        {
            _udpClient?.Dispose();
            _receiveThread?.Join(TimeSpan.FromSeconds(1));
        }
        catch (ObjectDisposedException)
        {
            // 忽略释放期间的套接字关闭异常。
        }

        _receiveThread = null;

        _cts?.Dispose();
        _cts = null;

        _udpClient = null;

        lock (_commandLock)
        {
            _currentCommand = null;
            _lastSentCommand = null;
            CompleteCommandQueueLocked();
            _commandHistoryForTests = null;
            _hasPendingCommandForStart = false;
            _monitorOnlyAfterLastData = false;
            _motionStarted = false;
        }

        lock (_responseLock)
        {
            _latestResponse = null;
        }
    }

    /// <summary>
    /// 释放客户端资源。
    /// </summary>
    public void Dispose()
    {
        if (_disposed)
        {
            return;
        }

        _disposed = true;
        _cts?.Cancel();

        try
        {
            _udpClient?.Dispose();
            _receiveThread?.Join(TimeSpan.FromSeconds(1));
        }
        catch (ObjectDisposedException)
        {
            // 忽略释放期间的套接字关闭异常。
        }

        _cts?.Dispose();

        lock (_commandLock)
        {
            CompleteCommandQueueLocked();
            _hasPendingCommandForStart = false;
            _monitorOnlyAfterLastData = false;
        }
    }

    private static void ConfigureProcessPriority()
    {
        try
        {
            var process = Process.GetCurrentProcess();
            if (process.PriorityClass < ProcessPriorityClass.High)
            {
                process.PriorityClass = ProcessPriorityClass.High;
            }
        }
        catch (Exception)
        {
            // 某些部署环境不允许提升进程优先级；实时链路仍按普通优先级运行。
        }
    }

    /// <summary>
    /// 配置 J519 UDP 套接字的低延迟参数。
    /// </summary>
    /// <param name="socket">已连接 FANUC 60015 的 UDP 套接字。</param>
    private void ConfigureRealtimeSocket(Socket socket)
    {
        socket.ReceiveBufferSize = 1024 * 1024;
        socket.SendBufferSize = 1024 * 1024;

        try
        {
            // DSCP EF(46) 标记低延迟流量，是否生效取决于现场网卡、交换机和控制柜网络策略。
            socket.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.TypeOfService, 0xB8);
        }
        catch (SocketException ex)
        {
            _logger?.LogWarning(ex, "J519 UDP 套接字无法设置 DSCP EF 优先级标记");
        }
    }

    /// <summary>
    /// 后台接收循环：在专用高优先级线程中同步接收 132B 响应并立即回发命令。
    /// </summary>
    private void ReceiveLoop()
    {
        var udpClient = _udpClient;
        var cancellationToken = _cts?.Token ?? CancellationToken.None;
        if (udpClient is null)
        {
            return;
        }

        _logger?.LogInformation("J519 ReceiveLoop 启动");
        long receiveCount = 0;
        FanucJ519Response? lastLoggedResponse = null;
        var receiveBuffer = new byte[FanucJ519Protocol.ResponsePacketLength];
        var commandBuffer = new byte[FanucJ519Protocol.CommandPacketLength];

        try
        {
            while (!cancellationToken.IsCancellationRequested)
            {
                var received = udpClient.Client.Receive(receiveBuffer);
                if (received == FanucJ519Protocol.ResponsePacketLength)
                {
                    var receiveTicks = Stopwatch.GetTimestamp();
                    var response = FanucJ519Protocol.ParseResponse(receiveBuffer);

                    // 先按状态包节拍回发命令，再做低频日志处理，减少受信周期内的非必要工作。
                    if (response.AcceptsCommand)
                    {
                        SendCommandForStatus(udpClient.Client, response, commandBuffer, receiveTicks);
                    }

                    lock (_responseLock)
                    {
                        _latestResponse = response;
                    }

                    receiveCount++;

                    // 仅在状态变化时记录 Info，避免高频日志。
                    if (lastLoggedResponse is null
                        || lastLoggedResponse.Status != response.Status
                        || lastLoggedResponse.RobotInMotion != response.RobotInMotion
                        || lastLoggedResponse.SystemReady != response.SystemReady
                        || lastLoggedResponse.AcceptsCommand != response.AcceptsCommand)
                    {
                        _logger?.LogInformation(
                            "J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, received={received}, SystemReady={SystemReady}, RobotInMotion={RobotInMotion}, pose=[{Pose}], joints=[{Joints}]",
                            response.Status,
                            response.Sequence,
                            response.AcceptsCommand,
                            response.ReceivedCommand,
                            response.SystemReady,
                            response.RobotInMotion,
                            string.Join(", ", response.Pose.Select(v => v.ToString("F3"))),
                            string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F3"))));

                        var lastSentTargetJoints = GetLastSentTargetJointsLogText();
                        _logger?.LogInformation("J519 最后一条发送目标关节轴: joints=[{Joints}]", lastSentTargetJoints);
                        lastLoggedResponse = response;

                        // 如果状态从AcceptsCommand true 变为false,说明机器人报错,清空队列
                        if (!response.AcceptsCommand)
                        {
                            lock (_commandLock)
                            {
                                _currentCommand = null;
                                CompleteCommandQueueLocked();
                            }
                            _logger?.LogWarning("J519 接收状态包显示机器人不可接受命令，已清空命令队列");
                        }

                    }
                    else if (receiveCount % 1000 == 0)
                    {
                        var maxReceiveToSendMs = Stopwatch.GetElapsedTime(0, Interlocked.Read(ref _maxReceiveToSendTicks)).TotalMilliseconds;
                        _logger?.LogDebug(
                            "J519 已接收 {Count} 个响应包，receive-to-send 最大耗时约 {MaxMs:F3}ms，超过 0.5ms 次数 {SlowCount}",
                            receiveCount,
                            maxReceiveToSendMs,
                            Interlocked.Read(ref _slowSendCount));
                    }
                }
            }
        }
        catch (OperationCanceledException)
        {
            _logger?.LogInformation("J519 ReceiveLoop 正常取消，共接收 {Count} 个包", receiveCount);
        }
        catch (ObjectDisposedException)
        {
            _logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
        }
        catch (SocketException ex) when (cancellationToken.IsCancellationRequested)
        {
            _logger?.LogInformation(ex, "J519 ReceiveLoop 因取消关闭套接字退出，共接收 {Count} 个包", receiveCount);
        }
    }

    /// <summary>
    /// 按机器人状态包的 sequence 立即回发当前 J519 命令。
    /// </summary>
    /// <param name="socket">已连接的 UDP 套接字。</param>
    /// <param name="response">刚收到的机器人状态包。</param>
    /// <param name="commandBuffer">可复用的 64B 命令包缓冲区。</param>
    /// <param name="receiveTicks">收到状态包后的时间戳。</param>
    private void SendCommandForStatus(Socket socket, FanucJ519Response response, byte[] commandBuffer, long receiveTicks)
    {
        FanucJ519Command? command;
        var willDrainQueue = false;
        var suppressHoldCommand = false;
        lock (_commandLock)
        {
            if (_monitorOnlyAfterLastData)
            {
                command = null;
                suppressHoldCommand = true;
            }
            else if (!_motionStarted)
            {
                command = null;
            }
            else if (_commandQueue is { Count: > 0 } queue)
            {
                // 状态包是唯一节拍源：每收到一帧可接收状态，才取出下一条目标。
                var queuedCommand = queue.Dequeue();
                _currentCommand = queuedCommand;
                willDrainQueue = queue.Count == 0;
                command = queuedCommand;
            }
            else
            {
                command = _currentCommand;
            }
        }

        if (command is null && !suppressHoldCommand)
        {
            command = TryBuildHoldCommandFromLatestResponse(response);
        }

        if (command is null)
        {
            return;
        }

        uint sequenceToSend;
        lock (_commandLock)
        {
            sequenceToSend = response.Sequence + _sequenceBufferSize;
        }

        FanucJ519Protocol.PackCommandPacket(command, sequenceToSend, commandBuffer);
        socket.Send(commandBuffer);
        TrackReceiveToSendLatency(receiveTicks);
        // _logger?.LogDebug("J519 已回发命令包，seq={Seq}", sequence);
        // _logger?.LogDebug(
        //     "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
        //     command.TargetJoints,
        //     command.WriteIoMask,
        //     command.WriteIoValue);
        lock (_commandLock)
        {
            _lastSentCommand = command;
            if (_endCommandStreamAfterQueueDrained && command.LastData == 1)
            {
                // LastData 是命令流的正常结束语义；结束后保留状态接收，但不继续回发保姿包。
                // _monitorOnlyAfterLastData = true;
                // _motionStarted = false;
                CompleteCommandQueueLocked();
            }
            else if (willDrainQueue && _commandQueue is { Count: 0 })
            {
                CompleteCommandQueueLocked();
            }
        }
    }

    /// <summary>
    /// 当当前没有显式目标时，使用最近一帧状态反馈关节角构造保姿命令，维持机器人当前位置。
    /// </summary>
    /// <param name="response">当前收到的机器人状态包。</param>
    /// <returns>可用于保姿的临时 J519 命令；若反馈关节不足则返回 null。</returns>
    private static FanucJ519Command? TryBuildHoldCommandFromLatestResponse(FanucJ519Response response)
    {
        if (response.JointDegrees.Count < 6)
        {
            return null;
        }

        // 无运动目标时，持续回发机器人当前反馈关节，保持伺服流与机器人当前位置一致。
        return new FanucJ519Command(
            sequence: response.Sequence,
            targetJoints:
            [
                response.JointDegrees[0],
                response.JointDegrees[1],
                response.JointDegrees[2],
                response.JointDegrees[3],
                response.JointDegrees[4],
                response.JointDegrees[5]
            ]);
    }

    /// <summary>
    /// 记录状态包到命令包发出的最大耗时和慢发送次数，供低频诊断日志观察调度抖动。
    /// </summary>
    /// <param name="receiveTicks">收到状态包后的时间戳。</param>
    private void TrackReceiveToSendLatency(long receiveTicks)
    {
        var elapsedTicks = Stopwatch.GetTimestamp() - receiveTicks;
        var currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
        while (elapsedTicks > currentMax
            && Interlocked.CompareExchange(ref _maxReceiveToSendTicks, elapsedTicks, currentMax) != currentMax)
        {
            currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
        }

        if (Stopwatch.GetElapsedTime(0, elapsedTicks) > TimeSpan.FromMilliseconds(0.5))
        {
            Interlocked.Increment(ref _slowSendCount);
        }
    }

    /// <summary>
    /// 清空当前命令队列，并唤醒等待队列结束的运行时任务。
    /// </summary>
    private void CompleteCommandQueueLocked()
    {
        _commandQueue?.Clear();
        _commandQueue = null;
        _commandQueueDrainedCompletion?.TrySetResult();
        _commandQueueDrainedCompletion = null;
    }

    /// <summary>
    /// 读取最近一次已成功发送命令的目标关节轴文本，便于状态日志直接对照控制目标。
    /// </summary>
    /// <returns>格式化后的目标关节轴文本；如果尚未发送命令则返回占位符。</returns>
    private string GetLastSentTargetJointsLogText()
    {
        lock (_commandLock)
        {
            return _lastSentCommand is null
                ? "n/a"
                : string.Join(", ", _lastSentCommand.TargetJoints.Take(6).Select(v => v.ToString("F5")));
        }
    }
}