using System.Buffers.Binary;

namespace Flyshot.Runtime.Fanuc.Protocol;

/// <summary>
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧命令数据。
/// </summary>
public sealed class FanucJ519Command
{
    private readonly double[] _targetJoints;

    /// <summary>
    /// 初始化 J519 命令数据。
    /// </summary>
    /// <param name="sequence">命令序号。</param>
    /// <param name="targetJoints">目标关节或扩展轴数据，最多 9 个槽位。</param>
    /// <param name="lastData">是否为最后一帧数据。</param>
    /// <param name="readIoType">读取 IO 类型。</param>
    /// <param name="readIoIndex">读取 IO 起始索引。</param>
    /// <param name="readIoMask">读取 IO 掩码。</param>
    /// <param name="dataStyle">目标数据类型。</param>
    /// <param name="writeIoType">写入 IO 类型。</param>
    /// <param name="writeIoIndex">写入 IO 起始索引。</param>
    /// <param name="writeIoMask">写入 IO 掩码。</param>
    /// <param name="writeIoValue">写入 IO 数值。</param>
    public FanucJ519Command(
        uint sequence,
        IReadOnlyList<double> targetJoints,
        byte lastData = 0,
        byte readIoType = 2,
        ushort readIoIndex = 1,
        ushort readIoMask = 255,
        byte dataStyle = 1,
        byte writeIoType = 2,
        ushort writeIoIndex = 1,
        ushort writeIoMask = 0,
        ushort writeIoValue = 0)
    {
        ArgumentNullException.ThrowIfNull(targetJoints);
        if (targetJoints.Count is <= 0 or > 9)
        {
            throw new ArgumentOutOfRangeException(nameof(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。");
        }

        Sequence = sequence;
        LastData = lastData;
        ReadIoType = readIoType;
        ReadIoIndex = readIoIndex;
        ReadIoMask = readIoMask;
        DataStyle = dataStyle;
        WriteIoType = writeIoType;
        WriteIoIndex = writeIoIndex;
        WriteIoMask = writeIoMask;
        WriteIoValue = writeIoValue;
        _targetJoints = targetJoints.ToArray();
    }

    /// <summary>
    /// 获取命令序号。
    /// </summary>
    public uint Sequence { get; }

    /// <summary>
    /// 获取是否为最后一帧数据。
    /// </summary>
    public byte LastData { get; }

    /// <summary>
    /// 获取读取 IO 类型。
    /// </summary>
    public byte ReadIoType { get; }

    /// <summary>
    /// 获取读取 IO 起始索引。
    /// </summary>
    public ushort ReadIoIndex { get; }

    /// <summary>
    /// 获取读取 IO 掩码。
    /// </summary>
    public ushort ReadIoMask { get; }

    /// <summary>
    /// 获取目标数据类型。
    /// </summary>
    public byte DataStyle { get; }

    /// <summary>
    /// 获取写入 IO 类型。
    /// </summary>
    public byte WriteIoType { get; }

    /// <summary>
    /// 获取写入 IO 起始索引。
    /// </summary>
    public ushort WriteIoIndex { get; }

    /// <summary>
    /// 获取写入 IO 掩码。
    /// </summary>
    public ushort WriteIoMask { get; }

    /// <summary>
    /// 获取写入 IO 数值。
    /// </summary>
    public ushort WriteIoValue { get; }

    /// <summary>
    /// 获取目标关节或扩展轴数据。
    /// </summary>
    public IReadOnlyList<double> TargetJoints => _targetJoints;
}

/// <summary>
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧响应数据。
/// </summary>
public sealed class FanucJ519Response
{
    private readonly double[] _pose;
    private readonly double[] _externalAxes;
    private readonly double[] _jointDegrees;
    private readonly double[] _motorCurrents;

    /// <summary>
    /// 初始化 J519 响应数据。
    /// </summary>
    /// <param name="messageType">响应类型。</param>
    /// <param name="version">协议版本。</param>
    /// <param name="sequence">响应序号。</param>
    /// <param name="status">状态位集合。</param>
    /// <param name="readIoType">读取 IO 类型。</param>
    /// <param name="readIoIndex">读取 IO 起始索引。</param>
    /// <param name="readIoMask">读取 IO 掩码。</param>
    /// <param name="readIoValue">读取 IO 数值。</param>
    /// <param name="timestamp">控制器时间戳。</param>
    /// <param name="pose">TCP 笛卡尔位姿。</param>
    /// <param name="externalAxes">扩展轴反馈。</param>
    /// <param name="jointDegrees">关节角度反馈。</param>
    /// <param name="motorCurrents">电机电流反馈。</param>
    public FanucJ519Response(
        uint messageType,
        uint version,
        uint sequence,
        byte status,
        byte readIoType,
        ushort readIoIndex,
        ushort readIoMask,
        ushort readIoValue,
        uint timestamp,
        IEnumerable<double> pose,
        IEnumerable<double> externalAxes,
        IEnumerable<double> jointDegrees,
        IEnumerable<double> motorCurrents)
    {
        MessageType = messageType;
        Version = version;
        Sequence = sequence;
        Status = status;
        ReadIoType = readIoType;
        ReadIoIndex = readIoIndex;
        ReadIoMask = readIoMask;
        ReadIoValue = readIoValue;
        Timestamp = timestamp;
        _pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
        _externalAxes = externalAxes?.ToArray() ?? throw new ArgumentNullException(nameof(externalAxes));
        _jointDegrees = jointDegrees?.ToArray() ?? throw new ArgumentNullException(nameof(jointDegrees));
        _motorCurrents = motorCurrents?.ToArray() ?? throw new ArgumentNullException(nameof(motorCurrents));
    }

    /// <summary>
    /// 获取响应类型。
    /// </summary>
    public uint MessageType { get; }

    /// <summary>
    /// 获取协议版本。
    /// </summary>
    public uint Version { get; }

    /// <summary>
    /// 获取响应序号。
    /// </summary>
    public uint Sequence { get; }

    /// <summary>
    /// 获取状态位集合。
    /// </summary>
    public byte Status { get; }

    /// <summary>
    /// 获取读取 IO 类型。
    /// </summary>
    public byte ReadIoType { get; }

    /// <summary>
    /// 获取读取 IO 起始索引。
    /// </summary>
    public ushort ReadIoIndex { get; }

    /// <summary>
    /// 获取读取 IO 掩码。
    /// </summary>
    public ushort ReadIoMask { get; }

    /// <summary>
    /// 获取读取 IO 数值。
    /// </summary>
    public ushort ReadIoValue { get; }

    /// <summary>
    /// 获取控制器时间戳。
    /// </summary>
    public uint Timestamp { get; }

    /// <summary>
    /// 获取 TCP 笛卡尔位姿。
    /// </summary>
    public IReadOnlyList<double> Pose => _pose;

    /// <summary>
    /// 获取扩展轴反馈。
    /// </summary>
    public IReadOnlyList<double> ExternalAxes => _externalAxes;

    /// <summary>
    /// 获取关节角度反馈。
    /// </summary>
    public IReadOnlyList<double> JointDegrees => _jointDegrees;

    /// <summary>
    /// 获取电机电流反馈。
    /// </summary>
    public IReadOnlyList<double> MotorCurrents => _motorCurrents;

    /// <summary>
    /// 获取控制器是否接受命令。
    /// </summary>
    public bool AcceptsCommand => (Status & 0b0001) != 0;

    /// <summary>
    /// 获取控制器是否已收到命令。
    /// </summary>
    public bool ReceivedCommand => (Status & 0b0010) != 0;

    /// <summary>
    /// 获取控制器系统是否就绪。
    /// </summary>
    public bool SystemReady => (Status & 0b0100) != 0;

    /// <summary>
    /// 获取机器人是否处于运动中。
    /// </summary>
    public bool RobotInMotion => (Status & 0b1000) != 0;
}

/// <summary>
/// 提供 FANUC UDP 60015 J519/ICSP 伺服流的基础封包与响应解析能力。
/// </summary>
public static class FanucJ519Protocol
{
    /// <summary>
    /// J519 初始化和结束控制包长度。
    /// </summary>
    public const int ControlPacketLength = 8;

    /// <summary>
    /// J519 命令包长度。
    /// </summary>
    public const int CommandPacketLength = 64;

    /// <summary>
    /// J519 响应包长度。
    /// </summary>
    public const int ResponsePacketLength = 132;

    /// <summary>
    /// 封装 J519 初始化包。
    /// </summary>
    /// <returns>初始化包。</returns>
    public static byte[] PackInitPacket()
    {
        return PackControlPacket(0);
    }

    /// <summary>
    /// 封装 J519 结束包。
    /// </summary>
    /// <returns>结束包。</returns>
    public static byte[] PackEndPacket()
    {
        return PackControlPacket(2);
    }

    /// <summary>
    /// 封装 J519 64 字节命令包。
    /// </summary>
    /// <param name="command">命令数据。</param>
    /// <returns>命令包。</returns>
    public static byte[] PackCommandPacket(FanucJ519Command command)
    {
        ArgumentNullException.ThrowIfNull(command);

        var packet = new byte[CommandPacketLength];
        BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, sizeof(uint)), 1);
        BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, sizeof(uint)), 1);
        BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, sizeof(uint)), command.Sequence);
        packet[0x0c] = command.LastData;
        packet[0x0d] = command.ReadIoType;
        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, sizeof(ushort)), command.ReadIoIndex);
        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, sizeof(ushort)), command.ReadIoMask);
        packet[0x12] = command.DataStyle;
        packet[0x13] = command.WriteIoType;
        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x14, sizeof(ushort)), command.WriteIoIndex);
        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x16, sizeof(ushort)), command.WriteIoMask);
        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x18, sizeof(ushort)), command.WriteIoValue);
        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x1a, sizeof(ushort)), 0);

        // J519 命令包固定保留 9 个 f32 目标槽位，少于 9 个时剩余槽位补零。
        for (var index = 0; index < 9; index++)
        {
            var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
            BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
        }

        return packet;
    }

    /// <summary>
    /// 解析 J519 132 字节响应包。
    /// </summary>
    /// <param name="packet">响应包。</param>
    /// <returns>响应解析结果。</returns>
    public static FanucJ519Response ParseResponse(ReadOnlySpan<byte> packet)
    {
        if (packet.Length != ResponsePacketLength)
        {
            throw new InvalidDataException("FANUC J519 响应包长度不正确。");
        }

        return new FanucJ519Response(
            BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x00, sizeof(uint))),
            BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x04, sizeof(uint))),
            BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x08, sizeof(uint))),
            packet[0x0c],
            packet[0x0d],
            BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort))),
            BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x10, sizeof(ushort))),
            BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x12, sizeof(ushort))),
            BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x14, sizeof(uint))),
            ReadFloatArray(packet, 0x18, 6),
            ReadFloatArray(packet, 0x30, 3),
            ReadFloatArray(packet, 0x3c, 9),
            ReadFloatArray(packet, 0x60, 9));
    }

    /// <summary>
    /// 封装 J519 控制包。
    /// </summary>
    /// <param name="packetType">控制包类型。</param>
    /// <returns>控制包。</returns>
    private static byte[] PackControlPacket(uint packetType)
    {
        var packet = new byte[ControlPacketLength];
        BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0, sizeof(uint)), packetType);
        BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(sizeof(uint), sizeof(uint)), 1);
        return packet;
    }

    /// <summary>
    /// 从响应包中读取固定长度 f32 数组。
    /// </summary>
    /// <param name="packet">响应包。</param>
    /// <param name="offset">数组起始偏移。</param>
    /// <param name="count">数组元素数量。</param>
    /// <returns>转换成 double 的数值数组。</returns>
    private static double[] ReadFloatArray(ReadOnlySpan<byte> packet, int offset, int count)
    {
        var values = new double[count];
        for (var index = 0; index < count; index++)
        {
            values[index] = BinaryPrimitives.ReadSingleBigEndian(packet.Slice(offset + (index * sizeof(float)), sizeof(float)));
        }

        return values;
    }
}