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;
    private UdpClient? _udpClient;
    private CancellationTokenSource? _cts;
    private CancellationTokenSource? _sendCts;
    private Task? _sendTask;
    private Task? _receiveTask;
    private FanucJ519Command? _currentCommand;
    private List<FanucJ519Command>? _commandHistoryForTests;
    private FanucJ519Response? _latestResponse;
    private uint _nextSequence;
    private bool _disposed;

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

    /// <summary>
    /// 初始化 FANUC J519 客户端。
    /// </summary>
    /// <param name="logger">日志记录器；允许 null。</param>
    public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
    {
        _logger = logger;
    }

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

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

        _cts = new CancellationTokenSource();
        _receiveTask = Task.Run(() => ReceiveLoopAsync(_cts.Token), _cts.Token);
    }

    /// <summary>
    /// 启动约 8ms 周期的 J519 命令发送循环。
    /// </summary>
    public void StartMotion()
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

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

        if (_sendTask is not null)
        {
            _logger?.LogDebug("J519 StartMotion: 发送循环已在运行");
            return; // 已在运行。
        }

        _logger?.LogInformation("J519 StartMotion: 启动发送循环");
        _sendCts = CancellationTokenSource.CreateLinkedTokenSource(_cts!.Token);
        _sendTask = Task.Run(() => SendLoopAsync(_sendCts.Token), _sendCts.Token);
    }

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

        if (_udpClient is null)
        {
            return;
        }

        _logger?.LogInformation("J519 StopMotionAsync: 停止发送循环");

        if (_sendTask is not null)
        {
            _sendCts?.Cancel();

            try
            {
                await _sendTask.WaitAsync(TimeSpan.FromSeconds(1), cancellationToken).ConfigureAwait(false);
            }
            catch (TimeoutException)
            {
                _logger?.LogWarning("J519 StopMotionAsync: 发送循环未能在 1 秒内结束");
            }
            catch (OperationCanceledException)
            {
                // 正常取消。
            }

            _sendTask = null;
        }

        _sendCts?.Dispose();
        _sendCts = null;

        // 发送结束包通知控制器停止伺服流。
        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)
        {
            _currentCommand = command;
            _commandHistoryForTests?.Add(command);
        }

        _logger?.LogDebug(
            "J519 UpdateCommand: joints=[{Joints}], ioMask=0x{IoMask:X4}, ioValue=0x{IoValue:X4}",
            string.Join(", ", command.TargetJoints.Select(j => j.ToString("F2"))),
            command.WriteIoMask,
            command.WriteIoValue);
    }

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

        _sendCts?.Cancel();
        _cts?.Cancel();

        try
        {
            _sendTask?.Wait(TimeSpan.FromSeconds(1));
        }
        catch (AggregateException)
        {
            // 忽略取消异常。
        }

        _sendTask?.Dispose();
        _sendTask = null;
        _sendCts?.Dispose();
        _sendCts = null;

        try
        {
            _receiveTask?.Wait(TimeSpan.FromSeconds(1));
        }
        catch (AggregateException)
        {
            // 忽略取消异常。
        }

        _receiveTask?.Dispose();
        _receiveTask = null;

        _cts?.Dispose();
        _cts = null;

        _udpClient?.Dispose();
        _udpClient = null;

        lock (_commandLock)
        {
            _currentCommand = null;
            _commandHistoryForTests = null;
            _nextSequence = 0;
        }

        lock (_responseLock)
        {
            _latestResponse = null;
        }
    }

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

        _disposed = true;
        _sendCts?.Cancel();
        _cts?.Cancel();

        try
        {
            _sendTask?.Wait(TimeSpan.FromSeconds(1));
        }
        catch (AggregateException)
        {
            // 忽略取消异常。
        }

        try
        {
            _receiveTask?.Wait(TimeSpan.FromSeconds(1));
        }
        catch (AggregateException)
        {
            // 忽略取消异常。
        }

        _sendTask?.Dispose();
        _receiveTask?.Dispose();
        _sendCts?.Dispose();
        _cts?.Dispose();
        _udpClient?.Dispose();
    }

    /// <summary>
    /// 后台发送循环：以 Stopwatch + SpinWait 实现高精度 8ms 周期发送当前命令。
    /// </summary>
    private async Task SendLoopAsync(CancellationToken cancellationToken)
    {
        if (_udpClient is null)
        {
            return;
        }

        // 8ms 伺服周期，对应 125Hz。
        var periodTicks = (long)(0.008 * Stopwatch.Frequency);
        var stopwatch = Stopwatch.StartNew();
        long nextTick = stopwatch.ElapsedTicks;

        try
        {
            while (!cancellationToken.IsCancellationRequested)
            {
                nextTick += periodTicks;

                FanucJ519Command? command;
                lock (_commandLock)
                {
                    command = _currentCommand is null ? null : WithSequence(_currentCommand, _nextSequence++);
                }

                if (command is not null)
                {
                    var packet = FanucJ519Protocol.PackCommandPacket(command);
                    await _udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false);
                }

                // 高精度忙等待直到下一周期，避免 PeriodicTimer 的 ±15ms 抖动。
                while (stopwatch.ElapsedTicks < nextTick)
                {
                    Thread.SpinWait(1);
                }
            }
        }
        catch (OperationCanceledException)
        {
            // 正常取消，退出循环。
        }
    }

    private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence)
    {
        return new FanucJ519Command(
            sequence,
            source.TargetJoints,
            source.LastData,
            source.ReadIoType,
            source.ReadIoIndex,
            source.ReadIoMask,
            source.DataStyle,
            source.WriteIoType,
            source.WriteIoIndex,
            source.WriteIoMask,
            source.WriteIoValue);
    }

    /// <summary>
    /// 后台接收循环：持续接收 132B 响应并解析。
    /// </summary>
    private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
    {
        if (_udpClient is null)
        {
            return;
        }

        _logger?.LogInformation("J519 ReceiveLoop 启动");
        long receiveCount = 0;
        FanucJ519Response? lastLoggedResponse = null;

        try
        {
            while (!cancellationToken.IsCancellationRequested)
            {
                var result = await _udpClient.ReceiveAsync(cancellationToken).ConfigureAwait(false);
                if (result.Buffer.Length == FanucJ519Protocol.ResponsePacketLength)
                {
                    var response = FanucJ519Protocol.ParseResponse(result.Buffer);
                    lock (_responseLock)
                    {
                        _latestResponse = response;
                    }

                    receiveCount++;

                    // 仅在状态变化时记录 Info，避免高频日志。
                    if (lastLoggedResponse is null
                        || lastLoggedResponse.Status != response.Status
                        || lastLoggedResponse.RobotInMotion != response.RobotInMotion
                        || lastLoggedResponse.SystemReady != response.SystemReady)
                    {
                        _logger?.LogInformation(
                            "J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, sysrdy={SysRdy}, motion={Motion}, pose=[{Pose}], joints=[{Joints}]",
                            response.Status,
                            response.Sequence,
                            response.AcceptsCommand,
                            response.SystemReady,
                            response.RobotInMotion,
                            string.Join(", ", response.Pose.Select(v => v.ToString("F1"))),
                            string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F2"))));
                        lastLoggedResponse = response;
                    }
                    else if (receiveCount % 1000 == 0)
                    {
                        _logger?.LogDebug("J519 已接收 {Count} 个响应包", receiveCount);
                    }
                }
            }
        }
        catch (OperationCanceledException)
        {
            _logger?.LogInformation("J519 ReceiveLoop 正常取消，共接收 {Count} 个包", receiveCount);
        }
        catch (ObjectDisposedException)
        {
            _logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
        }
    }
}
