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

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

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 const int ReconnectPollMillis = 200;
        private const int ReconnectTimeMillis = 3000;

        
        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;

        private readonly object _reconnectTimeLock = new object();
        private DateTime _reconnectTime = DateTime.Now;

        static Client()
        {
        }

        private Client()
        {
        }

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

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

        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;
                }
            }
        }

        public DateTime ReconnectTime
        {
            get
            {
                lock (_reconnectTimeLock)
                {
                    return _reconnectTime;
                }
            }
            private set
            {
                lock (_reconnectTimeLock)
                {
                    _reconnectTime = value;
                }
            }
        }

        private void ThreadProcedure()
        {
            while (IsRunning)
            {
                try
                {
                    using var client = new TcpClient(DefaultServer, DefaultPort);
                    using var stream = client.GetStream();
                    BinaryReader reader = new BinaryReader(stream);
                    BinaryWriter writer = new BinaryWriter(stream);
                    IsConnected = true;

                    SendProtocolPreamble(writer);

                    // main message reception loop
                    while (IsRunning && IsConnected)
                    {
                        ReadMessages(reader, writer);
                    }
                }
                catch (Exception e)
                {
                    if (e is SocketException || e is IOException)
                    {
                        Debug.LogWarning($"Connection error:\n{e}");
                    }
                    else
                    {
                        Debug.LogError($"Exception in communication thread:\n{e}");
                    }
                }
                
                // wait before reconnection - short polls to prevent blocking if application is closed
                ReconnectTime = DateTime.Now.AddMilliseconds(ReconnectTimeMillis);
                while (IsRunning && DateTime.Now < ReconnectTime)
                {
                    Thread.Sleep(ReconnectPollMillis);
                }
            }
        }


        private void ReadMessages(BinaryReader reader, BinaryWriter writer)
        {
            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: // unknown message
                    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;
            }
        }

        private void SendProtocolPreamble(BinaryWriter writer)
        {
            writer.Write(ProtocolMagicId);
            writer.Write(ProtocolMagicValue);
            writer.Write(ProtocolVersionId);
            writer.Write(ProtocolVersionValue);
        }

        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);
        }
    }
}