aswi2021virtual-surreality-gitlab/deltarobot-vr/Assets/DeltaRobotVr/Client.cs @ 6d9543aa

using System;
using System.Net.Sockets;
using System.Threading;
using System.IO;
using System.Linq;
using System.Text;

namespace DeltaRobotVr
{
    public readonly struct Single3
    {
        public float X { get; }
        public float Y { get; }
        public float Z { get; }

        public Single3(float x, float y, float z)
        {
            X = x;
            Y = y;
            Z = z;
        }

        public override string ToString() => "Single3[ " + X + " " + Y + " " + Z + " ]";
    }

    public sealed class Client
    {
        // singleton instance
        public static readonly Client Instance = new Client();

        // default server
        private const string DefaultServer = "127.0.0.1";
        private const int DefaultPort = 4242;

        // protocol constants
        private const UInt16 ProtocolMagicId = 0x3001;
        private const UInt16 ProtocolVersionId = 0x2002;
        private const UInt16 PingId = 0x3004;
        private const UInt16 PongId = 0x3005;
        private const UInt16 EotId = 0xF006;
        private const UInt16 CurrentActuatorPositionId = 0x4003;
        private const UInt16 CurrentDirectionVectorId = 0x4007;
        private const UInt16 DesiredDirectionVectorId = 0x4008;
        private const UInt16 CurveId = 0xF009;

        private const UInt16 Size8 = 0x0000;
        private const UInt16 Size16 = 0x1000;
        private const UInt16 Size32 = 0x2000;
        private const UInt16 Size64 = 0x3000;
        private const UInt16 Size128 = 0x4000;
        private const UInt16 SizeVariable = 0xF000;
        
        private static readonly byte[] ProtocolMagicValue = Encoding.ASCII.GetBytes("DeltaRVr");
        private const UInt32 ProtocolVersionValue = 1;

        private const UInt32 PointLength = 12;

        
        private Thread _thread;
        
        private readonly object _isRunningLock = new object();
        private bool _isRunning;

        private readonly object _actuatorPositionLock = new object();
        private Single3 _actuatorPosition;

        private readonly object _currentDirectionVectorLock = new object();
        private Single3 _currentDirectionVector;

        private readonly object _desiredDirectionVectorLock = new object();
        private Single3 _desiredDirectionVector;

        private readonly object _curveLock = new object();
        private Single3[] _curve;
        private long _curveCounter = 0;

        private volatile bool _isConnected;
        private volatile string _eotMsg;

        static Client()
        {
        }

        private Client()
        {
        }

        public void Start()
        {
            _thread = new Thread(ThreadProcedure);
            IsRunning = true;
            _thread.Start();
        }

        public void Stop()
        {
            IsRunning = false;
            _thread.Join();
        }

        public string EotMsg
        {
            get => _eotMsg;
            private set => _eotMsg = value;
        }

        public bool IsConnected
        {
            get => _isConnected;
            private set => _isConnected = value;
        }

        public Single3 ActuatorPosition
        {
            get
            {
                lock (_actuatorPositionLock)
                {
                    return _actuatorPosition;
                }
            }
            private set
            {
                lock (_actuatorPositionLock)
                {
                    _actuatorPosition = value;
                }
            } 
        }

        public Single3 CurrentDirectionVector
        {
            get
            {
                lock (_currentDirectionVectorLock)
                {
                    return _currentDirectionVector;
                }
            }
            private set
            {
                lock (_currentDirectionVectorLock)
                {
                    _currentDirectionVector = value;
                }
            }
        }

        public Single3 DesiredDirectionVector
        {
            get
            {
                lock (_desiredDirectionVectorLock)
                {
                    return _desiredDirectionVector;
                }
            }
            private set
            {
                lock (_desiredDirectionVectorLock)
                {
                    _desiredDirectionVector = value;
                }
            }
        }

        public Single3[] Curve
        {
            get
            {
                lock (_curveLock)
                {
                    return _curve;
                }
            }
            private set
            {
                lock (_curveLock)
                {
                    _curve = value;
                }
            }
        }

        public long CurveCounter
        {
            get
            {
                lock (_curveLock)
                {
                    return _curveCounter;
                }
            }
            private set
            {
                lock (_curveLock)
                {
                    _curveCounter = value;
                }
            }
        }

        public bool IsRunning
        {
            get {
                lock (_isRunningLock)
                {
                    return _isRunning;
                }
            }
            private set
            {
                lock (_isRunningLock)
                {
                    _isRunning = value;
                }
            }
        }

        private void ThreadProcedure()
        {
            try
            {
                var client = new TcpClient(DefaultServer, DefaultPort);
                var stream = client.GetStream();
                BinaryReader reader = new BinaryReader(stream);
                BinaryWriter writer = new BinaryWriter(stream);
                IsConnected = true;
                
                writer.Write(ProtocolMagicId);
                writer.Write(ProtocolMagicValue);
                writer.Write(ProtocolVersionId);
                writer.Write(ProtocolVersionValue);
                
                try
                {
                    while (IsRunning && IsConnected)
                    {
                        var messageIdentifier = reader.ReadUInt16();
                        switch (messageIdentifier)
                        {
                            case ProtocolMagicId:
                                ProcessProtocolMagic(reader);
                                break;
                            case ProtocolVersionId:
                                ProcessProtocolVersion(reader);
                                break;
                            case PingId:
                                ProcessPing(reader, writer);
                                break;
                            case EotId:
                                ProcessEot(reader);
                                break;
                            case CurrentActuatorPositionId:
                                ProcessCurrentActuatorPosition(reader);
                                break;
                            case CurrentDirectionVectorId:
                                ProcessCurrentDirectionVector(reader);
                                break;
                            case DesiredDirectionVectorId:
                                ProcessDesiredDirectionVector(reader);
                                break;
                            case CurveId:
                                ProcessCurve(reader);
                                break;
                            default: //client tuto zpravu nezna
                                var sizeIdentifier = (UInt16) (messageIdentifier & 0xF000);
                                switch (sizeIdentifier)
                                {
                                    case Size8:
                                        Skip8(reader);
                                        break;
                                    case Size16:
                                        Skip16(reader);
                                        break;
                                    case Size32:
                                        Skip32(reader);
                                        break;
                                    case Size64:
                                        Skip64(reader);
                                        break;
                                    case Size128:
                                        Skip128(reader);
                                        break;
                                    case SizeVariable:
                                        SkipVariableLength(reader);
                                        break;
                                }

                                break;
                        }
                    }
                }
                finally
                {
                    stream.Close();
                    client.Close();
                    IsConnected = true;
                }
            }
            catch (Exception)
            {
                Console.WriteLine("failed to connect");
            }
        }


        private void Skip8(BinaryReader reader)
        {
            reader.ReadByte();
        }

        private void Skip16(BinaryReader reader)
        {
            reader.ReadUInt16();
        }

        private void Skip32(BinaryReader reader)
        {
            reader.ReadUInt32();
        }

        private void Skip64(BinaryReader reader)
        {
            reader.ReadUInt64();
        }

        private void Skip128(BinaryReader reader)
        {
            reader.ReadUInt64();
            reader.ReadUInt64();
        }

        private void SkipVariableLength(BinaryReader reader)
        {
            UInt32 numberOfBytes = reader.ReadUInt32();
            reader.ReadBytes((int) numberOfBytes);
        }

        private void ProcessCurrentActuatorPosition(BinaryReader reader)
        {
            var x = reader.ReadSingle();
            var y = reader.ReadSingle();
            var z = reader.ReadSingle();
            reader.ReadSingle(); // skip unused

            ActuatorPosition = new Single3(x, y, z);
        }

        private void ProcessCurrentDirectionVector(BinaryReader reader)
        {
            var x = reader.ReadSingle();
            var y = reader.ReadSingle();
            var z = reader.ReadSingle();
            reader.ReadSingle(); // skip unused

            CurrentDirectionVector = new Single3(x, y, z);
        }

        private void ProcessDesiredDirectionVector(BinaryReader reader)
        {
            var x = reader.ReadSingle();
            var y = reader.ReadSingle();
            var z = reader.ReadSingle();
            reader.ReadSingle(); // skip unused

            DesiredDirectionVector = new Single3(x, y, z);
        }

        private void ProcessCurve(BinaryReader reader)
        {
            UInt32 numberOfBytes = reader.ReadUInt32();
            UInt32 numberOfPoints = numberOfBytes / PointLength;
            Single3[] curve = new Single3[numberOfPoints];

            for(int i = 0; i < numberOfPoints; i++)
            {
                var x = reader.ReadSingle();
                var y = reader.ReadSingle();
                var z = reader.ReadSingle();
                curve[i] = new Single3(x, y, z);
            }

            lock(_curveLock)
            {
                CurveCounter++;
                Curve = curve;
            }
        }

        private void ProcessEot(BinaryReader reader)
        {
            UInt32 numberOfBytes = reader.ReadUInt32();
            Byte[] buffer = reader.ReadBytes((int) numberOfBytes);
            EotMsg = Encoding.ASCII.GetString(buffer);
            IsConnected = false;
        }

        private void ProcessProtocolMagic(BinaryReader reader)
        {
            byte[] value = reader.ReadBytes(8);
            if (!value.SequenceEqual(ProtocolMagicValue))
            {
                IsConnected = false;
            }
        }

        private void ProcessProtocolVersion(BinaryReader reader)
        {
            UInt32 value = reader.ReadUInt32();
            if (value != ProtocolVersionValue)
            {
                IsConnected = false;
            }
        }

        private void ProcessPing(BinaryReader reader, BinaryWriter writer)
        {
            UInt64 pingValue = reader.ReadUInt64();

            writer.Write(PongId);
            writer.Write(pingValue);
        }
    }
}