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

using System.Buffers.Binary;
using System.Globalization;
using Flyshot.Runtime.Fanuc.Protocol;

namespace Flyshot.Core.Tests;

/// <summary>
/// 使用 2026-04-28 UTTC 真实抓包验证 J519 主运行点位与 JointDetialTraj 重采样规则一致。
/// </summary>
public sealed class UttcJ519GoldenTests
{
    private const int JointCount = 6;
    private const int RobotJ519Port = 60015;
    private const double ServoPeriodSeconds = 0.008;

    public static IEnumerable<object[]> SpeedSweepCases()
    {
        yield return ["2026042802-0.5.pcap", 0.5, 1851, 14.800309];
        yield return ["2026042802-0.7.pcap", 0.7, 1322, 10.568313];
        yield return ["2026042802-1.pcap", 1.0, 926, 7.400125];
    }

    /// <summary>
    /// 验证 speed=0.5/0.7/1.0 三份真实抓包都符合当前运行时采用的发送点生成规则。
    /// </summary>
    [Theory]
    [MemberData(nameof(SpeedSweepCases))]
    public void CapturedJ519Run_ReplaysJointDetailTrajectoryWithSpeedRatio(
        string pcapFileName,
        double speedRatio,
        int expectedPointCount,
        double expectedSendDurationSeconds)
    {
        var repositoryRoot = FindRepositoryRoot();
        var pcapPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", pcapFileName);
        var jointDetailPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");

        var packets = ParsePcapUdp(pcapPath);
        var hostPort = DetectHostJ519Port(packets);
        var commands = ParseJ519Commands(packets, hostPort);
        var responses = ParseJ519Responses(packets, hostPort);
        var responseSegment = LongestStatusSegment(responses, status: 15);
        var jointRows = ReadJointDetail(jointDetailPath);
        var expected = GenerateExpectedPoints(jointRows, speedRatio);

        var commandBySequence = new Dictionary<uint, CapturedJ519Command>();
        var duplicateSequenceCount = 0;
        foreach (var command in commands)
        {
            if (!commandBySequence.TryAdd(command.Sequence, command))
            {
                duplicateSequenceCount++;
            }
        }

        var (startSequence, windowRms) = FindBestCommandWindow(commandBySequence, expected, responseSegment, searchRadius: 32);
        var actualRun = Enumerable
            .Range(0, expected.Length)
            .Select(index => commandBySequence[startSequence + (uint)index])
            .ToArray();
        var comparison = Compare(actualRun, expected);

        Assert.Equal(464, jointRows.Length);
        Assert.Equal(expectedPointCount, expected.Length);
        Assert.Equal(expectedPointCount, actualRun.Length);
        Assert.Equal(0, duplicateSequenceCount);
        Assert.Equal(17, comparison.IoSetPulses);
        Assert.Equal(17, comparison.IoClearFrames);
        Assert.Equal(
            new ushort[] { 10, 12, 14 },
            actualRun
                .Where(static command => command.WriteIoMask != 0)
                .Select(static command => command.WriteIoMask)
                .Distinct()
                .Order()
                .ToArray());
        Assert.True(responseSegment.Length >= expectedPointCount - 1, $"status=15 segment too short: {responseSegment.Length}");
        Assert.InRange((long)responseSegment[0].Sequence - startSequence, 2, 8);
        Assert.All(actualRun, static command => Assert.Equal(0, command.LastData));
        for (var index = 0; index < actualRun.Length; index++)
        {
            Assert.Equal(startSequence + (uint)index, actualRun[index].Sequence);
        }

        Assert.True(windowRms < 0.012, $"J519 global RMS {windowRms:F9} deg exceeds tolerance.");
        Assert.True(comparison.GlobalMaxAbsDeg < 0.07, $"J519 max abs diff {comparison.GlobalMaxAbsDeg:F9} deg exceeds tolerance.");

        var sendDuration = actualRun[^1].TimeRelativeSeconds - actualRun[0].TimeRelativeSeconds;
        Assert.InRange(sendDuration, expectedSendDurationSeconds - 0.04, expectedSendDurationSeconds + 0.04);
    }

    private static string FindRepositoryRoot()
    {
        var directory = new DirectoryInfo(AppContext.BaseDirectory);
        while (directory is not null)
        {
            var jointDetailPath = Path.Combine(directory.FullName, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
            if (File.Exists(jointDetailPath))
            {
                return directory.FullName;
            }

            directory = directory.Parent;
        }

        throw new DirectoryNotFoundException("Cannot locate repository root containing Rvbust/uttc-20260428/Data/JointDetialTraj.txt.");
    }

    private static IReadOnlyList<UdpPacket> ParsePcapUdp(string path)
    {
        using var stream = File.OpenRead(path);
        var header = new byte[24];
        stream.ReadExactly(header);

        var magic = header.AsSpan(0, 4);
        var bigEndian = false;
        var timestampScale = 1_000_000.0;
        if (magic.SequenceEqual(new byte[] { 0xd4, 0xc3, 0xb2, 0xa1 }))
        {
            bigEndian = false;
        }
        else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0xc3, 0xd4 }))
        {
            bigEndian = true;
        }
        else if (magic.SequenceEqual(new byte[] { 0x4d, 0x3c, 0xb2, 0xa1 }))
        {
            bigEndian = false;
            timestampScale = 1_000_000_000.0;
        }
        else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0x3c, 0x4d }))
        {
            bigEndian = true;
            timestampScale = 1_000_000_000.0;
        }
        else
        {
            throw new InvalidDataException($"Unsupported pcap magic: {Convert.ToHexString(header.AsSpan(0, 4))}");
        }

        var linkType = ReadUInt32(header.AsSpan(20, 4), bigEndian);
        if (linkType != 1)
        {
            throw new InvalidDataException($"Only Ethernet pcap is supported, got linktype {linkType}.");
        }

        var packets = new List<UdpPacket>();
        var recordHeader = new byte[16];
        double? firstTimestamp = null;
        var frameNumber = 0;
        while (ReadFullOrEnd(stream, recordHeader))
        {
            frameNumber++;
            var tsSec = ReadUInt32(recordHeader.AsSpan(0, 4), bigEndian);
            var tsFraction = ReadUInt32(recordHeader.AsSpan(4, 4), bigEndian);
            var includedLength = ReadUInt32(recordHeader.AsSpan(8, 4), bigEndian);
            var packet = new byte[includedLength];
            stream.ReadExactly(packet);

            var timestamp = tsSec + (tsFraction / timestampScale);
            firstTimestamp ??= timestamp;
            var udp = ParseEthernetIpv4Udp(packet, frameNumber, timestamp - firstTimestamp.Value);
            if (udp is not null)
            {
                packets.Add(udp);
            }
        }

        return packets;
    }

    private static bool ReadFullOrEnd(Stream stream, byte[] buffer)
    {
        var offset = 0;
        while (offset < buffer.Length)
        {
            var read = stream.Read(buffer, offset, buffer.Length - offset);
            if (read == 0)
            {
                if (offset == 0)
                {
                    return false;
                }

                throw new EndOfStreamException("Truncated pcap record header.");
            }

            offset += read;
        }

        return true;
    }

    private static UdpPacket? ParseEthernetIpv4Udp(byte[] packet, int frameNumber, double timeRelativeSeconds)
    {
        if (packet.Length < 14)
        {
            return null;
        }

        var offset = 14;
        var etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(12, 2));
        if (etherType == 0x8100)
        {
            if (packet.Length < 18)
            {
                return null;
            }

            etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(16, 2));
            offset = 18;
        }

        if (etherType != 0x0800 || packet.Length < offset + 20)
        {
            return null;
        }

        var versionIhl = packet[offset];
        var version = versionIhl >> 4;
        var ihl = (versionIhl & 0x0f) * 4;
        if (version != 4 || ihl < 20 || packet.Length < offset + ihl)
        {
            return null;
        }

        var protocol = packet[offset + 9];
        if (protocol != 17)
        {
            return null;
        }

        var totalLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(offset + 2, 2));
        var udpOffset = offset + ihl;
        if (packet.Length < udpOffset + 8)
        {
            return null;
        }

        var sourcePort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset, 2));
        var destinationPort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 2, 2));
        var udpLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 4, 2));
        var payloadOffset = udpOffset + 8;
        var payloadLength = Math.Max(0, Math.Min(udpLength - 8, totalLength - ihl - 8));
        if (packet.Length < payloadOffset + payloadLength)
        {
            return null;
        }

        return new UdpPacket(
            frameNumber,
            timeRelativeSeconds,
            sourcePort,
            destinationPort,
            packet.AsSpan(payloadOffset, payloadLength).ToArray());
    }

    private static ushort DetectHostJ519Port(IEnumerable<UdpPacket> packets)
    {
        return packets
            .Where(static packet => packet.DestinationPort == RobotJ519Port && packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
            .GroupBy(static packet => packet.SourcePort)
            .OrderByDescending(static group => group.Count())
            .Select(static group => group.Key)
            .First();
    }

    private static CapturedJ519Command[] ParseJ519Commands(IEnumerable<UdpPacket> packets, ushort hostPort)
    {
        return packets
            .Where(packet =>
                packet.SourcePort == hostPort
                && packet.DestinationPort == RobotJ519Port
                && packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
            .Select(ParseCommand)
            .Where(static command => command is not null)
            .Cast<CapturedJ519Command>()
            .ToArray();
    }

    private static CapturedJ519Command? ParseCommand(UdpPacket packet)
    {
        var payload = packet.Payload;
        var messageType = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x00, 4));
        var version = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x04, 4));
        if (messageType != 1 || version != 1)
        {
            return null;
        }

        var targets = new double[9];
        for (var index = 0; index < targets.Length; index++)
        {
            targets[index] = BinaryPrimitives.ReadSingleBigEndian(payload.AsSpan(0x1c + (index * 4), 4));
        }

        return new CapturedJ519Command(
            packet.FrameNumber,
            packet.TimeRelativeSeconds,
            BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x08, 4)),
            payload[0x0c],
            BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x16, 2)),
            BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x18, 2)),
            targets);
    }

    private static FanucJ519Response[] ParseJ519Responses(IEnumerable<UdpPacket> packets, ushort hostPort)
    {
        return packets
            .Where(packet =>
                packet.SourcePort == RobotJ519Port
                && packet.DestinationPort == hostPort
                && packet.Payload.Length == FanucJ519Protocol.ResponsePacketLength)
            .Select(packet => FanucJ519Protocol.ParseResponse(packet.Payload))
            .ToArray();
    }

    private static FanucJ519Response[] LongestStatusSegment(IEnumerable<FanucJ519Response> responses, byte status)
    {
        var best = new List<FanucJ519Response>();
        var current = new List<FanucJ519Response>();
        foreach (var response in responses)
        {
            if (response.Status == status)
            {
                current.Add(response);
                continue;
            }

            if (current.Count > best.Count)
            {
                best = current;
            }

            current = [];
        }

        return (current.Count > best.Count ? current : best).ToArray();
    }

    private static JointRow[] ReadJointDetail(string path)
    {
        return File.ReadLines(path)
            .Where(static line => !string.IsNullOrWhiteSpace(line) && !line.TrimStart().StartsWith('#'))
            .Select(static line =>
            {
                var values = line
                    .Split((char[]?)null, StringSplitOptions.RemoveEmptyEntries)
                    .Select(value => double.Parse(value, CultureInfo.InvariantCulture))
                    .ToArray();
                return new JointRow(values[0], values.Skip(1).Take(JointCount).ToArray());
            })
            .ToArray();
    }

    private static ExpectedPoint[] GenerateExpectedPoints(IReadOnlyList<JointRow> rows, double speedRatio)
    {
        var durationSeconds = rows[^1].TimeSeconds;
        var trajectoryStepSeconds = ServoPeriodSeconds * speedRatio;
        var sampleCount = (int)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
        var points = new ExpectedPoint[sampleCount];
        var segmentIndex = 0;

        for (var index = 0; index < sampleCount; index++)
        {
            var trajectoryTime = Math.Min(index * trajectoryStepSeconds, durationSeconds);
            var jointsRad = Interpolate(rows, trajectoryTime, ref segmentIndex);
            points[index] = new ExpectedPoint(
                index,
                trajectoryTime,
                jointsRad.Select(static value => value * 180.0 / Math.PI).ToArray());
        }

        return points;
    }

    private static double[] Interpolate(IReadOnlyList<JointRow> rows, double trajectoryTime, ref int segmentIndex)
    {
        if (rows.Count == 1 || trajectoryTime <= rows[0].TimeSeconds)
        {
            return rows[0].JointsRad.ToArray();
        }

        var lastIndex = rows.Count - 1;
        if (trajectoryTime >= rows[lastIndex].TimeSeconds)
        {
            return rows[lastIndex].JointsRad.ToArray();
        }

        while (segmentIndex < lastIndex - 1 && rows[segmentIndex + 1].TimeSeconds < trajectoryTime)
        {
            segmentIndex++;
        }

        var start = rows[segmentIndex];
        var end = rows[segmentIndex + 1];
        var duration = end.TimeSeconds - start.TimeSeconds;
        var alpha = duration <= 0.0 ? 0.0 : (trajectoryTime - start.TimeSeconds) / duration;
        var joints = new double[JointCount];
        for (var index = 0; index < joints.Length; index++)
        {
            joints[index] = start.JointsRad[index] + ((end.JointsRad[index] - start.JointsRad[index]) * alpha);
        }

        return joints;
    }

    private static (uint StartSequence, double RmsDeg) FindBestCommandWindow(
        IReadOnlyDictionary<uint, CapturedJ519Command> commandBySequence,
        IReadOnlyList<ExpectedPoint> expected,
        IReadOnlyList<FanucJ519Response> responseSegment,
        int searchRadius)
    {
        if (responseSegment.Count == 0)
        {
            throw new InvalidDataException("No status=15 response segment found.");
        }

        var responseStartSequence = (long)responseSegment[0].Sequence;
        uint? bestStartSequence = null;
        var bestRms = double.PositiveInfinity;

        for (var startSequence = responseStartSequence - searchRadius; startSequence <= responseStartSequence + searchRadius; startSequence++)
        {
            if (startSequence < 0)
            {
                continue;
            }

            var differences = new List<double>(expected.Count * JointCount);
            var completeWindow = true;
            for (var index = 0; index < expected.Count; index++)
            {
                var sequence = (uint)(startSequence + index);
                if (!commandBySequence.TryGetValue(sequence, out var command))
                {
                    completeWindow = false;
                    break;
                }

                for (var joint = 0; joint < JointCount; joint++)
                {
                    differences.Add(command.TargetDegrees[joint] - expected[index].JointsDeg[joint]);
                }
            }

            if (!completeWindow)
            {
                continue;
            }

            var rms = Rms(differences);
            if (rms < bestRms)
            {
                bestRms = rms;
                bestStartSequence = (uint)startSequence;
            }
        }

        if (bestStartSequence is null)
        {
            throw new InvalidDataException("No complete command window found near the status=15 response segment.");
        }

        return (bestStartSequence.Value, bestRms);
    }

    private static ComparisonSummary Compare(IReadOnlyList<CapturedJ519Command> actual, IReadOnlyList<ExpectedPoint> expected)
    {
        var differences = new List<double>(actual.Count * JointCount);
        var maxAbs = 0.0;
        for (var index = 0; index < actual.Count; index++)
        {
            for (var joint = 0; joint < JointCount; joint++)
            {
                var difference = actual[index].TargetDegrees[joint] - expected[index].JointsDeg[joint];
                differences.Add(difference);
                maxAbs = Math.Max(maxAbs, Math.Abs(difference));
            }
        }

        var ioSetPulses = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue != 0);
        var ioClearFrames = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue == 0);
        return new ComparisonSummary(Rms(differences), maxAbs, ioSetPulses, ioClearFrames);
    }

    private static double Rms(IEnumerable<double> values)
    {
        var sum = 0.0;
        var count = 0;
        foreach (var value in values)
        {
            sum += value * value;
            count++;
        }

        return count == 0 ? 0.0 : Math.Sqrt(sum / count);
    }

    private static uint ReadUInt32(ReadOnlySpan<byte> value, bool bigEndian)
    {
        return bigEndian ? BinaryPrimitives.ReadUInt32BigEndian(value) : BinaryPrimitives.ReadUInt32LittleEndian(value);
    }

    private sealed record UdpPacket(
        int FrameNumber,
        double TimeRelativeSeconds,
        ushort SourcePort,
        ushort DestinationPort,
        byte[] Payload);

    private sealed record CapturedJ519Command(
        int FrameNumber,
        double TimeRelativeSeconds,
        uint Sequence,
        byte LastData,
        ushort WriteIoMask,
        ushort WriteIoValue,
        IReadOnlyList<double> TargetDegrees);

    private sealed record JointRow(double TimeSeconds, IReadOnlyList<double> JointsRad);

    private sealed record ExpectedPoint(int Index, double TrajectoryTimeSeconds, IReadOnlyList<double> JointsDeg);

    private sealed record ComparisonSummary(double GlobalRmsDeg, double GlobalMaxAbsDeg, int IoSetPulses, int IoClearFrames);
}