using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.IO.Ports;
using System.Net;
using System.Net.Sockets;
using System.Net.NetworkInformation;
using System.IO;
using System.Threading;

namespace RateTableGUI
{
    /// <summary>
    /// Driver for AERO4000.
    /// </summary>
    public class AERO4000driver : IRateTable
    {
        #region --------------------------- Private global values ---------------------------
        #region ------Commands-------
        /// <summary>
        /// Default acceleration for motion commands. Limited by MXA command.
        /// </summary>
        /// <example>Use it after PKS or SAV</example>
        /// <deg>0 to MXA deg/s^2</deg>
        private const string ACL = "ACL";

        /// <summary>
        /// Apply offset to the input voltage
        /// </summary>
        /// <example>Immediately</example>
        /// <deg>+/- 10V</deg>
        private const string AIB = "AIB";

        /// <summary>
        /// Apply gain to the input voltage
        /// </summary>
        /// <example>Immediately</example>
        /// <deg>Any non-zero number</deg>
        private const string AIG = "AIG";

        /// <summary>
        /// Sets analog input mode.
        /// </summary>
        /// <deg>
        /// P or 2 = Position mode
        /// V or 1 = Velocity mode
        /// 0 = Off
        /// </deg>
        private const string AIM = "AIM";

        /// <summary>
        /// Analog input select
        /// </summary>
        /// <deg>Range 1 to 2</deg>
        private const string AIS = "AIS";

        /// <summary>
        /// Sine Amplitude
        /// </summary>
        /// <deg>Any positive deg</deg>
        private const string AMP = "AMP";

        /// <summary>
        /// Calibration pulse angle. Sets pulses between Rate Pulse BNC.
        /// </summary>
        /// <deg>0 to 360 deg</deg>
        private const string ANG = "ANG";

        /// <summary>
        /// Analog output bias. Offset voltage to the output.
        /// </summary>
        /// <example>Immediately</example>
        /// <deg>+/- 10V</deg>
        private const string AOB = "AOB";

        /// <summary>
        /// Analog output gain. Apply a gain to the source deg when calculating the output voltage.
        /// </summary>
        /// <example>Immediately</example>
        /// <deg>Any non-zero number</deg>
        private const string AOG = "AOG";

        /// <summary>
        /// Analog output mode.
        /// </summary>
        /// <deg>
        /// A or 4 = Acceleration
        /// V or 3 = Velocity
        /// PE or 2 = Position Error
        /// P or 1 = Position
        /// 0 = Off
        /// </deg>
        private const string AOM = "AOM";

        /// <summary>
        /// Analog Output Select.
        /// </summary>
        /// <deg>range 1 to 2</deg>
        private const string AOS = "AOS";

        /// <summary>
        /// Inner axis select. Directly address the inner axis for communication on 2 or 3 axis tables.
        /// </summary>
        private const string AXI = "AXI";

        /// <summary>
        /// Middle Axis Select. Directly address the middle axis for communication on 3 axis tables.
        /// </summary>
        private const string AXM = "AXM";

        /// <summary>
        /// Outer Axis Select. Directly address the outer axis for communication on 2 or 3 axis tables.
        /// </summary>
        private const string AXO = "AXO";

        /// <summary>
        /// Axis select.
        /// </summary>
        /// <deg>1 to number of axes</deg>
        private const string AXS = "AXS";

        /// <summary>
        /// Brake.
        /// </summary>
        /// <deg>
        /// 0 = Off
        /// 1 = On
        /// </deg>
        private const string BRK = "BRK";

        /// <summary>
        /// Counts per degree. Readonly.
        /// </summary>
        private const string CPD = "CPD";

        /// <summary>
        /// Counts per revolution.
        /// </summary>
        private const string CPR = "CPR";

        /// <summary>
        /// Sine cycles. O = continuous sinusoidal motion <=> stoped by STO command.
        /// </summary>
        /// <deg>Positive deg</deg>
        private const string CYC = "CYC";

        /// <summary>
        /// Derivative gain. Set to selected axis. This gain affects the damping of the axis.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>Positive deg</deg>
        private const string DER = "DER";

        /// <summary>
        /// Direction. 
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>
        /// -, 0 or -1 = counterclockwise positive
        /// + or 1 = clockwise positive
        /// </deg>
        private const string DIR = "DIR";

        /// <summary>
        /// Feed forward acceleration gain.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>
        /// Positive deg
        /// 0 = Off
        /// </deg>
        private const string FAC = "FAC";

        /// <summary>
        /// Following error limit. Tolerance for axis.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>
        /// Positive deg
        /// -1 = Off
        /// </deg>
        private const string FEL = "FEL";

        /// <summary>
        /// Sine frequency.
        /// </summary>
        /// <deg>Positive deg</deg>
        private const string FRQ = "FRQ";

        /// <summary>
        /// Feed forward velocity gain.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>
        /// Positive deg
        /// 0 = Off
        /// </deg>
        private const string FVE = "FVE";

        /// <summary>
        /// Home offset. Determine the location of the axis zero.
        /// </summary>
        /// <deg>-180 to 180 deg</deg>
        private const string HOF = "HOF";

        /// <summary>
        /// Home. This command will cause the axis to move to position 0.
        /// </summary>
        private const string HOM = "HOM";

        /// <summary>
        /// Integral limit. Sets limits to the integrator function.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>Positive deg</deg>
        private const string ILI = "ILI";

        /// <summary>
        /// Integral Gain. Sets integral gain for selected axis.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>Positive deg</deg>
        private const string INT = "INT";

        /// <summary>
        /// Jog at Rate. Command rotates the axis at a specified velicity.
        /// </summary>
        /// <deg>
        /// Range Speed = 0 to MXV
        /// Speed = VEL
        /// Range acceleration = 0 to MXA
        /// Acceleration = ACL
        /// </deg>
        private const string JOG = "JOG";

        /// <summary>
        /// Move complete. This command is used to determine if the axis has reached its final position.
        /// </summary>
        /// <deg>
        /// Positive deg for tolerance.
        /// </deg>
        private const string MCO = "MCO";

        /// <summary>
        /// Minimum position. Sets minimum position limit.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>
        /// 0 to 360
        /// </deg>
        private const string MNP = "MNP";

        /// <summary>
        /// Move to position.
        /// </summary>
        /// <example>
        /// MOV10 - Move the axis to 10deg
        /// MOV50,25 - Mote the axis to 50deg using a peak velocity of 25 deg/s
        /// Range Speed = 0 to MXV
        /// Speed = VEL
        /// Range acceleration = 0 to MXA
        /// Acceleration = ACL
        /// </example>
        private const string MOV = "MOV";

        /// <summary>
        /// Maximum Acceleration.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>Positive deg</deg>
        private const string MXA = "MXA";

        /// <summary>
        /// Maximum Position.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>0(unlimited) to 360</deg>
        private const string MXP = "MXP";

        /// <summary>
        /// Maximum velocity.
        /// </summary>
        /// <example>After SAV or PKS</example>
        /// <deg>Positive values</deg>
        private const string MXV = "MXV";

        /// <summary>
        /// Number of Axes.
        /// </summary>
        private const string NOA = "NOA";

        /// <summary>
        /// Sine acceleration Onset/Decay Time
        /// </summary>
        /// <deg>Positive values</deg>
        private const string ODT = "ODT";

        /// <summary>
        /// Poll Acceleration. Returns average acceleration of axis.
        /// </summary>
        private const string PAC = "PAC";

        /// <summary>
        /// Position Capture Data.
        /// </summary>
        /// <deg>
        /// Buffer size = 1000
        /// Range 1 to PCD?
        /// </deg>
        private const string PCD = "PCD";

        /// <summary>
        /// Save all settings
        /// </summary>
        private const string SAV = "SAV";

        /// <summary>
        /// Active status
        /// </summary>
        /// <deg>
        /// Bit Status</br>
        /// 0 Axis Busy
        /// 1 Following Error Exceeded
        /// 2 Latched Hardware Travel Limit
        /// 3 Latched Emergency Stop
        /// 4 Shot-pin Engaged Latch
        /// 5 Servo Motor Off
        /// 6 Brake Engaged
        /// 7 Absolute Position Not Established
        /// 8 Unused
        /// 9 Latched Chamber Door Ajar Alarm
        /// 10 Brake Switch On
        /// 11 Software Servo Off
        /// 12 Software Brake On
        /// 13 Driver Not Initialized
        /// 14 Latched Motor Alarm (over-temp or current limit)
        /// 15 Servo Switch Off
        /// 16 Latched Chamber Alarm
        /// 17 Latched Shaft Alarm
        /// </deg>
        private const string STA = "STA";

        /// <summary>
        /// Stop
        /// </summary>
        private const string STO = "STO";

        /// <summary>
        /// Enable/Disable climatic chamber
        /// </summary>
        private const string THO = "THO";

        /// <summary>
        /// Velocity
        /// </summary>
        /// <example>After SAV or PKS</example>
        private const string VEL = "VEL";

        /// <summary>
        /// Returns the instantaneous position if the axis
        /// </summary>
        private const string PPO = "PPO";

        /// <summary>
        /// Returns the instantaneous reference position of the axis. AKA commanded position.
        /// </summary>
        private const string PRP = "PRP";

        /// <summary>
        /// Returns the average velocity of the axis.
        /// </summary>
        private const string PVE = "PVE";

        /// <summary>
        /// Returns current firmware and/or software versions used by the system.
        /// </summary>
        private const string VER = "VER";

        /// <summary>
        /// Return zero axis position.
        /// </summary>
        private const string ZER = "ZER";
        #endregion//------Commands-------

        /// <summary>
        /// Carriage return
        /// </summary>
        private const char CR = (char)0x0D;
        private const char SPACE = (char)0x20;
        private const char BACKSPACE = (char)0x08;
        private const char LINESYMBOL = '>';
        /// <summary>
        /// Mandatory char for getting information from device
        /// </summary>
        private const char GETVAL = '?';
        /// <summary>
        /// Mandatory char for separation two values
        /// </summary>
        private const char SEPARATOR = ',';
        /// <summary>
        /// New line alias end of one command
        /// </summary>
        private const char NL = (char)0x0A;

        /// <summary>
        /// This parameter can be manualy change in AERO400
        /// </summary>
        private const int BAUDRATE = 9600;
        private const Parity PARITY = Parity.None;
        private const int DATABITS = 8;
        private const StopBits STOPBIT = StopBits.One;
        private const bool isECHOenable = false;

        /// <summary>
        /// Communication through simple RS232
        /// </summary>
        private SerialPort MySerialPort;
        /// <summary>
        /// Communication through Ethernet
        /// </summary>
        private TcpClient MyTCPclient;
        /// <summary>
        /// Handler for read response.
        /// </summary>
        private EventWaitHandle RxCancelEvent;
        /// <summary>
        /// Locks in thread function for send commands due to overwrite parameters.
        /// </summary>
        private Object DoWork4MeLocker;
        /// <summary>
        /// Commands who needs response and haven't got '?'
        /// </summary>
        private List<string> CommandsReqRx = new List<string>()
            { 
                CPD, CPR, NOA, PAC, PPO, STA, PVE, PRP, PPO
            };
        /// <summary>
        /// Last know parameters
        /// </summary>
        private AxisValues_t InnerAxisActualValues;
        /// <summary>
        /// Last know parameters
        /// </summary>
        private AxisValues_t OuterAxisActualValues;
        #endregion

        #region --------------------------- Public global values ---------------------------
        /// <summary>
        /// Get name of rateTable
        /// </summary>
        public string ChamberName { get { return "AERO4000"; } }
        #endregion

        #region --------------------------- Structures&Enums ---------------------------
        /// <summary>
        /// Response type
        /// </summary>
        private enum RxMessageType_e
        { 
            /// <summary>
            /// No data in response, just acknowledgement
            /// </summary>
            Non_data,
            /// <summary>
            /// Useful data in response
            /// </summary>
            With_data,
            /// <summary>
            /// Error response.
            /// </summary>
            Error
        }

        /// <summary>
        /// Type of axis
        /// </summary>
        public enum Axes_e
        { 
            /// <summary>
            /// Inner axis
            /// </summary>
            Inner,
            /// <summary>
            /// Outer axis
            /// </summary>
            Outer,
            /// <summary>
            /// Inner and Outer axes
            /// </summary>
            All
        }

        /// <summary>
        /// Structure for one contains detail information about command.
        /// </summary>
        public struct CommandsParametrs_t
        {
            /// <summary>
            /// Means if GetChar will use or not
            /// </summary>
            public bool isQuestion;
            /// <summary>
            /// First value for command
            /// </summary>
            public char[] value1;
            /// <summary>
            /// Second value for command
            /// </summary>
            public char[] value2;
        }

        /// <summary>
        /// Structure for last know values from AERO4000
        /// </summary>
        public struct AxisValues_t
        {
            public float ZeroPosition;
            public const string ZeroPositionCmd = ZER;

            public float Velocity;
            public const string VelocityCmd = PVE;

            public int AxisStatus;
            public const string AxisStatusCmd = STA;

            public float RefPosition;
            public const string RefPositionCmd = PRP;

            public float ActualPosition;
            public const string ActualPositionCmd = PPO;

            public float ActualAcceleration;
            public const string ActualAccelerationCmd = PAC;
        }
        #endregion

        #region --------------------------- Construcotrs&Destructor ---------------------------
        public AERO4000driver()
        {
            InnerAxisActualValues = new AxisValues_t();
            OuterAxisActualValues = new AxisValues_t();
            DoWork4MeLocker = new object();
            RxCancelEvent = new EventWaitHandle(false, EventResetMode.ManualReset);
        }

        ~AERO4000driver()
        {
            if (MySerialPort != null)
            {
                if (MySerialPort.IsOpen)
                    MySerialPort.Close();
                MySerialPort.Dispose();
            }
            if (MyTCPclient != null)
            {
                if (MyTCPclient.Connected)
                    MyTCPclient.Close();
                MyTCPclient = null;
            }
        }
        #endregion

        #region --------------------------- Private sector ---------------------------
        /// <summary>
        /// Creates message.
        /// </summary>
        /// <param name="cmd">Only command</param>
        /// <returns>Message as char array</returns>
        private char[] BuildMessage(string cmd)
        {
            char[] _res = new char[cmd.ToCharArray().Length];
            int i = 0;
            foreach (char c in cmd.ToCharArray())
            {
                _res[i++] = c;
            }

            return _res;
        }

        /// <summary>
        /// Creates message.
        /// </summary>
        /// <param name="cmd">Command</param>
        /// <param name="getval">'?' will use</param>
        /// <returns>Message as char array</returns>
        private char[] BuildMessage(string cmd, char getval)
        {
            char[] _res = new char[cmd.ToCharArray().Length + 1];
            int i = 0;
            foreach (char c in cmd.ToCharArray())
            {
                _res[i++] = c;
            }
            _res[i] = getval;

            return _res;
        }

        /// <summary>
        /// Creates message.
        /// </summary>
        /// <param name="cmd">Command</param>
        /// <param name="value">Value for command (int or float)</param>
        /// <returns>Message as char array</returns>
        private char[] BuildMessage(string cmd, string value)
        {
            char[] _res = new char[cmd.ToCharArray().Length + value.ToCharArray().Length];
            int i = 0;
            foreach (char c in cmd.ToCharArray())
            {
                _res[i++] = c;
            }
            foreach (char c in value.ToCharArray())
            {
                _res[i++] = c;
            }

            return _res;
        }

        /// <summary>
        /// Creates message.
        /// </summary>
        /// <param name="cmd">Command</param>
        /// <param name="value">Value for command (int or float)</param>
        /// <returns>Message as char array</returns>
        private char[] BuildMessage(string cmd, char[] value)
        {
            char[] _res = new char[cmd.ToCharArray().Length + value.Length];
            int i = 0;
            foreach (char c in cmd.ToCharArray())
            {
                _res[i++] = c;
            }
            foreach (char c in value)
            {
                _res[i++] = c;
            }

            return _res;
        }

        /// <summary>
        /// Creates message.
        /// </summary>
        /// <param name="cmd">Command</param>
        /// <param name="value1">First value for command (int or float)</param>
        /// <param name="value2">Second value for command (int or float)</param>
        /// <returns>Message as char array</returns>
        private char[] BuildMessage(string cmd, string value1, string value2)
        {
            char[] _res = new char[cmd.ToCharArray().Length + value1.ToCharArray().Length + value2.ToCharArray().Length];
            int i = 0;
            foreach (char c in cmd.ToCharArray())
            {
                _res[i++] = c;
            }
            foreach (char c in value1.ToCharArray())
            {
                _res[i++] = c;
            }
            _res[i++] = SEPARATOR;
            foreach (char c in value2.ToCharArray())
            {
                _res[i++] = c;
            }

            return _res;
        }

        /// <summary>
        /// Creates message.
        /// </summary>
        /// <param name="cmd">Command</param>
        /// <param name="value1">First value for command (int or float)</param>
        /// <param name="value2">Second value for command (int or float)</param>
        /// <returns>Message as char array</returns>
        private char[] BuildMessage(string cmd, char[] value1, char[] value2)
        {
            char[] _res = new char[cmd.ToCharArray().Length + value1.Length + value2.Length + 1];
            int i = 0;
            foreach (char c in cmd.ToCharArray())
            {
                _res[i++] = c;
            }
            foreach (char c in value1)
            {
                _res[i++] = c;
            }
            _res[i++] = SEPARATOR;
            foreach (char c in value2)
            {
                _res[i++] = c;
            }

            return _res;
        }

        /// <summary>
        /// Sends message.
        /// </summary>
        /// <param name="msg">Message to send</param>
        private void SendProvider(char[] msg)
        {
            if (MySerialPort != null)
            {
                if (MySerialPort.IsOpen)
                {
                    MySerialPort.Write(msg, 0, msg.Length);
                    MySerialPort.BaseStream.WriteByte(Convert.ToByte(CR));
                }
            }
            else if(MyTCPclient != null)
            {
                if (MyTCPclient.Connected)
                {
                    NetworkStream _netstream = MyTCPclient.GetStream();
                    _netstream.Write(Convert.FromBase64CharArray(msg, 0, msg.Length), 0, msg.Length);
                    _netstream.WriteByte(Convert.ToByte(CR));
                }
            }
        }

        /// <summary>
        /// Read from receive buffer
        /// </summary>
        /// <param name="InStream">Stream for serial port</param>
        /// <param name="buffer">Out buffer</param>
        /// <param name="Length">Length of response</param>
        /// <param name="CancelEvent">Handle for cancel event</param>
        /// <returns>Success or not</returns>
        private Boolean ReadToBuffer(Stream InStream,Byte[] buffer,int Length,WaitHandle CancelEvent)
        {
            int pos = 0;
            IAsyncResult Result;
            WaitHandle[] WaitHandles = new WaitHandle[2];
            WaitHandles[1] = CancelEvent;
            Boolean Response = true;

            while (Length != pos)
            {
                Result = InStream.BeginRead(buffer, pos, (Length - pos), null, null);
                WaitHandles[0] = Result.AsyncWaitHandle;

                if (0 == WaitHandle.WaitAny(WaitHandles))
                {
                    try
                    {
                        pos += InStream.EndRead(Result);
                    }
                    catch (Exception ex)
                    {
                        RateTableError.Add("AERO4000driver.cs", "Can't read data from receive buffer", ex.ToString());
                        Response = false; ;
                    }
                }
                else
                {
                    Response = false;
                    break;
                }
            }

            return Response;
        }

        /// <summary>
        /// Read from receive buffer
        /// </summary>
        /// <param name="InStream">Stream for serial port</param>
        /// <param name="buffer">Out buffer</param>
        /// <param name="Length">Length of response</param>
        /// <param name="CancelEvent">Handle for cancel event</param>
        /// <returns>Success or not</returns>
        private Boolean ReadToBuffer(NetworkStream InStream,Byte[] buffer, int Length,WaitHandle CancelEvent)
        {
            int pos = 0;
            IAsyncResult Result;
            WaitHandle[] WaitHandles = new WaitHandle[2];
            WaitHandles[1] = CancelEvent;
            Boolean Response = true;

            while (Length != pos)
            {
                Result = InStream.BeginRead(buffer, pos, (Length - pos), null, null);
                WaitHandles[0] = Result.AsyncWaitHandle;

                if (0 == WaitHandle.WaitAny(WaitHandles))
                {
                    try
                    {
                        pos += InStream.EndRead(Result);
                    }
                    catch (Exception ex)
                    {
                        RateTableError.Add("AERO4000.cs", "Can't read from receive buffer", ex.ToString());
                        Response = false;
                    }
                }
                else
                {
                    Response = false;
                    break;
                }
            }

            return Response;
        }

        /// <summary>
        /// Reads response from AERO4000
        /// </summary>
        /// <returns>Response</returns>
        private byte[] ReadProvider()
        {
            byte[] _buffer = new byte[4];
            byte[] _res = new byte[1];
            RxMessageType_e _msgtype = RxMessageType_e.Error;

            if (MySerialPort != null)
            {
                if (MySerialPort.IsOpen)
                {
                    Thread.Sleep(500);
                    int bytes2read = MySerialPort.BytesToRead;
                    if (bytes2read == 0)
                        return _buffer;
                    _buffer = new byte[1];
                    if(ReadToBuffer(MySerialPort.BaseStream, _buffer, 1, RxCancelEvent))
                    {
                        if (_buffer[0] == Convert.ToByte(CR))
                            _msgtype = RxMessageType_e.Non_data;
                        else if (_buffer[0] != Convert.ToByte(GETVAL))
                            _msgtype = RxMessageType_e.With_data;
                    }

                    _res[0] = _buffer[0];

                    switch (_msgtype)
                    { 
                        case RxMessageType_e.Non_data:
                            _buffer = new byte[4];
                            if (ReadToBuffer(MySerialPort.BaseStream, _buffer, 4, RxCancelEvent))
                            {
                                BufferCopy(ref _buffer, 0, ref _res, 1, _buffer.Length);
                            }
                            break;
                        case RxMessageType_e.Error:
                            _buffer = new byte[5];
                            if (ReadToBuffer(MySerialPort.BaseStream, _buffer, 4, RxCancelEvent))
                            {
                                BufferCopy(ref _buffer, 0, ref _res, 1, _buffer.Length);
                            }
                            break;
                        case RxMessageType_e.With_data:
                            int nlcounter = 0;
                            while (nlcounter < 2)
                            {
                                _buffer = new byte[1];
                                if(ReadToBuffer(MySerialPort.BaseStream, _buffer, 1, RxCancelEvent))
                                {
                                    if (_buffer[0] == NL)
                                        nlcounter++;
                                }
                                BufferCopy(ref _buffer, 0, ref _res, _res.Length, _buffer.Length);
                            }
                            break;
                    }
                }
            }
            else if (MyTCPclient != null)
            {
                if (MyTCPclient.Connected)
                {
                    NetworkStream netstream = MyTCPclient.GetStream();
                    if (ReadToBuffer(netstream, _buffer, 1, RxCancelEvent))
                    {
                        if (_buffer[0] == Convert.ToByte(CR))
                            _msgtype = RxMessageType_e.Non_data;
                        else if (_buffer[0] != Convert.ToByte(GETVAL))
                            _msgtype = RxMessageType_e.With_data;
                    }

                    _res[0] = _buffer[0];

                    switch (_msgtype)
                    {
                        case RxMessageType_e.Non_data:
                            _buffer = new byte[4];
                            if (ReadToBuffer(netstream, _buffer, 4, RxCancelEvent))
                            {
                                BufferCopy(ref _buffer, 0, ref _res, 1, _buffer.Length);
                            }
                            break;
                        case RxMessageType_e.Error:
                            _buffer = new byte[5];
                            if (ReadToBuffer(netstream, _buffer, 4, RxCancelEvent))
                            {
                                BufferCopy(ref _buffer, 0, ref _res, 1, _buffer.Length);
                            }
                            break;
                        case RxMessageType_e.With_data:
                            int nlcounter = 0;
                            while (nlcounter < 2)
                            {
                                _buffer = new byte[1];
                                if (ReadToBuffer(netstream, _buffer, 1, RxCancelEvent))
                                {
                                    if (_buffer[0] == NL)
                                        nlcounter++;
                                }
                                BufferCopy(ref _buffer, 0, ref _res, _res.Length, _buffer.Length);
                            }
                            break;
                    }
                }
            }

            return _res;
        }

        /// <summary>
        /// Copy one byte array to another byte array
        /// </summary>
        /// <param name="src">Reference to source array</param>
        /// <param name="srcOffset">Offset in source array</param>
        /// <param name="dest">Reference to destination array</param>
        /// <param name="destOffset">Offset in destination array</param>
        /// <param name="length">Quantity of elements to copy</param>
        private void BufferCopy(ref byte[] src, int srcOffset, ref byte[] dest, int destOffset, int length)
        {
            int i = 0;

            byte[] mem = dest;
            dest = new byte[length + destOffset];
            foreach (byte b in mem)
                dest[i++] = b;
                      
            for (i = 0; i < length; i++)
                dest[destOffset + i] = src[i + srcOffset];

        }

        /// <summary>
        /// Thread's function. Parse information from inparam and sends all commands. Only one function can send data at time.
        /// </summary>
        /// <param name="inparam">Dictionary(string, CommandsParametrs_t) fill with commands</param>
        private void DoWork4Me(object inparam)
        {
            Dictionary<string, CommandsParametrs_t> comList = (Dictionary<string, CommandsParametrs_t>)inparam;

            lock (DoWork4MeLocker)
            {
                try
                {
                    bool isInner = true;

                    for (int i = 0; i < comList.Count; i++)
                    {
                        byte[] _res;
                        string _command = comList.Keys.ElementAt(i);
                        CommandsParametrs_t _paramList = new CommandsParametrs_t();

                        if (comList.TryGetValue(_command, out _paramList))
                        {
                            if (_command == AXI)
                            {
                                isInner = true;
                            }
                            else if (_command == AXO)
                            {
                                isInner = false;
                            }
                            System.Diagnostics.Stopwatch sp = new System.Diagnostics.Stopwatch();
                            sp.Start();

                            //SendProvider
                            if (_paramList.isQuestion)
                            {
                                SendProvider(BuildMessage(_command, GETVAL));
                            }
                            else if (_paramList.value1 != null && _paramList.value2 == null)
                            {
                                SendProvider(BuildMessage(_command, _paramList.value1));
                            }
                            else if (_paramList.value1 != null && _paramList.value2 != null)
                            {
                                SendProvider(BuildMessage(_command, _paramList.value1, _paramList.value2));
                            }
                            else
                            {
                                SendProvider(BuildMessage(_command));
                            }

                            //ResponseProvider 
                            //Partly commendted because AERO does not response
                            if (_paramList.isQuestion)
                            {
                                //_res = ReadProvider();
                                _res = new byte[4];
                                sp.Stop();
                                //Write in log
                                if (isInner)
                                    SetActualValue(Axes_e.Inner, _res, _command);
                                else
                                    SetActualValue(Axes_e.Outer, _res, _command);
                            }
                            else if (CommandsReqRx.Contains(_command))
                            {
                                //_res = ReadProvider();
                                _res = new byte[4];
                                if (isInner)
                                    SetActualValue(Axes_e.Inner, _res, _command);
                                else
                                    SetActualValue(Axes_e.Outer, _res, _command);
                                sp.Stop();
                            }
                        } //if(comList.TryGetValue(_command, out _paramList))
                    }//for (int i = 0; i < comList.Count; i++)
                }
                catch (Exception ex)
                {
                    Console.WriteLine(ex.ToString());
                }
            }//lock (DoWork4MeLocker)
        }

        /// <summary>
        /// Sets actual values from specific axis
        /// </summary>
        /// <param name="type">Type of axis</param>
        /// <param name="value">New actual value</param>
        /// <param name="command">Last sends command</param>
        private void SetActualValue(Axes_e type, byte[] value, string command)
        {
            switch (command)
            { 
                case AxisValues_t.ZeroPositionCmd:
                    if (type == Axes_e.Inner)
                        InnerAxisActualValues.ZeroPosition = BitConverter.ToSingle(value, 0);
                    else
                        OuterAxisActualValues.ZeroPosition = BitConverter.ToSingle(value, 0);
                    break;
                case AxisValues_t.VelocityCmd:
                    if (type == Axes_e.Inner)
                        InnerAxisActualValues.Velocity = BitConverter.ToSingle(value, 0);
                    else
                        OuterAxisActualValues.Velocity = BitConverter.ToSingle(value, 0);
                    break;
                case AxisValues_t.RefPositionCmd:
                    if (type == Axes_e.Inner)
                        InnerAxisActualValues.RefPosition = BitConverter.ToSingle(value, 0);
                    else
                        OuterAxisActualValues.RefPosition = BitConverter.ToSingle(value, 0);
                    break;
                case AxisValues_t.AxisStatusCmd:
                    if (type == Axes_e.Inner)
                        InnerAxisActualValues.AxisStatus = BitConverter.ToInt32(value, 0);
                    else
                        OuterAxisActualValues.AxisStatus = BitConverter.ToInt32(value, 0);
                    break;
                case AxisValues_t.ActualPositionCmd:
                    if (type == Axes_e.Inner)
                        InnerAxisActualValues.ActualPosition = BitConverter.ToSingle(value, 0);
                    else
                        OuterAxisActualValues.ActualPosition = BitConverter.ToSingle(value, 0);
                    break;
                case AxisValues_t.ActualAccelerationCmd:
                    if (type == Axes_e.Inner)
                        InnerAxisActualValues.ActualAcceleration = BitConverter.ToSingle(value, 0);
                    else
                        OuterAxisActualValues.ActualAcceleration = BitConverter.ToSingle(value, 0);
                    break;
            }
        }
        #endregion

        #region --------------------------- Protected sector ---------------------------
        #endregion

        #region --------------------------- Public sector ---------------------------
        /// <summary>
        /// Starts one thread controlled by ThreadPool
        /// </summary>
        /// <param name="cmdList">All commands</param>
        public void StartTask(Dictionary<string, CommandsParametrs_t> cmdList)
        {
            ThreadPool.QueueUserWorkItem(DoWork4Me, (object)cmdList);
        }

        /// <summary>
        /// Connects to AERO4000 through simple RS232
        /// </summary>
        /// <param name="portName">COM name</param>
        /// <param name="baudRate">Doesn't matter. This value is defined by vendor.</param>
        /// <param name="dataBits">Doesn't matter. This value is defined by vendor.</param>
        /// <param name="stopBits">Doesn't matter. This value is defined by vendor.</param>
        /// <param name="parity">Doesn't matter. This value is defined by vendor.</param>
        /// <returns>State of port</returns>
        public bool Connect(string portName, string baudRate, string dataBits, string stopBits, string parity)
        {
            MyTCPclient = null;
            MySerialPort = new SerialPort()
                {
                    BaudRate = BAUDRATE,
                    Parity = PARITY,
                    StopBits = STOPBIT,
                    DataBits = DATABITS,
                    PortName = portName
                };

            try
            {
                MySerialPort.Open();
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.ToString());
            }

            return MySerialPort.IsOpen;
        }

        /// <summary>
        /// Connects to AERO4000 through ethernet
        /// </summary>
        /// <param name="ip">Destination IP</param>
        /// <param name="port">Destination port</param>
        /// <returns>Connection state</returns>
        public bool Connect(IPAddress ip, int port)
        {
            MySerialPort = null;
            MyTCPclient = new TcpClient();

            //string s_ConnectionString =  string.Format("tcp://{0}:{1}/IaiInterfaces", ip, port);
            //string s_ConnectionString2 = string.Format("tcp://{0}:{1}/IaiInterfaces", ip, Convert.ToInt32(port.ToString(), 10) + 1);

            try
            {
                MyTCPclient.Connect(ip, port);
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.ToString());
            }

            return MyTCPclient.Connected;
        }

        /// <summary>
        /// Disconnect from AERO4000
        /// </summary>
        public void Disconnect()
        { 
            if(MySerialPort != null)
                if (MySerialPort.IsOpen)
                {
                    MySerialPort.Close();
                    MySerialPort = null;
                }
                else
                {
                    MySerialPort = null;
                }
            else if(MyTCPclient != null)
                if (MyTCPclient.Connected)
                {
                    MyTCPclient.Close();
                    MyTCPclient = null;
                }
                else
                {
                    MyTCPclient = null;
                }
        }

        /// <summary>
        /// Save all settings
        /// </summary>
        public void Init()
        {
            SendProvider(BuildMessage(SAV));
        }

        /// <summary>
        /// Set position degree from home value (default 0deg) for inner axis
        /// </summary>
        /// <param name="deg">Position [deg]</param>
        public void SetAxis1(int deg)
        {
            SendProvider(BuildMessage(AXI));

            SendProvider(BuildMessage(MOV, deg.ToString()));
        }

        /// <summary>
        /// Set position degree from home value (default 0deg), and velocity for inner axis
        /// </summary>
        /// <param name="deg">Position [deg]</param>
        /// <param name="vel">Velocity [deg/s]</param>
        public void SetAxis1(int deg, int vel)
        {
            SendProvider(BuildMessage(AXI));

            SendProvider(BuildMessage(MOV, deg.ToString(), vel.ToString()));
        }

        /// <summary>
        /// Set position degree from home value (default 0deg) for outer axis
        /// </summary>
        /// <param name="deg">Position [deg]</param>
        public void SetAxis2(int deg)
        {
            SendProvider(BuildMessage(AXO));

            SendProvider(BuildMessage(MOV, deg.ToString()));
        }

        /// <summary>
        /// Set position degree from home value (default 0deg), and velocity for inner axis
        /// </summary>
        /// <param name="deg">Position [deg]</param>
        /// <param name="vel">Velocity [deg/s]</param>
        public void SetAxis2(int deg, int vel)
        {
            SendProvider(BuildMessage(AXO));

            SendProvider(BuildMessage(MOV, deg.ToString(), vel.ToString()));
        }

        /// <summary>
        /// Stop select axis. Not turn on breake, just stop moving
        /// </summary>
        /// <param name="axes">Selected axis</param>
        public void Stop(Axes_e axes)
        {
            switch (axes)
            { 
                case Axes_e.Inner:
                    SendProvider(BuildMessage(AXI));

                    SendProvider(BuildMessage(STO));
                    break;
                case Axes_e.Outer:
                    SendProvider(BuildMessage(AXO));

                    SendProvider(BuildMessage(STO));
                    break;
                case Axes_e.All:
                    SendProvider(BuildMessage(AXI));

                    SendProvider(BuildMessage(STO));
                    
                    SendProvider(BuildMessage(AXO));

                    SendProvider(BuildMessage(STO));
                    break;
            }
        }

        /// <summary>
        /// Send command.
        /// </summary>
        /// <param name="cmd">Command</param>
        public void SendCommandProvider(string cmd)
        {
            SendProvider(BuildMessage(cmd));
        }

        /// <summary>
        /// read reposnse if it is possible
        /// </summary>
        /// <returns>Respose</returns>
        public string ReadCommandProvider()
        {
            var response = ReadProvider();
            return ASCIIEncoding.ASCII.GetString(response);
        }

        /// <summary>
        /// Provides test step.
        /// </summary>
        /// <param name="rateTable">State of rate-table</param>
        /// <param name="climaChamber">State of climatic chamber</param>
        /// <param name="axisInner"></param>
        /// <param name="axisInnerRotate"></param>
        /// <param name="axisInnerPos"></param>
        /// <param name="axisInnerVel"></param>
        /// <param name="axisOuter"></param>
        /// <param name="axisOuterRotate"></param>
        /// <param name="axisOuterPos"></param>
        /// <param name="axisOuterVel"></param>
        public void ProvidesCommands(bool rateTable, bool climaChamber, bool axisInner, bool axisInnerRotate, int axisInnerPos, int axisInnerVel, bool axisOuter,bool axisOuterRotate, int axisOuterPos, int axisOuterVel)
        {
            CommandsParametrs_t _val = new CommandsParametrs_t();
            Dictionary<string, CommandsParametrs_t> _cmdlist = new Dictionary<string, CommandsParametrs_t>();

            if (climaChamber)
            {
                _val.value1 = new char[] { '1' };
                _cmdlist.Add(THO, _val);
            }
            else
            { 
                _val.value1 = new char[] { '0' };
                _cmdlist.Add(THO, _val);
            }

            if (rateTable)
            {
                _val = new CommandsParametrs_t();
                _cmdlist.Add(AXI, _val);

                if (axisInner)
                {
                    if (axisInnerRotate)
                    {
                        _val.value1 = axisInnerVel.ToString().ToCharArray();
                        _cmdlist.Add(JOG, _val);
                    }
                    else
                    {
                        _val.value1 = axisInnerPos.ToString().ToCharArray();
                        _val.value2 = axisInnerVel.ToString().ToCharArray();
                        _cmdlist.Add(MOV, _val);
                    }
                }
                else
                {
                    _val.value1 = new char[] { '1' };
                    _cmdlist.Add(BRK, _val);
                }

                StartTask(_cmdlist);
                _cmdlist = new Dictionary<string, CommandsParametrs_t>();

                _val = new CommandsParametrs_t();
                _cmdlist.Add(AXO, _val);
                
                if (axisOuter)
                {
                    if (axisOuterRotate)
                    {
                        _val.value1 = axisOuterVel.ToString().ToCharArray();
                        _cmdlist.Add(JOG, _val);
                    }
                    else
                    {
                        _val.value1 = axisOuterPos.ToString().ToCharArray();
                        _val.value2 = axisOuterVel.ToString().ToCharArray();
                        _cmdlist.Add(MOV, _val);
                    }
                }
                else
                {
                    _val.value1 = new char[] { '1' };
                    _cmdlist.Add(BRK, _val);
                }

                StartTask(_cmdlist);
                _cmdlist = new Dictionary<string, CommandsParametrs_t>();
            }
            else
            {
                _val = new CommandsParametrs_t();
                _cmdlist.Add(AXI, _val);
                _val.value1 = new char[] { '1' };
                _cmdlist.Add(BRK, _val);
                StartTask(_cmdlist);

                _cmdlist = new Dictionary<string, CommandsParametrs_t>();
                _cmdlist.Add(AXO, _val);
                _val.value1 = new char[] { '1' };
                _cmdlist.Add(BRK, _val);
                StartTask(_cmdlist);
            }
        }

        public void ManualUpdate()
        {
            CommandsParametrs_t _val = new CommandsParametrs_t();
            Dictionary<string, CommandsParametrs_t> _cmdlist = new Dictionary<string, CommandsParametrs_t>();

            #region ------- Inner
            _cmdlist.Add(AXI, _val);
            
            _val.isQuestion = true;
            _cmdlist.Add(AxisValues_t.ZeroPositionCmd, _val);

            _cmdlist.Add(AxisValues_t.VelocityCmd, _val);

            _val.isQuestion = false;
            _cmdlist.Add(AxisValues_t.RefPositionCmd, _val);

            _cmdlist.Add(AxisValues_t.ActualPositionCmd, _val);

            _cmdlist.Add(AxisValues_t.ActualAccelerationCmd, _val);

            StartTask(_cmdlist);
            #endregion

            #region ------- Outer
            _cmdlist = new Dictionary<string, CommandsParametrs_t>();
            _cmdlist.Add(AXO, _val);

            _val.isQuestion = true;
            _cmdlist.Add(AxisValues_t.ZeroPositionCmd, _val);

            _cmdlist.Add(AxisValues_t.VelocityCmd, _val);

            _val.isQuestion = false;
            _cmdlist.Add(AxisValues_t.RefPositionCmd, _val);

            _cmdlist.Add(AxisValues_t.ActualPositionCmd, _val);

            _cmdlist.Add(AxisValues_t.ActualAccelerationCmd, _val);

            StartTask(_cmdlist);
            #endregion       
        }

        /// <summary>
        /// Stop both axis and active breaks
        /// </summary>
        /// <returns></returns>
        public bool Stop()
        {
            CommandsParametrs_t _val = new CommandsParametrs_t();
            Dictionary<string, CommandsParametrs_t> _cmdlist = new Dictionary<string, CommandsParametrs_t>();
            _cmdlist.Add(AXO, _val);
            _val.value1 = new char[] { '1' };
            _cmdlist.Add(BRK, _val);

            _val = new CommandsParametrs_t();
            _cmdlist.Add(AXI, _val);
            _val.value1 = new char[] { '1' };
            _cmdlist.Add(BRK, _val);

            StartTask(_cmdlist);

            return true;
        }

        /// <summary>
        /// Release breaks and active both axis to position {0,0}
        /// </summary>
        /// <returns></returns>
        public bool Run()
        {
            CommandsParametrs_t _val = new CommandsParametrs_t();
            Dictionary<string, CommandsParametrs_t> _cmdlist = new Dictionary<string, CommandsParametrs_t>();
            _cmdlist.Add(AXO, _val);
            _cmdlist.Add(HOM, _val);
            //_val.value1 = new char[] { '1' };
            //_cmdlist.Add(BRK, _val);
            StartTask(_cmdlist);

            _cmdlist = new Dictionary<string, CommandsParametrs_t>();
            _val = new CommandsParametrs_t();
            _cmdlist.Add(AXI, _val);
            _cmdlist.Add(HOM, _val);
            //_val.value1 = new char[] { '1' };
            //_cmdlist.Add(BRK, _val);
            StartTask(_cmdlist);

            return true;
        }

        public float GetInnerZeroPos()
        {
            return InnerAxisActualValues.ZeroPosition;
        }

        public float GetInnerVel()
        {
            return InnerAxisActualValues.Velocity;
        }

        public int GetInnerStatus()
        {
            return InnerAxisActualValues.AxisStatus;
        }

        public float GetInnerRefPos()
        {
            return InnerAxisActualValues.RefPosition;
        }

        public float GetInnerActPoc()
        {
            return InnerAxisActualValues.ActualPosition;
        }

        public float GetInnerActAcc()
        {
            return InnerAxisActualValues.ActualAcceleration;
        }

        public float GetOuterZeroPos()
        {
            return OuterAxisActualValues.ZeroPosition;
        }

        public float GetOuterVel()
        {
            return OuterAxisActualValues.Velocity;
        }

        public int GetOuterStatus()
        {
            return OuterAxisActualValues.AxisStatus;
        }

        public float GetOuterRefPos()
        {
            return OuterAxisActualValues.RefPosition;
        }

        public float GetOuterActPoc()
        {
            return OuterAxisActualValues.ActualPosition;
        }

        public float GetOuterActAcc()
        {
            return OuterAxisActualValues.ActualAcceleration;
        }

        #endregion
    }
}
