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>
    /// 验证启动运动后必须等到状态包到达，不能由上位机本地 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>
    /// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
    /// </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_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>
    /// 验证在连接前调用 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>
    /// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
    /// </summary>
    private async Task SendStatusPacketAsync(IPEndPoint clientEndpoint, uint sequence)
    {
        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;
        await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
    }
}