HumanoidControl_Free/Messages.cs

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

namespace Passer.Control {

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

        public virtual byte[] Serialize() { return null; }
        public virtual void Deserialize(byte[] data) { }

        public static bool SendMsg(Client client, IMessage msg) {
            return SendMsg(client, msg.Serialize());
        }
        public static bool SendMsg(Client client, byte[] data) {
            if (client == null || client.ipAddress == null)
                return false;

            client.udpClient.Send(data, data.Length, client.ipAddress, client.port);
            return true;
        }

        public static bool PublishMsg(Client client, IMessage msg) {
            return PublishMsg(client, msg.Serialize());
        }
        public static bool PublishMsg(Client client, byte[] data) {
            if (client == null)
                return false;

            client.udpClient.Send(data, data.Length, "127.0.0.1", client.port);
            return true;
        }

        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 Client

    public class ClientMsg : IMessage {
        public const byte Id = 0xA0;
        public const byte length = 2;
        public byte networkId;

        public ClientMsg(byte networkId) {
            this.networkId = networkId;
        }
        public ClientMsg(byte[] data) : base(data) { }

        public override byte[] Serialize() {
            byte[] buffer = new byte[ClientMsg.length];
            buffer[0] = ClientMsg.Id;
            buffer[1] = networkId;
            return buffer;
        }
        public override void Deserialize(byte[] data) {
            base.Deserialize(data);
            uint ix = 0;
            networkId = data[ix];
        }

        public static bool Send(Client client, byte networkId) {
            ClientMsg msg = new(networkId);
            return SendMsg(client, msg);
        }
        public static bool Publish(Client client, byte networkId) {
            ClientMsg msg = new(networkId);
            return PublishMsg(client, msg);
        }
        public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
            if (packetSize != length)
                return false;

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

            if (client.networkId == 0) {
                client.networkId = (byte)(Client.clients.Count);
                NetworkIdMsg.Send(client, client.networkId);
                //if (string.IsNullOrEmpty(sceneUrl) == false)
                //SendModelUrl(client, sceneUrl);
            }
            else if (msg.networkId == 0) {
                NetworkIdMsg.Send(client, client.networkId);
                //if (string.IsNullOrEmpty(sceneUrl) == false)
                //SendModelUrl(client, sceneUrl);
            }

            return true;
        }
    }

    #endregion Client

    #region Network Id

    public class NetworkIdMsg : IMessage {
        public const byte Id = 0xA1;
        public const byte length = 2;
        public byte networkId;

        NetworkIdMsg(byte networkId) {
            this.networkId = networkId;
        }
        NetworkIdMsg(byte[] data) : base(data) { }

        public override byte[] Serialize() {
            byte[] data = new byte[NetworkIdMsg.length];
            data[0] = NetworkIdMsg.Id;
            data[1] = this.networkId;
            return data;
        }
        public override void Deserialize(byte[] data) {
            uint ix = 0;
            this.networkId = data[ix];
        }

        public static bool Send(Client client, byte networkId) {
            NetworkIdMsg msg = new(networkId);
            return SendMsg(client, msg);
            //byte[] data = new byte[NetworkIdMsg.length];
            //data[0] = NetworkIdMsg.Id;
            //data[1] = client.networkId;
            //return SendMsg(client, data);
        }
        public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
            if (packetSize != length)
                return false;

            byte[] buffer = await Receive(dataStream, packetSize);
            NetworkIdMsg msg = new(buffer);
            client.messageQueue.Enqueue(msg);
            return true;
        }
    }

    #endregion Network Id

    #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[] data) : base(data) { }

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

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

            byte[] buffer = await Receive(dataStream, packetSize);
            InvestigateMsg msg = new(buffer);
            client.messageQueue.Enqueue(msg);
            return true;

        }
    }

    #endregion Investigate

    #region Thing

    public class ThingMsg : IMessage {
        public const byte length = 4;
        public const byte Id = 0x80;
        public byte thingId;
        public byte thingType;
        public byte parentId;

        public ThingMsg(byte thingId, byte thingType, byte parentId) {
            this.thingId = thingId;
            this.thingType = thingType;
            this.parentId = parentId;
        }
        public ThingMsg(byte[] data) : base(data) { }

        public override byte[] Serialize() {
            byte[] data = new byte[ThingMsg.length];
            byte ix = 0;
            data[ix++] = ThingMsg.Id;
            data[ix++] = this.thingId;
            data[ix++] = this.thingType;
            data[ix] = this.parentId;
            return data;
        }
        public override void Deserialize(byte[] data) {
            uint ix = 0;
            this.thingId = data[ix++];
            this.thingType = data[ix++];
            this.parentId = data[ix];
        }

        public static bool Send(Client client, byte thingId, byte thingType, byte parentId) {
            ThingMsg msg = new(thingId, thingType, parentId);
            return SendMsg(client, msg);
        }
        public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
            if (packetSize != length)
                return false;

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

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

    #endregion Thing

    #region Name

    public class NameMsg : IMessage {
        public const byte Id = 0x91; // 145
        public const byte length = 3;
        public byte thingId;
        public byte len;
        public string name;

        public NameMsg(byte thingId, string name) {
            this.thingId = thingId;
            this.name = name;
        }
        public NameMsg(byte[] data) : base(data) { }

        public override byte[] Serialize() {
            byte[] buffer = new byte[length + this.name.Length];
            buffer[0] = NameMsg.Id;
            buffer[1] = this.thingId;
            buffer[2] = (byte)this.name.Length;
            for (int ix = 0; ix < this.name.Length; ix++)
                buffer[3 + ix] = (byte)this.name[ix];
            return buffer;
        }
        public override void Deserialize(byte[] data) {
            uint ix = 0;
            this.thingId = data[ix++];
            int strlen = data[ix++];
            this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen);
        }

        public static bool Send(Client client, byte thingId, string name) {
            NameMsg msg = new(thingId, name);
            return SendMsg(client, msg);
        }
        public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
            byte[] buffer = await Receive(dataStream, packetSize);
            NameMsg msg = new(buffer);

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

    #endregion

    #region Model URL

    public class ModelUrlMsg : IMessage {
        public const byte Id = 0x90; // (144) Model URL
        public byte thingId;
        public Spherical position;
        public float scale;
        public string url;

        public ModelUrlMsg(byte thingId, string url, float scale = 1) {
            this.thingId = thingId;
            this.url = url;
            this.scale = scale;
            this.position = Spherical.zero;
        }
        public ModelUrlMsg(byte[] data) : base(data) { }

        public override byte[] Serialize() {
            byte[] data = new byte[this.url.Length + 9];
            data[0] = ModelUrlMsg.Id;
            data[1] = this.thingId; // Thing Id
                                    // data[2]..[5] == position 0, 0, 0
            data[6] = 0x3C; // Dummy float16 value 1
            data[7] = 0x00;

            data[8] = (byte)url.Length;
            for (int ix = 0; ix < this.url.Length; ix++)
                data[9 + ix] = (byte)url[ix];
            return data;
        }
        public override void Deserialize(byte[] data) {
            uint ix = 0;
            this.thingId = data[ix++];
            this.position = LowLevelMessages.ReceiveSpherical(data, ref ix);
            this.scale = LowLevelMessages.ReceiveFloat16(data, ref ix);
            int strlen = data[ix++];
            url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen);
        }

        public static bool Send(Client client, byte thingId, string modelUrl) {
            ModelUrlMsg msg = new(thingId, modelUrl);
            return SendMsg(client, msg);
        }
        public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
            byte[] buffer = await Receive(dataStream, packetSize);
            ModelUrlMsg msg = new(buffer);
            client.messageQueue.Enqueue(msg);
            return true;
        }
    }

    #endregion Model URL

    #region Pose

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

        public byte poseType;
        public const byte Pose_Position = 0x01;
        public const byte Pose_Orientation = 0x02;

        public Spherical position;
        public Quat32 orientation;

        public PoseMsg(byte thingId, Spherical position, Quat32 orientation) {
            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 = new Quat32(0, 0, 0, 1);
        }
        public PoseMsg(byte[] data) : base(data) { }

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

            LowLevelMessages.SendSpherical(buffer, ref ix, position);
            LowLevelMessages.SendQuat32(buffer, ref ix, orientation);
            return buffer;
        }
        public override void Deserialize(byte[] data) {
            uint ix = 0;
            thingId = data[ix++];
            poseType = data[ix++];

            //if ((poseType & Pose_Position) != 0)
            position = LowLevelMessages.ReceiveSpherical(data, ref ix);
            //if ((poseType & Pose_Orientation) != 0) {
            orientation = LowLevelMessages.ReceiveQuat32(data, ref ix);
        }

        public static bool Send(Client client, byte thingId, Spherical position, Quat32 orientation) {
            PoseMsg msg = new(thingId, position, orientation);
            return SendMsg(client, msg);
        }
        public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
            if (packetSize != length)
                return false;

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

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

    #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[] data) : base(data) { }
        public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
            this.networkId = networkId;
            this.thingId = thingId;
            this.bytes = bytes;
        }

        public override byte[] Serialize() {
            byte[] buffer = new byte[4 + this.bytes.Length];
            int 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 buffer;
        }
        public override void Deserialize(byte[] data) {
            uint ix = 0;
            this.thingId = data[ix++];
            this.bytes = new byte[data.Length - ix];
            for (uint bytesIx = 0; ix < data.Length; ix++, bytesIx++)
                this.bytes[bytesIx] = data[ix];
        }


        public static void Send(Client client, byte thingId, byte[] bytes) {
            CustomMsg msg = new(client.networkId, thingId, bytes);
            SendMsg(client, msg);
        }
        public static async Task<bool> Receive(Stream dataStream, Client 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[] data) : base(data) { }
        public override void Deserialize(byte[] data) {
            uint ix = 0;
            uint strlen = data[ix++];
            this.text = System.Text.Encoding.UTF8.GetString(data, (int)ix, (int)strlen);
        }

        public static async Task<bool> Receive(Stream dataStream, Client 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 = 2;
        public byte objectId;

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

        public override void Deserialize(byte[] data) {
            objectId = data[0];
        }

        public static async Task<bool> Receive(Stream dataStream, Client 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
}