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

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

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>
    /// 验证启动运动后能按周期发送命令包。
    /// </summary>
    [Fact]
    public async Task StartMotion_SendsPeriodicCommands()
    {
        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();

        // 接收至少一个命令包。
        var commandResult = await _server.ReceiveAsync(_cts.Token);
        Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length);
        Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));

        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>
    /// 验证 UpdateCommand 替换当前命令后下一周期发送新命令。
    /// </summary>
    [Fact]
    public async Task UpdateCommand_ReplacesCurrentCommand()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        await _server.ReceiveAsync(_cts.Token); // init

        var command1 = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
        var command2 = new FanucJ519Command(sequence: 2, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]);

        client.UpdateCommand(command1);
        client.StartMotion();

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

        client.UpdateCommand(command2);

        var result2 = await _server.ReceiveAsync(_cts.Token);
        Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
        Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 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>
    /// 验证 Stopwatch + SpinWait 发送循环能保持约 8ms 周期抖动在亚毫秒级。
    /// </summary>
    [Fact]
    public async Task StartMotion_MaintainsSubMillisecondPeriod()
    {
        using var client = new FanucJ519Client();
        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        await _server.ReceiveAsync(_cts.Token); // init

        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>();
        for (var i = 0; i < 5; i++)
        {
            var result = await _server.ReceiveAsync(_cts.Token);
            timestamps.Add(DateTimeOffset.UtcNow);
        }

        await client.StopMotionAsync(_cts.Token);

        // 计算相邻包间隔并断言最大抖动。
        var intervals = new List<TimeSpan>();
        for (var i = 1; i < timestamps.Count; i++)
        {
            intervals.Add(timestamps[i] - timestamps[i - 1]);
        }

        // 允许 ±2ms 的测量误差（含 UDP 传输和调度延迟）。
        Assert.All(intervals, interval =>
        {
            Assert.True(interval >= TimeSpan.FromMilliseconds(6), $"间隔 {interval.TotalMilliseconds:F2}ms 过短。");
            Assert.True(interval <= TimeSpan.FromMilliseconds(10), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
        });
    }
}