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