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

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

namespace Flyshot.Core.Tests;

/// <summary>
/// 验证 FANUC TCP 10012 命令客户端的帧收发与响应解析。
/// </summary>
public sealed class FanucCommandClientTests : IDisposable
{
    private readonly TcpListener _listener;
    private readonly CancellationTokenSource _cts = new();

    /// <summary>
    /// 在随机可用端口启动本地模拟控制器。
    /// </summary>
    public FanucCommandClientTests()
    {
        _listener = new TcpListener(IPAddress.Loopback, 0);
        _listener.Start();
    }

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

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

    /// <summary>
    /// 验证命令客户端可以连接本地模拟控制器。
    /// </summary>
    [Fact]
    public async Task ConnectAsync_ConnectsToLocalListener()
    {
        using var client = new FanucCommandClient();
        var acceptTask = _listener.AcceptTcpClientAsync();

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

        Assert.True(client.IsConnected);
        // 确保模拟侧也完成握手
        await acceptTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 StopProgram 命令帧与抓包样本一致，并能解析成功响应。
    /// </summary>
    [Fact]
    public async Task StopProgramAsync_SendsCorrectFrameAndParsesSuccess()
    {
        using var client = new FanucCommandClient();
        var handlerTask = RunSingleResponseControllerAsync(
            Convert.FromHexString("646f7a0000001a0000210300000008525642555354534d7a6f64"),
            Convert.FromHexString("646f7a0000001200002103000000007a6f64"),
            _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.StopProgramAsync("RVBUSTSM", _cts.Token);

        Assert.True(response.IsSuccess);
        Assert.Equal(FanucCommandMessageIds.StopProgram, response.MessageId);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 ResetRobot 空命令帧能正确发送并解析结果响应。
    /// </summary>
    [Fact]
    public async Task ResetRobotAsync_SendsEmptyCommandAndParsesResponse()
    {
        using var client = new FanucCommandClient();
        var expectedFrame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
        var responseFrame = Convert.FromHexString("646f7a0000001200002100000000007a6f64");
        var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.ResetRobotAsync(_cts.Token);

        Assert.True(response.IsSuccess);
        Assert.Equal(FanucCommandMessageIds.ResetRobot, response.MessageId);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 GetProgramStatus 命令帧能正确发送并解析程序状态响应。
    /// </summary>
    [Fact]
    public async Task GetProgramStatusAsync_SendsFrameAndParsesStatusResponse()
    {
        using var client = new FanucCommandClient();
        var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, "RVBUSTSM");
        var responseFrame = Convert.FromHexString("646f7a000000160000200300000000000000017a6f64");
        var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.GetProgramStatusAsync("RVBUSTSM", _cts.Token);

        Assert.True(response.IsSuccess);
        Assert.Equal(FanucCommandMessageIds.GetProgramStatus, response.MessageId);
        Assert.Equal(1u, response.ProgramStatus);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 StartProgram 命令帧能正确发送并解析成功响应。
    /// </summary>
    [Fact]
    public async Task StartProgramAsync_SendsCorrectFrameAndParsesSuccess()
    {
        using var client = new FanucCommandClient();
        var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StartProgram, "RVBUSTSM");
        var responseFrame = Convert.FromHexString("646f7a0000001200002102000000007a6f64");
        var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.StartProgramAsync("RVBUSTSM", _cts.Token);

        Assert.True(response.IsSuccess);
        Assert.Equal(FanucCommandMessageIds.StartProgram, response.MessageId);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 GetSpeedRatio 发送空业务体命令，并按 ratio_int / 100.0 解析倍率。
    /// </summary>
    [Fact]
    public async Task GetSpeedRatioAsync_SendsFrameAndParsesRatio()
    {
        using var client = new FanucCommandClient();
        var handlerTask = RunSingleResponseControllerAsync(
            FanucCommandProtocol.PackGetSpeedRatioCommand(),
            FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetSpeedRatio, Convert.FromHexString("0000005a00000000")),
            _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.GetSpeedRatioAsync(_cts.Token);

        Assert.True(response.IsSuccess);
        Assert.Equal(0.9, response.Ratio, precision: 6);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 SetSpeedRatio 会把 double 倍率夹到 0..100 的整数百分比后下发。
    /// </summary>
    [Fact]
    public async Task SetSpeedRatioAsync_SendsClampedPercentAndParsesSuccess()
    {
        using var client = new FanucCommandClient();
        var handlerTask = RunSingleResponseControllerAsync(
            FanucCommandProtocol.PackSetSpeedRatioCommand(2.0),
            FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetSpeedRatio, Convert.FromHexString("00000000")),
            _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.SetSpeedRatioAsync(2.0, _cts.Token);

        Assert.True(response.IsSuccess);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 GetTcp 会发送 tcp_id 请求，并解析 result_code + tcp_id + 7 个 float 位姿。
    /// </summary>
    [Fact]
    public async Task GetTcpAsync_SendsFrameAndParsesPose()
    {
        using var client = new FanucCommandClient();
        var handlerTask = RunSingleResponseControllerAsync(
            FanucCommandProtocol.PackGetTcpCommand(1),
            FanucCommandProtocol.PackFrame(
                FanucCommandMessageIds.GetTcp,
                Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")),
            _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.GetTcpAsync(1, _cts.Token);

        Assert.True(response.IsSuccess);
        Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], response.Pose);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 SetTcp 会按 tcp_id + 7 个 float 位姿下发并解析结果码。
    /// </summary>
    [Fact]
    public async Task SetTcpAsync_SendsFrameAndParsesSuccess()
    {
        using var client = new FanucCommandClient();
        var handlerTask = RunSingleResponseControllerAsync(
            FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]),
            FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetTcp, Convert.FromHexString("00000000")),
            _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.SetTcpAsync(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], _cts.Token);

        Assert.True(response.IsSuccess);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 GetIo 会按 io_type、io_index 顺序请求，并解析 float IO 值。
    /// </summary>
    [Fact]
    public async Task GetIoAsync_SendsFrameAndParsesValue()
    {
        using var client = new FanucCommandClient();
        var handlerTask = RunSingleResponseControllerAsync(
            FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7),
            FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetIo, Convert.FromHexString("000000003f800000")),
            _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.GetIoAsync(7, "DO", _cts.Token);

        Assert.True(response.IsSuccess);
        Assert.True(response.Value);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证 SetIo 会按 io_type、io_index、float value 顺序下发并解析结果码。
    /// </summary>
    [Fact]
    public async Task SetIoAsync_SendsFrameAndParsesSuccess()
    {
        using var client = new FanucCommandClient();
        var handlerTask = RunSingleResponseControllerAsync(
            FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true),
            FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetIo, Convert.FromHexString("00000000")),
            _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var response = await client.SetIoAsync(7, true, "DO", _cts.Token);

        Assert.True(response.IsSuccess);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证命令响应 result_code 非零时，客户端会抛出可诊断异常而不是让上层误判成功。
    /// </summary>
    [Fact]
    public async Task StopProgramAsync_NonZeroResultCode_ThrowsDiagnosticException()
    {
        using var client = new FanucCommandClient();
        var handlerTask = RunSingleResponseControllerAsync(
            FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StopProgram, "RVBUSTSM"),
            Convert.FromHexString("646f7a00000012000021030000002a7a6f64"),
            _cts.Token);

        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
        var exception = await Assert.ThrowsAsync<InvalidOperationException>(
            () => client.StopProgramAsync("RVBUSTSM", _cts.Token));

        Assert.Contains("0x2103", exception.Message);
        Assert.Contains("42", exception.Message);
        await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
    }

    /// <summary>
    /// 验证在连接前调用命令会抛出 InvalidOperationException。
    /// </summary>
    [Fact]
    public async Task SendProgramCommandAsync_BeforeConnect_Throws()
    {
        using var client = new FanucCommandClient();
        await Assert.ThrowsAsync<InvalidOperationException>(
            () => client.StopProgramAsync("RVBUSTSM", _cts.Token));
    }

    /// <summary>
    /// 启动模拟控制器，接收一条请求帧并比对期望内容，然后返回预设响应。
    /// </summary>
    private async Task RunSingleResponseControllerAsync(
        byte[] expectedFrame,
        byte[] responseFrame,
        CancellationToken cancellationToken)
    {
        using var controller = await _listener.AcceptTcpClientAsync(cancellationToken);
        await using var stream = controller.GetStream();

        var buffer = new byte[expectedFrame.Length];
        await ReadExactAsync(stream, buffer, cancellationToken);
        Assert.Equal(expectedFrame, buffer);

        await stream.WriteAsync(responseFrame, cancellationToken);
    }

    /// <summary>
    /// 从流中精确读取指定长度的字节。
    /// </summary>
    private static async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
    {
        var totalRead = 0;
        while (totalRead < buffer.Length)
        {
            var read = await stream.ReadAsync(buffer.AsMemory(totalRead), cancellationToken);
            if (read == 0)
            {
                throw new IOException("模拟控制器读取到 EOF。");
            }

            totalRead += read;
        }
    }
}