FlyShotHost/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs

using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;
using Flyshot.Runtime.Fanuc.Protocol;
using Microsoft.Extensions.Logging;

namespace Flyshot.Core.Tests;

/// <summary>
/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、状态包驱动发送与响应解析。
/// </summary>
public sealed class FanucJ519ClientTests : IDisposable
{
    private readonly UdpClient _server;
    private readonly CancellationTokenSource _cts = new();

    /// <summary>
    /// 在随机可用端口启动本地 UDP 模拟控制器。
    /// </summary>
    public FanucJ519ClientTests()
    {
        _server = new UdpClient(0);
    }

    /// <summary>
    /// 获取分配给本地模拟控制器的端口。
    /// </summary>
    private int Port => ((IPEndPoint)_server.Client.LocalEndPoint!).Port;

    /// <summary>
    /// 清理模拟控制器和取消源。
    /// </summary>
    public void Dispose()
    {
        _cts.Cancel();
        _server.Dispose();
        _cts.Dispose();
    }

    /// <summary>
    /// 验证连接时会发送初始化包。
    /// </summary>
    [Fact]
    public async Task ConnectAsync_SendsInitPacket()
    {
        using var client = new FanucJ519Client();
        var receiveTask = _server.ReceiveAsync(_cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);

        Assert.True(client.IsConnected);
        var result = await receiveTask.AsTask().WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
        Assert.Equal(FanucJ519Protocol.ControlPacketLength, result.Buffer.Length);
        Assert.Equal(Convert.FromHexString("0000000000000001"), result.Buffer);
    }

    /// <summary>
    /// 验证启动运动后必须等到状态包到达，不能由上位机本地 8ms 循环主动发命令。
    /// </summary>
    [Fact]
    public async Task StartMotion_WaitsForStatusPacketBeforeSendingCommand()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);

        // 接收并丢弃初始化包。
        await _server.ReceiveAsync(_cts.Token);

        var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
        client.UpdateCommand(command);
        client.StartMotion();

        // 机器人尚未回状态包时，上位机不应自行发 64B command packet。
        await Assert.ThrowsAsync<TimeoutException>(
            () => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120)));

        await client.StopMotionAsync(_cts.Token);
    }

    /// <summary>
    /// 验证停止运动时会发送结束包。
    /// </summary>
    [Fact]
    public async Task StopMotionAsync_SendsEndPacket()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);

        // 接收并丢弃初始化包。
        await _server.ReceiveAsync(_cts.Token);

        await client.StopMotionAsync(_cts.Token);

        // 服务器应该收到结束包。
        var endResult = await _server.ReceiveAsync(_cts.Token);
        Assert.Equal(FanucJ519Protocol.ControlPacketLength, endResult.Buffer.Length);
        Assert.Equal(Convert.FromHexString("0000000200000001"), endResult.Buffer);
    }

    /// <summary>
    /// 验证响应解析和最新响应缓存。
    /// </summary>
    [Fact]
    public async Task GetLatestResponse_ParsesIncomingResponse()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);

        // 接收初始化包，获取客户端端点。
        var initResult = await _server.ReceiveAsync(_cts.Token);
        var clientEndpoint = initResult.RemoteEndPoint;

        // 构造 132B 响应包并发送回客户端。
        var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), 5);
        responsePacket[0x0c] = 15; // 所有状态位为真。
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x14, 4), 999u);
        BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x18, 4), 10.0f);
        BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x3c, 4), 0.5f);
        BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x60, 4), 1.0f);

        await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);

        // 给接收循环留出时间。
        await Task.Delay(200, _cts.Token);

        var latest = client.GetLatestResponse();
        Assert.NotNull(latest);
        Assert.Equal(5u, latest.Sequence);
        Assert.True(latest.AcceptsCommand);
        Assert.True(latest.ReceivedCommand);
        Assert.True(latest.SystemReady);
        Assert.True(latest.RobotInMotion);
        Assert.Equal(999u, latest.Timestamp);
        Assert.Equal(10.0, latest.Pose[0], precision: 6);
        Assert.Equal(0.5, latest.JointDegrees[0], precision: 6);
        Assert.Equal(1.0, latest.MotorCurrents[0], precision: 6);
    }

    /// <summary>
    /// 验证收到状态包后，下一帧命令使用该状态包的序号。
    /// </summary>
    [Fact]
    public async Task StartMotion_UsesLatestStatusSequenceForFirstCommand()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
        client.UpdateCommand(command);
        client.StartMotion();

        await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);

        var result = await _server.ReceiveAsync(_cts.Token);
        Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
        Assert.Equal(100u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
        Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result.Buffer.AsSpan(0x1c, 4)), precision: 6);

        await client.StopMotionAsync(_cts.Token);
    }

    /// <summary>
    /// 验证直角坐标命令会把 Data format 写为 0，并按通用目标槽位写入 X/Y/Z/W/P/R。
    /// </summary>
    [Fact]
    public void PackCommandPacket_WritesCartesianDataFormatAndTargetValues()
    {
        var command = new FanucJ519Command(
            sequence: 7,
            targetValues: [100.0, 200.0, 300.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0],
            dataStyle: 0);

        var packet = FanucJ519Protocol.PackCommandPacket(command);

        Assert.Equal(0, packet[0x12]);
        Assert.Equal(100.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
        Assert.Equal(200.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x20, 4)));
        Assert.Equal(300.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x24, 4)));
        Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x28, 4)));
        Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x2c, 4)));
        Assert.Equal(3.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
    }

    /// <summary>
    /// 验证配置 J519 buffer size 后，实际回发命令序号会在状态包序号基础上增加该缓冲深度。
    /// </summary>
    [Fact]
    public async Task StartMotion_AddsConfiguredBufferSizeToStatusSequence()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        client.SetSequenceBufferSize(8);
        client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
        client.StartMotion();

        await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);

        var result = await _server.ReceiveAsync(_cts.Token);
        Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
        Assert.Equal(108u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));

        await client.StopMotionAsync(_cts.Token);
    }

    /// <summary>
    /// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
    /// </summary>
    [Fact]
    public async Task StartMotion_SendsOneCommandForEachStatusPacketWithMatchingSequence()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        var command = new FanucJ519Command(sequence: 99, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
        client.UpdateCommand(command);
        client.StartMotion();

        var packets = new List<byte[]>();
        for (uint sequence = 100; sequence < 104; sequence++)
        {
            await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
            var result = await _server.ReceiveAsync(_cts.Token);
            packets.Add(result.Buffer);
        }

        await client.StopMotionAsync(_cts.Token);

        var sequences = packets
            .Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
            .ToArray();
        Assert.Equal([100u, 101u, 102u, 103u], sequences);
        Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
    }

    /// <summary>
    /// 验证预装命令队列会被机器人状态包逐帧出队，队列耗尽后继续保持最后目标。
    /// </summary>
    [Fact]
    public async Task StartMotion_DequeuesPreparedCommandsForStatusPacketsAndHoldsLastCommand()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        client.StartMotion();
        client.LoadCommandQueue(
        [
            new FanucJ519Command(sequence: 0, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
            new FanucJ519Command(sequence: 0, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
            new FanucJ519Command(sequence: 0, targetJoints: [3.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        ]);

        var packets = new List<byte[]>();
        for (uint sequence = 700; sequence < 704; sequence++)
        {
            await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
            var result = await _server.ReceiveAsync(_cts.Token);
            packets.Add(result.Buffer);
        }

        await client.StopMotionAsync(_cts.Token);

        var sequences = packets
            .Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
            .ToArray();
        var firstJointTargets = packets
            .Select(packet => BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)))
            .ToArray();

        Assert.Equal([700u, 701u, 702u, 703u], sequences);
        Assert.Equal([1.0f, 2.0f, 3.0f, 3.0f], firstJointTargets);
        Assert.True(client.IsCommandQueueDrainedForTests());
    }

    /// <summary>
    /// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
    /// </summary>
    [Fact]
    public async Task StartMotion_CanRestartAfterStopMotionAndUseNewStatusSequence()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        client.UpdateCommand(new FanucJ519Command(sequence: 10, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
        client.StartMotion();

        await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 200);
        var first = await _server.ReceiveAsync(_cts.Token);
        var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4));
        Assert.Equal(200u, firstSequence);

        await client.StopMotionAsync(_cts.Token);

        byte[] packet;
        do
        {
            packet = (await _server.ReceiveAsync(_cts.Token)).Buffer;
        }
        while (packet.Length != FanucJ519Protocol.ControlPacketLength);

        client.UpdateCommand(new FanucJ519Command(sequence: 20, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
        client.StartMotion();

        await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 300);
        var restarted = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);

        Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length);
        Assert.Equal(300u, BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)));
        Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6);

        await client.StopMotionAsync(_cts.Token);
    }

    /// <summary>
    /// 验证没有显式目标时，会使用最近一帧状态反馈里的关节角持续构造 hold 命令。
    /// </summary>
    [Fact]
    public async Task StartMotion_WithoutExplicitCommand_HoldsLatestResponseJointDegrees()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        client.StartMotion();

        await SendStatusPacketAsync(
            initResult.RemoteEndPoint,
            sequence: 401,
            jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]);
        var holdPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);

        Assert.Equal(FanucJ519Protocol.CommandPacketLength, holdPacket.Buffer.Length);
        Assert.Equal(401u, BinaryPrimitives.ReadUInt32BigEndian(holdPacket.Buffer.AsSpan(0x08, 4)));
        Assert.Equal(10.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x1c, 4)), precision: 6);
        Assert.Equal(20.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x20, 4)), precision: 6);
        Assert.Equal(60.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x30, 4)), precision: 6);

        await client.StopMotionAsync(_cts.Token);
    }

    /// <summary>
    /// 验证存在显式目标时，状态反馈生成的 hold 命令不会覆盖当前待发送目标。
    /// </summary>
    [Fact]
    public async Task StartMotion_WithExplicitCommand_PrefersExplicitTargetOverHoldCommand()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
        client.StartMotion();

        await SendStatusPacketAsync(
            initResult.RemoteEndPoint,
            sequence: 402,
            jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]);
        var commandPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);

        Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandPacket.Buffer.Length);
        Assert.Equal(402u, BinaryPrimitives.ReadUInt32BigEndian(commandPacket.Buffer.AsSpan(0x08, 4)));
        Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x1c, 4)), precision: 6);
        Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x20, 4)), precision: 6);
        Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x30, 4)), precision: 6);

        await client.StopMotionAsync(_cts.Token);
    }

    /// <summary>
    /// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
    /// </summary>
    [Fact]
    public void StartMotion_BeforeConnect_Throws()
    {
        using var client = new FanucJ519Client();
        Assert.Throws<InvalidOperationException>(() => client.StartMotion());
    }

    /// <summary>
    /// 验证状态包驱动发送能持续输出命令包。
    /// </summary>
    [Fact]
    public async Task StartMotion_MaintainsStatusDrivenCommandStream()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
        client.UpdateCommand(command);
        client.StartMotion();

        // 收集 5 个命令包到达时间戳和序号。
        var timestamps = new List<DateTimeOffset>();
        var sequences = new List<uint>();
        for (var i = 0; i < 5; i++)
        {
            await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: (uint)(500 + i));
            var result = await _server.ReceiveAsync(_cts.Token);
            Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
            sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
            timestamps.Add(DateTimeOffset.UtcNow);
        }

        await client.StopMotionAsync(_cts.Token);

        Assert.Equal([500u, 501u, 502u, 503u, 504u], sequences);

        // 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
        var intervals = new List<TimeSpan>();
        for (var i = 1; i < timestamps.Count; i++)
        {
            intervals.Add(timestamps[i] - timestamps[i - 1]);
        }

        Assert.All(intervals, interval =>
        {
            Assert.True(interval > TimeSpan.Zero, $"间隔 {interval.TotalMilliseconds:F2}ms 必须为正。");
            Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
        });
    }

    /// <summary>
    /// 验证状态变化日志会附带最近一次实际发送的目标关节轴，便于联调时对照控制目标。
    /// </summary>
    [Fact]
    public async Task ReceiveLoop_LogsLastSentTargetJointsWhenStatusChanges()
    {
        var logger = new CapturingLogger<FanucJ519Client>();
        using var client = new FanucJ519Client(logger);
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var initResult = await _server.ReceiveAsync(_cts.Token);

        client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
        client.StartMotion();

        await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 42);
        _ = await _server.ReceiveAsync(_cts.Token);
        await Task.Delay(200, _cts.Token);

        Assert.Contains(
            logger.Entries,
            entry => entry.Level == LogLevel.Information
                && entry.Message.Contains("J519 最后一条发送目标关节轴", StringComparison.Ordinal)
                && entry.Message.Contains("1.00000, 2.00000, 3.00000, 4.00000, 5.00000, 6.00000", StringComparison.Ordinal));
    }

    /// <summary>
    /// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
    /// </summary>
    private async Task SendStatusPacketAsync(
        IPEndPoint clientEndpoint,
        uint sequence,
        IReadOnlyList<double>? jointDegrees = null)
    {
        var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
        responsePacket[0x0c] = 15;

        if (jointDegrees is not null)
        {
            for (var index = 0; index < Math.Min(6, jointDegrees.Count); index++)
            {
                BinaryPrimitives.WriteSingleBigEndian(
                    responsePacket.AsSpan(0x3c + index * sizeof(float), sizeof(float)),
                    (float)jointDegrees[index]);
            }
        }

        await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
    }

    /// <summary>
    /// 收集测试过程中的结构化日志，便于断言运行期输出内容。
    /// </summary>
    private sealed class CapturingLogger<T> : ILogger<T>
    {
        /// <summary>
        /// 获取已记录的日志条目。
        /// </summary>
        public List<LogEntry> Entries { get; } = [];

        /// <summary>
        /// 开始日志作用域；当前测试无需作用域，直接返回空对象。
        /// </summary>
        public IDisposable BeginScope<TState>(TState state)
            where TState : notnull
        {
            return NullScope.Instance;
        }

        /// <summary>
        /// 指示所有日志级别均启用，便于测试完整捕获输出。
        /// </summary>
        public bool IsEnabled(LogLevel logLevel)
        {
            return true;
        }

        /// <summary>
        /// 记录一条格式化后的日志消息。
        /// </summary>
        public void Log<TState>(
            LogLevel logLevel,
            EventId eventId,
            TState state,
            Exception? exception,
            Func<TState, Exception?, string> formatter)
        {
            Entries.Add(new LogEntry(logLevel, formatter(state, exception)));
        }

        /// <summary>
        /// 表示一次日志记录。
        /// </summary>
        public sealed record LogEntry(LogLevel Level, string Message);

        /// <summary>
        /// 提供空日志作用域，避免测试中额外分配。
        /// </summary>
        private sealed class NullScope : IDisposable
        {
            /// <summary>
            /// 获取单例空作用域。
            /// </summary>
            public static NullScope Instance { get; } = new();

            /// <summary>
            /// 释放空作用域；无需实际动作。
            /// </summary>
            public void Dispose()
            {
            }
        }
    }
}