FlyShotHost/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs

using System.Buffers.Binary;

namespace Flyshot.Runtime.Fanuc.Protocol;

/// <summary>
/// 表示 FANUC TCP 10010 状态通道中的单个状态帧。
/// </summary>
public sealed class FanucStateFrame
{
    private readonly double[] _pose;
    private readonly double[] _jointOrExtensionValues;
    private readonly double[] _jointDegrees;
    private readonly double[] _externalAxes;
    private readonly uint[] _tailWords;

    /// <summary>
    /// 初始化状态帧解析结果。
    /// </summary>
    /// <param name="messageId">状态帧消息号或序号。</param>
    /// <param name="pose">控制器回传的笛卡尔位姿。</param>
    /// <param name="jointOrExtensionValues">控制器回传的关节或扩展轴状态。</param>
    /// <param name="tailWords">状态帧尾部状态槽位。</param>
    public FanucStateFrame(
        uint messageId,
        IEnumerable<double> pose,
        IEnumerable<double> jointOrExtensionValues,
        IEnumerable<uint> tailWords)
    {
        MessageId = messageId;
        _pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
        _jointOrExtensionValues = jointOrExtensionValues?.ToArray() ?? throw new ArgumentNullException(nameof(jointOrExtensionValues));
        _tailWords = tailWords?.ToArray() ?? throw new ArgumentNullException(nameof(tailWords));

        if (_pose.Length != 6)
        {
            throw new ArgumentException("状态帧位姿必须包含 6 个 float。", nameof(pose));
        }

        if (_jointOrExtensionValues.Length != 9)
        {
            throw new ArgumentException("状态帧关节/扩展轴必须包含 9 个 float。", nameof(jointOrExtensionValues));
        }

        if (_tailWords.Length != 4)
        {
            throw new ArgumentException("状态帧尾部状态字必须包含 4 个 u32。", nameof(tailWords));
        }

        _jointDegrees = _jointOrExtensionValues.Take(6).ToArray();
        _externalAxes = _jointOrExtensionValues.Skip(6).ToArray();
    }

    /// <summary>
    /// 获取状态帧消息号或序号。
    /// </summary>
    public uint MessageId { get; }

    /// <summary>
    /// 获取控制器回传的笛卡尔位姿。
    /// </summary>
    public IReadOnlyList<double> Pose => _pose;

    /// <summary>
    /// 获取控制器回传的笛卡尔位姿 X/Y/Z/W/P/R，单位来自 FANUC 状态服务器。
    /// </summary>
    public IReadOnlyList<double> CartesianPose => _pose;

    /// <summary>
    /// 获取控制器回传的关节或扩展轴状态。
    /// </summary>
    public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;

    /// <summary>
    /// 获取前 6 个机器人关节角度，单位为度。
    /// </summary>
    public IReadOnlyList<double> JointDegrees => _jointDegrees;

    /// <summary>
    /// 获取后 3 个扩展轴槽位。当前现场样本中这些值通常为 0。
    /// </summary>
    public IReadOnlyList<double> ExternalAxes => _externalAxes;

    /// <summary>
    /// 获取状态帧尾部状态槽位。
    /// </summary>
    public IReadOnlyList<uint> TailWords => _tailWords;

    /// <summary>
    /// 获取原始尾部状态字。当前抓包中恒为 [2,0,0,1]，语义暂不强行推断。
    /// </summary>
    public IReadOnlyList<uint> RawTailWords => _tailWords;

    /// <summary>
    /// 获取第 0 个原始尾部状态字。
    /// </summary>
    public uint StatusWord0 => _tailWords[0];

    /// <summary>
    /// 获取第 1 个原始尾部状态字。
    /// </summary>
    public uint StatusWord1 => _tailWords[1];

    /// <summary>
    /// 获取第 2 个原始尾部状态字。
    /// </summary>
    public uint StatusWord2 => _tailWords[2];

    /// <summary>
    /// 获取第 3 个原始尾部状态字。
    /// </summary>
    public uint StatusWord3 => _tailWords[3];
}

/// <summary>
/// 提供 FANUC TCP 10010 状态通道固定帧解析能力。
/// </summary>
public static class FanucStateProtocol
{
    /// <summary>
    /// FANUC 状态通道抓包确认的完整帧长度。
    /// </summary>
    public const int StateFrameLength = 90;

    /// <summary>
    /// 解析 TCP 10010 状态通道中的单个完整状态帧。
    /// </summary>
    /// <param name="frame">完整状态帧。</param>
    /// <returns>状态帧解析结果。</returns>
    public static FanucStateFrame ParseFrame(ReadOnlySpan<byte> frame)
    {
        ValidateFrame(frame);

        var pose = new double[6];
        var jointOrExtensionValues = new double[9];
        var tailWords = new uint[4];

        // 状态帧采用固定布局，偏移来自抓包与 StateServer 逆向结论。
        for (var index = 0; index < pose.Length; index++)
        {
            pose[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(11 + (index * sizeof(float)), sizeof(float)));
        }

        for (var index = 0; index < jointOrExtensionValues.Length; index++)
        {
            jointOrExtensionValues[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(35 + (index * sizeof(float)), sizeof(float)));
        }

        for (var index = 0; index < tailWords.Length; index++)
        {
            tailWords[index] = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(71 + (index * sizeof(uint)), sizeof(uint)));
        }

        return new FanucStateFrame(
            BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint))),
            pose,
            jointOrExtensionValues,
            tailWords);
    }

    /// <summary>
    /// 校验状态帧的长度、magic 和长度字段。
    /// </summary>
    /// <param name="frame">完整状态帧。</param>
    private static void ValidateFrame(ReadOnlySpan<byte> frame)
    {
        if (frame.Length != StateFrameLength)
        {
            throw new InvalidDataException("FANUC 状态帧长度不符合 TCP 10010 固定帧布局。");
        }

        if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
        {
            throw new InvalidDataException("FANUC 状态帧头 magic 不正确。");
        }

        if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
        {
            throw new InvalidDataException("FANUC 状态帧尾 magic 不正确。");
        }

        var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
        if (declaredLength != frame.Length)
        {
            throw new InvalidDataException("FANUC 状态帧长度字段与实际长度不一致。");
        }
    }
}