RoboidControl-csharp/Messages.cs

using System.IO;
using System.Threading.Tasks;

using Passer.LinearAlgebra;
namespace Passer.Control.Core {

    public class IMessage {
        public IMessage() { }
        public IMessage(byte[] buffer) {
            Deserialize(buffer);
        }

        public virtual byte Serialize(ref byte[] buffer) { return 0; }
        public virtual void Deserialize(byte[] buffer) { }

        public bool SendTo(Participant client) {
            Serialize(ref client.buffer);
            return client.SendBuffer(client.buffer.Length);
        }

        public static bool SendMsg(Participant client, IMessage msg) {
            msg.Serialize(ref client.buffer);
            return client.SendBuffer(client.buffer.Length);
        }

        public static bool PublishMsg(Participant client, IMessage msg) {
            msg.Serialize(ref client.buffer);
            return client.PublishBuffer(client.buffer.Length);
        }

        public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
            byte[] buffer = new byte[packetSize - 1]; //  without msgId
            int byteCount = dataStream.Read(buffer, 0, packetSize - 1);
            while (byteCount < packetSize - 1) {
                // not all bytes have been read, wait and try again
                await Task.Delay(1);
                byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount);
            }
            return buffer;
        }
    }

    #region Investigate

    public class InvestigateMsg : IMessage {
        public const byte Id = 0x81;
        public const byte length = 3;
        public byte networkId;
        public byte thingId;

        public InvestigateMsg(byte networkId, byte thingId) {
            this.networkId = networkId;
            this.thingId = thingId;
        }
        public InvestigateMsg(byte[] buffer) : base(buffer) { }

        public override byte Serialize(ref byte[] buffer) {
            byte ix = 0;
            buffer[ix++] = InvestigateMsg.Id;
            buffer[ix++] = this.networkId;
            buffer[ix++] = this.thingId;
            return ix;
        }
        public override void Deserialize(byte[] buffer) {
            uint ix = 0;
            this.networkId = buffer[ix++];
            this.thingId = buffer[ix++];
        }

        //public static bool Send(Participant client, CoreThing thing) {
        //    InvestigateMsg msg = new(thing.networkId, thing.id);
        //    return SendMsg(client, msg);
        //}
        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
            if (packetSize != length)
                return false;

            byte[] buffer = await Receive(dataStream, packetSize);
            InvestigateMsg msg = new(buffer);
            //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]");

            client.messageQueue.Enqueue(msg);
            return true;

        }
    }

    #endregion Investigate

    #region Pose

    public class PoseMsg : IMessage {
        public const byte Id = 0x10;
        public const byte length = 4 + 4 + 4;
        public byte networkId;
        public byte thingId;

        public byte poseType;
        public const byte Pose_Position = 0x01;
        public const byte Pose_Orientation = 0x02;
        public const byte Pose_LinearVelocity = 0x04;
        public const byte Pose_AngularVelocity = 0x08;

        public Spherical position;
        public SwingTwist orientation;
        public Spherical linearVelocity;
        public Spherical angularVelocity;

        public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) {
            this.networkId = networkId;
            this.thingId = thingId;
            this.position = position;
            this.orientation = orientation;
            this.poseType = 0;
            if (this.position != null)
                this.poseType |= Pose_Position;
            else
                this.position = new Spherical(0, 0, 0);
            if (this.orientation != null)
                this.poseType |= Pose_Orientation;
            else
                this.orientation = SwingTwist.zero;
        }
        public PoseMsg(byte[] buffer) : base(buffer) { }

        public override byte Serialize(ref byte[] buffer) {
            byte ix = 0;
            buffer[ix++] = PoseMsg.Id;
            buffer[ix++] = this.networkId;
            buffer[ix++] = this.thingId;
            buffer[ix++] = this.poseType;

            if ((poseType & Pose_Position) != 0)
                LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
            if ((poseType & Pose_Orientation) != 0)
                LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
            if ((poseType & Pose_LinearVelocity) != 0)
                LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
            if ((poseType & Pose_AngularVelocity) != 0)
                LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
            return ix;
        }
        public override void Deserialize(byte[] buffer) {
            byte ix = 0;
            this.networkId = buffer[ix++];
            this.thingId = buffer[ix++];
            this.poseType = buffer[ix++];

            if ((poseType & Pose_Position) != 0)
                this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
            if ((poseType & Pose_Orientation) != 0)
                this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix);
            if ((poseType & Pose_LinearVelocity) != 0)
                this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
            if ((poseType & Pose_AngularVelocity) != 0)
                this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
        }

        public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) {
            PoseMsg msg = new(client.networkId, thingId, position, orientation);
            return SendMsg(client, msg);
        }
        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
            byte[] buffer = await Receive(dataStream, packetSize);
            PoseMsg msg = new(buffer);

            // Do no process poses with nwid == 0 (== local)
            if (msg.networkId == 0)
                return true;

            client.messageQueue.Enqueue(msg);
            return true;
        }

        public static bool SendTo(Participant participant, Thing thing) {
            if (participant == null || thing == null)
                return false;

            byte ix = 0;
            participant.buffer[ix++] = PoseMsg.Id;
            participant.buffer[ix++] = participant.networkId;
            participant.buffer[ix++] = thing.id;
            participant.buffer[ix++] = thing.poseUpdated;

            if ((thing.poseUpdated & Pose_Position) != 0 && thing.position != null)
                LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.position);
            if ((thing.poseUpdated & Pose_Orientation) != 0 && thing.orientation != null)
                LowLevelMessages.SendQuat32(participant.buffer, ref ix, thing.orientation);
            if ((thing.poseUpdated & Pose_LinearVelocity) != 0 && thing.linearVelocity != null)
                LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.linearVelocity);
            if ((thing.poseUpdated & Pose_AngularVelocity) != 0 && thing.angularVelocity != null)
                LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.angularVelocity);

            return participant.SendBuffer(ix);
        }
    }

    #endregion Pose

    #region Custom

    public class CustomMsg : IMessage {
        public const byte Id = 0xB1;
        public byte networkId;
        public byte thingId;
        public byte[] bytes;

        public CustomMsg(byte[] buffer) {
            byte ix = 0;
            this.networkId = buffer[ix++];
            this.thingId = buffer[ix++];
            byte length = (byte)(buffer.Length - ix);
            this.bytes = new byte[length];
            for (uint bytesIx = 0; bytesIx < length; bytesIx++)
                this.bytes[bytesIx] = buffer[ix++];
        }
        public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
            this.networkId = networkId;
            this.thingId = thingId;
            this.bytes = bytes;
        }

        public override byte Serialize(ref byte[] buffer) {
            byte ix = 0;
            buffer[ix++] = CustomMsg.Id;
            buffer[ix++] = this.networkId;
            buffer[ix++] = this.thingId;
            //buffer[ix++] = (byte)bytes.Length;
            foreach (byte b in bytes)
                buffer[ix++] = b;

            return ix;
        }

        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
            byte[] buffer = await Receive(dataStream, packetSize);

            CustomMsg msg = new(buffer);
            client.messageQueue.Enqueue(msg);
            return true;
        }
    }

    #endregion Custom

    #region Text

    public class TextMsg : IMessage {
        public const byte Id = 0xB0;
        public string text;

        public TextMsg(byte[] buffer) : base(buffer) { }
        public override void Deserialize(byte[] buffer) {
            uint ix = 0;
            uint strlen = buffer[ix++];
            this.text = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, (int)strlen);
        }

        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
            byte[] buffer = await Receive(dataStream, packetSize);
            TextMsg msg = new(buffer);

            client.messageQueue.Enqueue(msg);
            return true;
        }
    }

    #endregion

    #region Destroy

    public class DestroyMsg : IMessage {
        public const byte Id = 0x20;
        public const byte length = 3;
        public byte networkId;
        public byte thingId;

        public DestroyMsg(byte[] buffer) : base(buffer) { }

        public override void Deserialize(byte[] buffer) {
            this.networkId = buffer[0];
            this.thingId = buffer[1];
        }

        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
            if (packetSize != length)
                return false;

            byte[] buffer = await Receive(dataStream, packetSize);
            DestroyMsg msg = new(buffer);

            client.messageQueue.Enqueue(msg);
            return true;
        }
    }


    #endregion Destroy
}