using System;
using System.Collections.Generic;
using System.Linq;
using System.IO;
using iSTP_Embedded;
using System.Timers;

namespace iSTP_DataProcessing
{
    /// <summary>
    /// SPS mode 
    /// </summary>
    public class SPSLogger
    {
        #region Private global values
        private DateTime Timestamp_TestStart;
        private SPSsensorClass SPSsensorLists;
        private bool isScriptsRun = false;
        /// <summary>
        /// All active sensor
        /// </summary>
        private Dictionary<string, ActiveAccGyroStruct> ActiveSensors = new Dictionary<string, ActiveAccGyroStruct>(100);
        private Timer myTimer = new Timer(10000d);
        private bool[] IsStabTimeLog;

        private BinaryWriter[] BiasFile;

        private BinaryWriter[] AvgFile;
        private BinaryWriter[] RawFile;
        private BinaryWriter[] UncompensatedFile;
        private bool isAVGinit = false;
        private int CycleNumber = 0;
        private ISTPConfiguration EmbConfiguration;
        static private SPSscripts SPSscript = new SPSscripts();
        private List<string> sensors = new List<string>();
        private uint sensorNumber = 1;
        static private SPSscripts.Sets_t ActiveSet = new SPSscripts.Sets_t();
        private List<string> sensorsSerial = new List<string>();
        private bool AreCountsSame ///<summary> sensors == sensorsSerial </summary>
        {
            get
            {
                if (sensors != null && sensorsSerial != null)
                {
                    if (sensorsSerial.Count == sensors.Count)
                        return true;
                }
                return false;
            }
        }
        private readonly string[] FolderTop = new string[] { "SensorResults", "T1", "T2", "T3" };
        private FolderManagerClass FoldersInTest = null;
        private string RootFolder = null;

        private uint T3counter = 1;
        /// <summary>
        /// Create string 'Cxxx'
        /// </summary>
        private string GetT3testFolderName { get { return String.Format("C{0:000}", T3counter++); } }
        /// <summary>
        /// Holds all strings 'Cxxx'
        /// </summary>
        private List<string> T3testFoleders = new List<string>();
        /// <summary>
        /// Gets last string 'Cxxx' 
        /// </summary>
        private string GetT3LastTestpath
        {
            get
            {
                if (T3testFoleders.Count > 0)
                    return T3testFoleders[T3testFoleders.Count - 1];
                else
                    return string.Empty;
            }
        }
        private object locker = new object();
        private string LastPath2Cycle = null;
        private string ActualCyclePath = string.Empty;
        private bool isAvgAllowWrite = false;
        private float[] AvgDataMem = null;
        private float AvgDataCounter = 0f;
        private float TempNominal = 0f;
        private float Range = 0.5f;
        #endregion

        #region Public global values
        /// <summary>
        /// Gets current sensor SPS configuration
        /// </summary>
        public Dictionary<string, ActiveAccGyroStruct> GetConfigSPS { get { return ActiveSensors; } }

        public string GetRootFolder { get { return RootFolder; } }
        #endregion

        #region Structs & Enums
        /// <summary>
        /// Database of actives sensors
        /// </summary>
        public struct ActiveSensorStruct
        {
            public ISTPConfiguration.Sensor_t SensInfo;
            public int StartData;
            public int StopData;
        }

        /// <summary>
        /// Database of actives accelerometers and gyroscopes
        /// </summary>
        public struct ActiveAccGyroStruct
        {
            public ActiveSensorStruct Acc;
            public ActiveSensorStruct Gyro;
            public TempStabTime_t tempStabTimes;
            public List<int> TempIndexes;
            public BiasTime_t BiasTime;
        }

        /// <summary>
        /// Bias parameters database for one sensor
        /// </summary>
        public struct BiasTime_t
        {
            public bool isInitCompl;
            public bool isOneHourCompl;
            public float OneHourTime;
            public bool isTwoHourCompl;
            public float TwoHourTime;
            public float[] BiasTemp;
            public float[] BiasTime;
            public float[][] BiasData;
        }

        /// <summary>
        /// Conditions database for one sensor
        /// </summary>
        public struct TempStabTime_t
        {
            public bool isTimeActive;
            public bool isConditionsActive;
            public double Time;
            public double Conditions_Temp;
            public double Conditions_Maxlimit;
            public double Conditions_Minlimit;
            public double Conditions_TimeLimit;
            public double Conditions_FailTime;
            public double Conditions_OffsetTime;
            public double Stopwatch;
            public System.Diagnostics.Stopwatch MeasureTime;
            public bool Finish;
            public double FirstPacketTimestamp;
        }

        #endregion

        #region Constructors & Destructor
        /// <summary>
        /// Create all folders without tests folders and create internal sensor database
        /// Folder name is same as Definitions.FolderName
        /// </summary>
        public SPSLogger(string aDestination, ISTPConfiguration aConfig, SPSscripts.Sets_t aActive)
        {
            SPSsensorLists = new SPSsensorClass();
            ActiveSet = aActive;
            RootFolder = aDestination;
            if (!Directory.Exists(aDestination))
            {
                try
                {
                    Directory.CreateDirectory(aDestination);
                }
                catch (Exception ex)
                {
                    DataProcessingError.Add("SPSLogger.cs", "Error during creating SPS folders", ex.ToString());
                }
            }

            createSubrootFolders(RootFolder);
            aConfig.DumpConfiguration(aDestination + "\\EmbeddedConfig.txt");

            JoinAccAndGyro(aConfig);
            ChangeTempIndex(aConfig);
            EmbConfiguration = aConfig;
            myTimer.Elapsed += myTimer_Elapsed;
        }
        #endregion

        #region Private Sector
        /// <summary>
        /// Allow average calculation
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void myTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            isAvgAllowWrite = true;
        }
        /// <summary>
        /// Saves list of active sensors in *.csv iFile
        /// </summary>
        private void SaveSensorList()
        {
            try
            {
                if (GetT3LastTestpath == string.Empty)
                    return;
                StreamWriter csvfile = new StreamWriter(string.Format("{0}{1}SensorList.csv", GetT3LastTestpath, Path.DirectorySeparatorChar));
                string[] line = new string[sensors.Count];
                for (int i = 0; i < sensors.Count; i++)
                {
                    string sensserial = sensorsSerial[i];
                    if (sensserial.Contains(' '))
                        sensserial = sensserial.Replace(' ','0');
                    line[i] = string.Format("{0},{1}", sensors[i], sensserial);
                }
                foreach (string s in line)
                    csvfile.WriteLine(s);
                csvfile.Close();

                SPSsensorLists.Add(sensors.ToArray());
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Can't create SensorList.csv", ex.ToString());
            }
        }

        /// <summary>
        /// Save global SensorList.csv in root folder as merge of all SensorList.csv in cycles Cxxx
        /// </summary>
        private void SaveSensorListGlobal()
        {
            try
            {
                string[] _aliveSens = SPSsensorLists.GetSensorList();
                StreamWriter _csvfile = new StreamWriter(string.Format("{0}{1}SensorList.csv", RootFolder, Path.DirectorySeparatorChar));
                foreach (string sens in _aliveSens)
                {
                    for (int i = 0; i < sensors.Count; i++)
                    {
                        if (sensors[i] == sens)
                        {
                            _csvfile.WriteLine(string.Format("{0},{1}", sensors[i], sensorsSerial[i]));
                        }
                    }
                }
                _csvfile.Close();
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Can't create SensorList.csv", ex.ToString());
            }
        }
        /// <summary>
        /// Create Cxxx folder and save its path ('xxx' is number of folder _start at 0 and increment by 1).
        /// </summary>
        private void AddT3testFolder()
        {
            string name = GetT3testFolderName;
            string path = string.Format("{0}{1}T3", RootFolder, Path.DirectorySeparatorChar);
            CreateT3testFolder(path, name);
            path += Path.DirectorySeparatorChar + name;
            T3testFoleders.Add(path);
            ActualCyclePath = path;
        }

        /// <summary>
        /// Create folder with input name in input path
        /// </summary>
        /// <param name="path">User specific path</param>
        /// <param name="name">Define name</param>
        private void CreateT3testFolder(string path, string name)
        {
            try
            {
                path += Path.DirectorySeparatorChar + name;
                if (!Directory.Exists(path))
                    Directory.CreateDirectory(path);
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", string.Format("Can't create folder {0} at {1}", name, path), ex.ToString());
            }
        }

        /// <summary>
        /// Creates all mandatory folders in last cycle's folder Cxxx
        /// </summary>
        private void CreateT3folderInLastCxx()
        {
            if (GetT3LastTestpath == string.Empty)
                return;

            FoldersInTest = new FolderManagerClass(GetT3LastTestpath);
        }

        /// <summary>
        /// Creates SubRoots folders only. Basic folders T1,T2, ...
        /// </summary>
        /// <param name="path">Path when all test will be save</param>
        /// <returns>True - success, False - Error appears, exception throw and check ErrorLog for more _info</returns>
        private Boolean createSubrootFolders(string path)
        {
            if (path != null)
            {
                try
                {
                    foreach (string dirName in FolderTop)
                    {
                        string _mypath = path + Path.DirectorySeparatorChar + dirName;

                        if (!Directory.Exists(_mypath))
                        {
                            if (!Directory.CreateDirectory(_mypath).Exists)
                                return false;
                            if (dirName == "SensorResults")
                            {
                                _mypath = string.Format("{0}{1}Plots", _mypath, Path.DirectorySeparatorChar);

                                if (!Directory.Exists(_mypath))
                                {
                                    if (!Directory.CreateDirectory(_mypath).Exists)
                                        return false;
                                }
                            }
                        }
                    }
                    return true;
                }
                catch (Exception ex)
                {
                    DataProcessingError.Add("SPSLogger.cs", "Can't create subfolders", ex.ToString());
                    return false;
                }
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// Separation _data by input indexes (start, end)
        /// </summary>
        /// <param name="startIndex">Start index</param>
        /// <param name="stopIndex">End index</param>
        /// <param name="rawData">Raw _data</param>
        /// <returns>Array fill with values at specify indexes</returns>
        public static float[] GetDataPart(int startIndex, int stopIndex, ref float[] rawData)
        {
            float[] Result = new float[stopIndex - startIndex + 1];
            if (rawData.Length == 0)
            {
                for (int i = startIndex; i <= stopIndex; i++)
                    Result[i - startIndex] = 0f;
                return Result;
            }
            if (stopIndex >= rawData.Length)
                stopIndex = rawData.Length - 1;
            if (rawData.Length >= 1)
            {
                float[] _localBuff = rawData;
                for (int m = startIndex; m <= stopIndex; m++)
                {
                    Result[m - startIndex] = _localBuff[m];
                }
            }
            else
            {
                for (int j = (int)startIndex; j <= stopIndex; j++)
                {
                    Result[j - startIndex] = 0;
                }
            }
            return Result;
        }

        /// <summary>
        /// Create new average file in Cxxx/Avg/
        /// </summary>
        private void CreateNewAvgLog()
        {
            try
            {
                isAVGinit = false;
                string path = FoldersInTest.GetPath_Avg;
                AvgFile = new BinaryWriter[sensors.Count];
                for (int i = 0; i < sensors.Count; i++)
                    AvgFile[i] = new BinaryWriter(File.Open(string.Format("{0}{1}.bin", path, sensors[i]), FileMode.Create, FileAccess.Write));
                myTimer.Start();
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Can't create avg files", ex.ToString());
            }
        }

        /// <summary>
        /// Creates, open, write and close into *.bin File also fill bias structure
        /// </summary>
        /// <param name="aPacket">Data packet from platform</param>
        private void LogAvgData(EmbeddedPacket aPacket)
        {
            try
            {
                float[] AccData = null;
                float[] GyroData = null;
                float myTime = 0f;
                bool isAccActive = false;
                bool isGyroActive = false;
                int length = 0;

                if (!isAVGinit)
                {
                    AvgDataMem = new float[aPacket.MsgLength / 4];
                    isAVGinit = true;
                }

                for (int i = 0; i < AvgDataMem.Length; i++)
                {
                    AvgDataMem[i] += aPacket[i];
                }
                AvgDataCounter++;

                if (!isAvgAllowWrite)
                {
                    return;
                }
                else
                {
                    isAvgAllowWrite = false;
                    if (AvgDataCounter == 0)
                        AvgDataCounter++;

                    for (int i = 0; i < AvgDataMem.Length; i++)
                    {
                        AvgDataMem[i] = AvgDataMem[i] / AvgDataCounter;
                    }
                    AvgDataCounter = 0;
                }

                #region read data
                float[] allData = AvgDataMem;
                myTime = Convert.ToSingle(aPacket.MsgTimeStamp);
                #endregion

                for (int k = 0; k < ActiveSensors.Count; k++)
                {
                    if (k > AvgFile.Length)
                    {
                        DataProcessingError.Add("SPSLogger.cs", "Number of AVG file is not equal to number of sensors", string.Empty);
                        continue;
                    }

                    #region Sort _data
                    ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();
                    if (!this.ActiveSensors.TryGetValue(ActiveSensors.Keys.ElementAt(k), out _localBuffer))
                    {
                        DataProcessingError.Add("SPSLogger.LogAvgData()", string.Format("Cannot find {0} in my internal memory", sensors[k]), string.Empty);
                        continue;
                    }
                    SortData(ref AccData, ref GyroData, ref isAccActive, ref isGyroActive, ref length, ref allData, ref _localBuffer);
                    #endregion

                    #region Data processing
                    float _tempFinal = 0f;
                    if (_localBuffer.TempIndexes != null)
                    {
                        _tempFinal = aPacket[_localBuffer.TempIndexes[0]];
                        if (_localBuffer.TempIndexes.Count == 2)
                        {
                            _tempFinal += aPacket[_localBuffer.TempIndexes[1]];
                            _tempFinal = _tempFinal / 2;
                        }
                    }

                    float[] data = new float[] 
                    {
                        GyroData[0], GyroData[1], GyroData[2],
                        AccData[0], AccData[1], AccData[2]
                    };

                    for (int i = 0; i < data.Length; i++)
                        if (data[i] == float.NaN || data[i] == float.NegativeInfinity || data[i] == float.PositiveInfinity)
                            data[i] = 0;

                    WriteInFile(myTime, _tempFinal, data, AvgFile[k]);

                    #region Write Bias
                    if (!_localBuffer.BiasTime.isInitCompl)
                    {
                        if ((_localBuffer.tempStabTimes.Finish || (!_localBuffer.tempStabTimes.isConditionsActive && !_localBuffer.tempStabTimes.isTimeActive)) && IsStabTimeLog[k])
                        {
                            _localBuffer.BiasTime.isInitCompl = true;
                            _localBuffer.BiasTime.BiasTime[0] = Convert.ToSingle(_localBuffer.tempStabTimes.Stopwatch);
                            _localBuffer.BiasTime.BiasTemp[0] = _tempFinal;
                            _localBuffer.BiasTime.BiasData[0] = data;

                            _localBuffer.BiasTime.OneHourTime = Convert.ToSingle(_localBuffer.tempStabTimes.Stopwatch) + 3600;
                            _localBuffer.BiasTime.TwoHourTime = Convert.ToSingle(_localBuffer.tempStabTimes.Stopwatch) + 7200;
                        }
                    }
                    else if ((myTime >= _localBuffer.BiasTime.OneHourTime) && !_localBuffer.BiasTime.isOneHourCompl)
                    {
                        _localBuffer.BiasTime.isOneHourCompl = true;
                        _localBuffer.BiasTime.BiasTime[1] = myTime;
                        _localBuffer.BiasTime.BiasTemp[1] = _tempFinal;
                        _localBuffer.BiasTime.BiasData[1] = data;
                    }
                    else if ((myTime >= _localBuffer.BiasTime.TwoHourTime) && !_localBuffer.BiasTime.isTwoHourCompl)
                    {
                        _localBuffer.BiasTime.isTwoHourCompl = true;
                        _localBuffer.BiasTime.BiasTime[2] = myTime;
                        _localBuffer.BiasTime.BiasTemp[2] = _tempFinal;
                        _localBuffer.BiasTime.BiasData[2] = data;
                    }
                    else
                    {
                        _localBuffer.BiasTime.BiasTime[3] = myTime;
                        _localBuffer.BiasTime.BiasTemp[3] = _tempFinal;
                        _localBuffer.BiasTime.BiasData[3] = data;
                    }
                    #endregion

                    #endregion
                    ActiveSensors[ActiveSensors.Keys.ElementAt(k)] = _localBuffer;
                } //for (int k = 0; k < ActiveSensors.Count; k++)
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Exception occurs", ex.ToString());
            }
        }

        /// <summary>
        /// Fills AccData and GyroData with real data from allData or set values to 0
        /// </summary>
        /// <param name="AccData">Reference to Acc data array. Overwrite in function</param>
        /// <param name="GyroData">Reference to Gyro data array. Overwrite in function</param>
        /// <param name="isAccActive">Returns active status of Acc data</param>
        /// <param name="isGyroActive">Returns active status of Gyro data</param>
        /// <param name="length"></param>
        /// <param name="allData">Data from packet</param>
        /// <param name="_localBuffer">Current sensor</param>
        private void SortData(ref float[] AccData, ref float[] GyroData, ref bool isAccActive, ref bool isGyroActive, ref int length, ref float[] allData, ref ActiveAccGyroStruct _localBuffer)
        {
            if (_localBuffer.Acc.StartData != -1 && _localBuffer.Gyro.StartData != -1)
            {
                isAccActive = true;
                isGyroActive = true;
                lock (locker)
                {
                    AccData = GetDataPart(_localBuffer.Acc.StartData, _localBuffer.Acc.StopData, ref allData);
                    GyroData = GetDataPart(_localBuffer.Gyro.StartData, _localBuffer.Gyro.StopData, ref allData);
                }
                length = _localBuffer.Acc.StopData - _localBuffer.Acc.StartData;
            }
            else if (_localBuffer.Acc.StartData != -1)
            {
                isAccActive = true;
                lock (locker)
                {
                    AccData = GetDataPart(_localBuffer.Acc.StartData, _localBuffer.Acc.StopData, ref allData);
                    GyroData = AccData;

                    for (uint j = 0; j < GyroData.Length; j++)
                        GyroData[j] = 0f;
                }
                length = _localBuffer.Acc.StopData - _localBuffer.Acc.StartData;
            }
            else
            {
                isGyroActive = true;
                lock (locker)
                {
                    GyroData = GetDataPart(_localBuffer.Gyro.StartData, _localBuffer.Gyro.StopData, ref allData);
                    AccData = GyroData;
                    for (uint j = 0; j < AccData.Length; j++)
                        AccData[j] = 0f;
                }
                length = _localBuffer.Gyro.StopData - _localBuffer.Gyro.StartData;
            }
        }

        /// <summary>
        /// Close active Avg files and stop myTimer
        /// </summary>
        private void TerminateAvg()
        {
            if (AvgFile != null)
            {
                for (int i = 0; i < AvgFile.Length; i++)
                {
                    if (AvgFile[i] != null)
                    {
                        AvgFile[i].Close();
                        AvgFile[i] = null;
                    }
                }
                AvgFile = null;
            }
            myTimer.Stop();
        }

        /// <summary>
        /// Creates new log for raw _data
        /// </summary>
        /// <param name="aContext"></param>
        private void CreateNewRawLog()
        {
            try
            {
                string path = FoldersInTest.GetPath_Raw;
                RawFile = new BinaryWriter[sensors.Count];
                for (int i = 0; i < sensors.Count; i++)
                    RawFile[i] = new BinaryWriter(File.Open(string.Format("{0}{1}.bin", path, sensors[i]), FileMode.Create, FileAccess.Write));
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Can't create raw files", ex.ToString());
            }
        }

        /// <summary>
        /// Creates all required uncompensated files
        /// </summary>
        private void CreateNewUncompensatedLog()
        {
            try
            {
                string path = string.Format("{0}{1}Uncompensated", FoldersInTest.GetPath_Raw, Path.DirectorySeparatorChar);
                if (!Directory.Exists(path))
                    Directory.CreateDirectory(path);
                path += Path.DirectorySeparatorChar;

                UncompensatedFile = new BinaryWriter[sensors.Count];
                for (int i = 0; i < sensors.Count; i++)
                    UncompensatedFile[i] = new BinaryWriter(File.Open(string.Format("{0}{1}.bin", path, sensors[i]), FileMode.Create, FileAccess.Write));
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Can't create raw files", ex.ToString());
            }
        }

        /// <summary>
        /// Creates, open, write and close into *.bin File
        /// </summary>
        /// <param name="threadcontext">BinarythreadsContextStrc input parameter as object</param>
        private void LogRawData(EmbeddedPacket aPacket)
        {
            try
            {
                float[] AccData = null;
                float[] GyroData = null;
                Single myTime = Convert.ToSingle(aPacket.MsgTimeStamp);
                bool isAccActive = false;
                bool isGyroActive = false;
                int length = 0;

                #region read _data
                float[] allData = new float[aPacket.MsgLength / sizeof(float)];
                for (int i = 0; i < allData.Length; i++)
                {
                    allData[i] += aPacket[i];
                }
                #endregion

                for (int k = 0; k < ActiveSensors.Count; k++)
                {
                    if (k > RawFile.Length)
                    {
                        DataProcessingError.Add("SPSLogger.cs", "Number of RAW file is not equal to number of sensors", string.Empty);
                        continue;
                    }

                    #region Sort _data
                    ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();
                    if (!this.ActiveSensors.TryGetValue(ActiveSensors.Keys.ElementAt(k), out _localBuffer))
                    {
                        DataProcessingError.Add("SPSLogger.LogRawData()", string.Format("Cannot find {0} in my internal memory", sensors[k]), string.Empty);
                        continue;
                    }
                    SortData(ref AccData, ref GyroData, ref isAccActive, ref isGyroActive, ref length, ref allData, ref _localBuffer);
                    #endregion

                    #region ------------------------ Data processing ------------------------

                    #region ------------------------ check Temperature ------------------------
                    if(_localBuffer.TempIndexes == null)
                        _localBuffer.tempStabTimes.Finish = true;
                    else if (_localBuffer.TempIndexes.Count == 0)
                        _localBuffer.tempStabTimes.Finish = true;

                    float _tempFinal = 0f;
                    if (_localBuffer.TempIndexes != null)
                    {
                        _tempFinal = aPacket[_localBuffer.TempIndexes[0]];
                        if (_localBuffer.TempIndexes.Count == 2)
                        {
                            _tempFinal += aPacket[_localBuffer.TempIndexes[1]];
                            _tempFinal = _tempFinal / 2;
                        }
                    }
                    //Conditions is uncomplete
                    if (!_localBuffer.tempStabTimes.Finish)
                    {
                        //Conditions is active (no = time condition is active)
                        if (_localBuffer.tempStabTimes.isConditionsActive)
                        {
                            //Save first packet Timestamp
                            if (_localBuffer.tempStabTimes.FirstPacketTimestamp == 0)
                            {
                                _localBuffer.tempStabTimes.FirstPacketTimestamp = aPacket.MsgTimeStamp;
                            }
                            //Alocate stopwatch
                            if (_localBuffer.tempStabTimes.MeasureTime == null)
                                _localBuffer.tempStabTimes.MeasureTime = new System.Diagnostics.Stopwatch();
                            if (aPacket.MsgTimeStamp >= _localBuffer.tempStabTimes.Conditions_OffsetTime)
                            {
                                //Actual temperature is in range <MIN;MAX>
                                if (_tempFinal <= _localBuffer.tempStabTimes.Conditions_Maxlimit && _tempFinal >= _localBuffer.tempStabTimes.Conditions_Minlimit)
                                {
                                    //Stopwatch doesn't run => start it
                                    if (!_localBuffer.tempStabTimes.MeasureTime.IsRunning)
                                    {
                                        _localBuffer.tempStabTimes.MeasureTime.Start();
                                    }
                                    //Actual timestamp is greater than conditions time => save data for first row
                                    else if (_localBuffer.tempStabTimes.MeasureTime.Elapsed.TotalSeconds >= _localBuffer.tempStabTimes.Conditions_TimeLimit)
                                    {
                                        if (!this.IsStabTimeLog[k])
                                        {
                                            _localBuffer.tempStabTimes.Finish = true;
                                            _localBuffer.tempStabTimes.MeasureTime.Stop();
                                            _localBuffer.tempStabTimes.Stopwatch = aPacket.MsgTimeStamp;
                                            this.IsStabTimeLog[k] = true;
                                        }
                                        if (aPacket.MsgTimeStamp < _localBuffer.tempStabTimes.Conditions_OffsetTime)
                                            _localBuffer.tempStabTimes.Stopwatch = _localBuffer.tempStabTimes.Conditions_OffsetTime;
                                    }
                                }
                                else
                                {
                                    _localBuffer.tempStabTimes.MeasureTime.Stop();
                                    _localBuffer.tempStabTimes.MeasureTime.Reset();
                                }
                            }

                            if ((aPacket.MsgTimeStamp - _localBuffer.tempStabTimes.FirstPacketTimestamp) >= _localBuffer.tempStabTimes.Conditions_FailTime)
                            {
                                if (!this.IsStabTimeLog[k])
                                {
                                    _localBuffer.tempStabTimes.Finish = true;
                                    _localBuffer.tempStabTimes.MeasureTime.Stop();
                                    _localBuffer.tempStabTimes.Stopwatch = _localBuffer.tempStabTimes.Conditions_FailTime;
                                    this.IsStabTimeLog[k] = true;
                                }
                            }
                        }
                    }
                    #endregion

                    float[] data = new float[] 
                    {
                        GyroData[0], GyroData[1], GyroData[2],
                        AccData[0], AccData[1], AccData[2]
                    };

                    for (int i = 0; i < data.Length; i++)
                        if (data[i] == float.NaN || data[i] == float.NegativeInfinity || data[i] == float.PositiveInfinity)
                            data[i] = 0;

                    WriteInFile(myTime, _tempFinal, data, RawFile[k]);
                    #endregion
                    this.ActiveSensors[ActiveSensors.Keys.ElementAt(k)] = _localBuffer;
                }
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Exception in raw data logger", ex.ToString());
            }
        }

        /// <summary>
        /// Writes uncompensated data in files
        /// </summary>
        /// <param name="aPacket"></param>
        private void LogUncompensatedData(EmbeddedPacket aPacket)
        {
            try
            {
                float[] AccData = null;
                float[] GyroData = null;
                Single myTime = (float)aPacket.MsgTimeStamp;
                bool isAccActive = false;
                bool isGyroActive = false;
                int length = 0;
                int _allDatLeng = aPacket.MsgLength / 4;

                #region ------ read _data ------
                float[] allData = new float[_allDatLeng];
                for (int i = 0; i < allData.Length; i++)
                {
                    allData[i] += aPacket[i];
                }
                #endregion

                for (int k = 0; k < ActiveSensors.Count; k++)
                {
                    #region Sort _data
                    ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();
                    if (!this.ActiveSensors.TryGetValue(ActiveSensors.Keys.ElementAt(k), out _localBuffer))
                    {
                        iSTP_DebugConsole.DebugConsole.WriteLine(string.Format("Cannot find {0} in my internal memory", sensors[k]));
                        continue;
                    }
                    SortData(ref AccData, ref GyroData, ref isAccActive, ref isGyroActive, ref length, ref allData, ref _localBuffer);
                    #endregion

                    #region Data processing

                    float _tempFinal = aPacket[_localBuffer.TempIndexes[0]];
                    if (_localBuffer.TempIndexes.Count == 2)
                    {
                        _tempFinal += aPacket[_localBuffer.TempIndexes[1]];
                        _tempFinal = _tempFinal / 2;
                    }

                    float[] data = new float[] {GyroData[0], GyroData[1], GyroData[2],
                                            AccData[0], AccData[1], AccData[2]};
                    WriteInFile(myTime, _tempFinal, data, UncompensatedFile[k]);
                    #endregion
                }
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Exception in Uncompensated data logger", ex.ToString());
            }
        }

        /// <summary>
        /// Checks if temperature is at define range
        /// </summary>
        /// <param name="aValue"></param>
        /// <returns></returns>
        private bool checkTemperature(float aValue)
        {
            if (TempNominal >= 0)
            {
                if (aValue < (TempNominal - Range) && aValue > (TempNominal + Range))
                {
                    return true;
                }
            }
            else
            {
                if (aValue < (TempNominal + Range) && aValue > (TempNominal - Range))
                {
                    return true;
                }
            }

            return false;
        }

        /// <summary>
        /// Close all files with raw data
        /// </summary>
        private void TerminateRaw()
        {
            if (RawFile != null)
            {
                for (int i = 0; i < RawFile.Length; i++)
                {
                    if (RawFile[i] != null)
                    {
                        RawFile[i].Close();
                        RawFile[i] = null;
                    }
                }
                RawFile = null;
            }
            LastPath2Cycle = GetT3LastTestpath;
        }

        /// <summary>
        /// Close all raw uncompensated files
        /// </summary>
        private void TerminateUncompensated()
        {
            if (UncompensatedFile != null)
            {
                for (int i = 0; i < UncompensatedFile.Length; i++)
                {
                    if (UncompensatedFile[i] != null)
                    {
                        UncompensatedFile[i].Close();
                        UncompensatedFile[i] = null;
                    }
                }
                UncompensatedFile = null;
            }
        }

        /// <summary>
        /// Creates tempStabTimes.bin. Writes all 88 values in it and close it.
        /// </summary>
        private void TempStabTimeWriter()
        {
            if (FoldersInTest == null)
                return;

            string path = FoldersInTest.GetPath_Res;
            BinaryWriter _file = new BinaryWriter(File.Open(path + "tempStabTimes.bin", FileMode.Create, FileAccess.Write));

            #region write in iFile
            string[] mySensors = sensors.ToArray();
            for (int i = 0; i < 88; i++)
            {
                if (i < ActiveSensors.Count)
                {
                    string _key = ActiveSensors.Keys.ElementAt(i);
                    if (ActiveSensors[_key].tempStabTimes.isTimeActive || ActiveSensors[_key].tempStabTimes.isConditionsActive)
                    {
                        WriteInFile((float)ActiveSensors[_key].tempStabTimes.Stopwatch, _file);
                    }
                    else
                    {
                        WriteInFile(0f, _file);
                    }
                }
                else
                {
                    WriteInFile(0f, _file);
                }
            }
            #endregion

            _file.Close();

        }

        /// <summary>
        /// Provides writing data in allbias files
        /// </summary>
        private void WriteBiasProvider()
        {
            if(BiasFile.Length == this.ActiveSensors.Count)
                for (int i = 0; i < BiasFile.Length; i++)
                {
                    ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();
                    if (this.ActiveSensors.TryGetValue(ActiveSensors.Keys.ElementAt(i), out _localBuffer))
                    {
                        WriteBias(_localBuffer.BiasTime.BiasTime[0], _localBuffer.BiasTime.BiasTemp[0], _localBuffer.BiasTime.BiasData[0], BiasFile[i]);
                        WriteBias(_localBuffer.BiasTime.BiasTime[1], _localBuffer.BiasTime.BiasTemp[1], _localBuffer.BiasTime.BiasData[1], BiasFile[i]);
                        WriteBias(_localBuffer.BiasTime.BiasTime[2], _localBuffer.BiasTime.BiasTemp[2], _localBuffer.BiasTime.BiasData[2], BiasFile[i]);
                        WriteBias(_localBuffer.BiasTime.BiasTime[3], _localBuffer.BiasTime.BiasTemp[3], _localBuffer.BiasTime.BiasData[3], BiasFile[i]);
                    }
                }
            else
            {
                int len = (BiasFile.Length > this.ActiveSensors.Count) ? this.ActiveSensors.Count : BiasFile.Length ;
                for (int i = 0; i < len; i++)
                {
                    ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();
                    if (this.ActiveSensors.TryGetValue(ActiveSensors.Keys.ElementAt(i), out _localBuffer))
                    {
                        WriteBias(_localBuffer.BiasTime.BiasTime[0], _localBuffer.BiasTime.BiasTemp[0], _localBuffer.BiasTime.BiasData[0], BiasFile[i]);
                        WriteBias(_localBuffer.BiasTime.BiasTime[1], _localBuffer.BiasTime.BiasTemp[1], _localBuffer.BiasTime.BiasData[1], BiasFile[i]);
                        WriteBias(_localBuffer.BiasTime.BiasTime[2], _localBuffer.BiasTime.BiasTemp[2], _localBuffer.BiasTime.BiasData[2], BiasFile[i]);
                        WriteBias(_localBuffer.BiasTime.BiasTime[3], _localBuffer.BiasTime.BiasTemp[3], _localBuffer.BiasTime.BiasData[3], BiasFile[i]);
                    }
                }
            }
        }

        /// <summary>
        /// Write in specific bias file.
        /// </summary>
        /// <param name="time">Packet time</param>
        /// <param name="temp">Sensors temperature</param>
        /// <param name="data">Data from sensors</param>
        /// <param name="file">Specific file</param>
        private void WriteBias(float time, float temp, float[] data, BinaryWriter file)
        {
            if (file != null)
            {
                if (data == null)
                    data = new float[6];
                else if (data.Length == 0)
                    data = new float[6];

                file.Write(time);
                file.Write(temp);
                int i = 0;
                foreach (float x in data)
                {
                    file.Write(x);
                    if (++i == 6)
                        break;
                }
                if (data.Length < 6)
                    for (i = data.Length; i < 6; i++)
                        file.Write(0f);
                file.Flush();
            }
        }

        /// <summary>
        /// Create all Bias files
        /// </summary>
        private void OpenBias()
        {
            try
            {
                string path = this.FoldersInTest.GetPath_ResSubfolders[0];
                this.BiasFile = new BinaryWriter[sensors.Count];
                for (int i = 0; i < sensors.Count; i++)
                {
                    this.BiasFile[i] = new BinaryWriter(File.Open(string.Format("{0}biasTestRes_{1}.bin", path, sensors[i]), FileMode.Create, FileAccess.Write));
                }
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Can't create bias files", ex.ToString());
            }
        }

        /// <summary>
        /// Close all Bias files
        /// </summary>
        private void CloseBias()
        {
            if (this.BiasFile != null)
            {
                for (int i = 0; i < this.BiasFile.Length; i++)
                {
                    this.BiasFile[i].Close();
                }
                this.BiasFile = null;
            }
        }

        /// <summary>
        /// Writes _data in specific iFile
        /// </summary>
        /// <param name="iTime">Time in float</param>
        /// <param name="_tempFinal">temperate</param>
        /// <param name="_data">_data(has to be 6 float array iLength)</param>
        /// <param name="iFile">Specific file</param>
        private void WriteInFile(float time, float temp, float[] data, BinaryWriter file)
        {
            try
            {
                if (file != null)
                {
                    file.Write(time);
                    file.Write(temp);
                    foreach (float x in data)
                        file.Write(x);
                    file.Flush();
                }
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Can't write data in file", ex.ToString());
            }
        }

        /// <summary>
        /// Writes _data in specific iFile
        /// </summary>
        /// <param name="time">Time in float</param>
        /// <param name="file">Specific file</param>
        private void WriteInFile(float time, BinaryWriter file)
        {
            try
            {
                if (file != null)
                {
                    file.Write(time);
                    file.Flush();
                }
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Can't write data in file", ex.ToString());
            }
        }

        /// <summary>
        /// Adds new sensor in sensor list
        /// </summary>
        /// <param name="serial">Serial number of sensor. If you can input more than one serial number use char '+' between them</param>
        /// <returns>True - Success, False - Error appears, check ErrorLog for more _info</returns>
        private Boolean AddNewSensor(string serial)
        {
            if (!AreCountsSame)
            {
                return false;
            }

            sensors.Add(string.Format("S{0:00}", sensorNumber++));

            if (serial.Contains('-'))
                serial.Replace('-', '_');
            sensorsSerial.Add(serial);

            return true;
        }

        /// <summary>
        /// Puts Accelerometers and gyroscope together
        /// </summary>
        private void JoinAccAndGyro(ISTPConfiguration aConfig)
        {
            try
            {
                this.ActiveSensors = new Dictionary<string, ActiveAccGyroStruct>(100);

                const string _acc = "acc";
                const string _gyro = "rate";
                const string _temp = "temp";
                int _index = 0;

                #region -------------------------------- load Acc & Gyro --------------------------------
                List<ActiveSensorStruct> Acc = new List<ActiveSensorStruct>();
                List<ActiveSensorStruct> Gyro = new List<ActiveSensorStruct>();
                Dictionary<string, int> _tempIndexes = new Dictionary<string, int>();

                while (aConfig.Sensors[_index].Quantities != null)
                {
                    if (aConfig.Sensors[_index].SensorName.Contains("TEMP"))
                    {
                        _index++;
                        continue;
                    }
                    bool _foundACC = false;

                    for (int i = 0; i < aConfig.Sensors[_index].Quantities.Length; i++)
                    {
                        if (aConfig.Sensors[_index].Quantities[i].Contains(_temp))
                        {
                            _tempIndexes.Add(aConfig.Sensors[_index].SensorName, aConfig.Sensors[_index].QuantityIndexes[i]);
                            break;
                        }
                    }

                    //try to find acc in sensors values
                    for (int i = 0; i < aConfig.Sensors[_index].Quantities.Length; i++)
                    {
                        if (aConfig.Sensors[_index].Quantities[i].Contains(_acc))
                        {
                            Acc.Add(new ActiveSensorStruct
                            {
                                SensInfo = aConfig.Sensors[_index],
                                StartData = aConfig.Sensors[_index].QuantityIndexes[i],
                                StopData = aConfig.Sensors[_index].QuantityIndexes[i] + 2
                            });
                            //finding rate in same sensor
                            for (i = 0; i < aConfig.Sensors[_index].Quantities.Length; i++)
                            {
                                if (aConfig.Sensors[_index].Quantities[i].Contains(_gyro))
                                {
                                    Gyro.Add(new ActiveSensorStruct
                                    {
                                        SensInfo = aConfig.Sensors[_index],
                                        StartData = aConfig.Sensors[_index].QuantityIndexes[i],
                                        StopData = aConfig.Sensors[_index].QuantityIndexes[i] + 2
                                    });
                                    break;
                                }
                            }

                            _foundACC = true;
                            break;
                        }
                    }

                    // Sensor_t may contains only rate
                    if (!_foundACC)
                    {
                        int i;
                        for (i = 0; i < aConfig.Sensors[_index].Quantities.Length; i++)
                        {
                            if (aConfig.Sensors[_index].Quantities[i].Contains(_gyro))
                            {
                                Gyro.Add(new ActiveSensorStruct
                                {
                                    SensInfo = aConfig.Sensors[_index],
                                    StartData = aConfig.Sensors[_index].QuantityIndexes[i],
                                    StopData = aConfig.Sensors[_index].QuantityIndexes[i] + 2
                                });
                                break;
                            }
                        }
                    }

                    _index++;
                } //while (aConfig.Sensors[_index].Quantities != null)
                #endregion

                List<string> _accnames = new List<string>(100);
                foreach (ActiveSensorStruct strc in Acc)
                {
                    _accnames.Add(strc.SensInfo.SensorName);
                }
                List<string> _gyronames = new List<string>(100);
                foreach (ActiveSensorStruct strc in Gyro)
                {
                    _gyronames.Add(strc.SensInfo.SensorName);
                }

                #region -------------------------------- Acc == Gyro --------------------------------
                if (Acc.Count == Gyro.Count)
                {
                    _index = 0;
                    int maxCyclus = Acc.Count;
                    while (_index < maxCyclus)
                    {
                        ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();

                        string serial = _accnames[0];
                        _accnames.Remove(serial);

                        _localBuffer.Acc = new ActiveSensorStruct()
                        {
                            SensInfo = Acc[_index].SensInfo,
                            StartData = Acc[_index].StartData,
                            StopData = Acc[_index].StopData
                        };

                        int _realAccTempIndex = 0;
                        if (_tempIndexes.TryGetValue(_localBuffer.Acc.SensInfo.SensorName, out _realAccTempIndex))
                        {
                            if (_localBuffer.TempIndexes == null)
                                _localBuffer.TempIndexes = new List<int>();
                            _localBuffer.TempIndexes.Add(_realAccTempIndex);
                        }

                        int _gyrindex = 0;
                        if (!_gyronames.Contains(serial))
                        {
                            int _g = 0;
                            string _gyronameBuf = _gyronames[_g];
                            while (_gyronameBuf.Contains("MPU"))
                            {
                                if (++_g >= _gyronames.Count)
                                    break;
                                _gyronameBuf = _gyronames[_g];
                            }
                            serial = string.Format("{0}+{1}", serial, _gyronameBuf);
                            _gyronames.Remove(_gyronameBuf);

                            foreach (ActiveSensorStruct strc in Gyro)
                            {
                                if (strc.SensInfo.SensorName == _gyronameBuf)
                                    break;
                                else
                                    _gyrindex++;
                            }
                        }
                        else
                        {
                            _gyronames.Remove(serial);
                            foreach (ActiveSensorStruct strc in Gyro)
                            {
                                if (strc.SensInfo.SensorName == serial)
                                    break;
                                else
                                    _gyrindex++;
                            }
                        }

                        _localBuffer.Gyro = new ActiveSensorStruct()
                        {
                            SensInfo = Gyro[_gyrindex].SensInfo,
                            StartData = Gyro[_gyrindex].StartData,
                            StopData = Gyro[_gyrindex].StopData
                        };

                        int _realGyroTempIndex = 0;
                        if (_tempIndexes.TryGetValue(_localBuffer.Gyro.SensInfo.SensorName, out _realGyroTempIndex))
                        {
                            if (_localBuffer.TempIndexes == null)
                                _localBuffer.TempIndexes = new List<int>();
                            if (_realGyroTempIndex != _realAccTempIndex)
                                _localBuffer.TempIndexes.Add(_realGyroTempIndex);
                        }

                        AddNewSensor(serial);

                        this.ActiveSensors.Add(serial, _localBuffer);

                        _index++;
                    } _index++;
                }
                #endregion
                #region -------------------------------- Acc > Gyro --------------------------------
                else if (Acc.Count > Gyro.Count)
                {
                    _index = 0;
                    int maxCyclus = Gyro.Count;
                    while (_index < maxCyclus)
                    {
                        ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();

                        string serial = _accnames[0];
                        _accnames.Remove(serial);

                        _localBuffer.Acc = new ActiveSensorStruct()
                        {
                            SensInfo = Acc[_index].SensInfo,
                            StartData = Acc[_index].StartData,
                            StopData = Acc[_index].StopData
                        };

                        int _realAccTempIndex = 0;
                        if (_tempIndexes.TryGetValue(_localBuffer.Acc.SensInfo.SensorName, out _realAccTempIndex))
                        {
                            if (_localBuffer.TempIndexes == null)
                                _localBuffer.TempIndexes = new List<int>();
                            _localBuffer.TempIndexes.Add(_realAccTempIndex);
                        }

                        int _gyrindex = 0;
                        if (!_gyronames.Contains(serial))
                        {
                            string _gyronameBuf = _gyronames[0];
                            serial = string.Format("{0}+{1}", serial, _gyronameBuf);
                            _gyronames.Remove(_gyronameBuf);

                            foreach (ActiveSensorStruct strc in Gyro)
                            {
                                if (strc.SensInfo.SensorName == _gyronameBuf)
                                    break;
                                else
                                    _gyrindex++;
                            }


                        }
                        else
                        {
                            _gyronames.Remove(serial);
                            foreach (ActiveSensorStruct strc in Gyro)
                            {
                                if (strc.SensInfo.SensorName == serial)
                                    break;
                                else
                                    _gyrindex++;
                            }
                        }

                        _localBuffer.Gyro = new ActiveSensorStruct()
                        {
                            SensInfo = Gyro[_gyrindex].SensInfo,
                            StartData = Gyro[_gyrindex].StartData,
                            StopData = Gyro[_gyrindex].StopData
                        };

                        int _realGyroTempIndex = 0;
                        if (_tempIndexes.TryGetValue(_localBuffer.Gyro.SensInfo.SensorName, out _realGyroTempIndex))
                        {
                            if (_localBuffer.TempIndexes == null)
                                _localBuffer.TempIndexes = new List<int>();
                            if (_realGyroTempIndex != _realAccTempIndex)
                                _localBuffer.TempIndexes.Add(_realGyroTempIndex);
                        }

                        AddNewSensor(serial);

                        this.ActiveSensors.Add(serial, _localBuffer);

                        _index++;
                    }
                    maxCyclus = Acc.Count;
                    while (_index < maxCyclus)
                    {
                        string serial = _accnames[0];
                        _accnames.Remove(serial);
                        AddNewSensor(serial);

                        ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();
                        _localBuffer.Acc = new ActiveSensorStruct()
                        {
                            SensInfo = Acc[_index].SensInfo,
                            StartData = Acc[_index].StartData,
                            StopData = Acc[_index].StopData
                        };
                        _localBuffer.Gyro = new ActiveSensorStruct()
                        {
                            SensInfo = new ISTPConfiguration.Sensor_t(),
                            StartData = -1,
                            StopData = -1
                        };
                        int _realAccTempIndex = 0;
                        if (_tempIndexes.TryGetValue(_localBuffer.Acc.SensInfo.SensorName, out _realAccTempIndex))
                        {
                            if (_localBuffer.TempIndexes == null)
                                _localBuffer.TempIndexes = new List<int>();
                            _localBuffer.TempIndexes.Add(_realAccTempIndex);
                        }
                        this.ActiveSensors.Add(serial, _localBuffer);

                        _index++;
                    }
                }
                #endregion
                #region -------------------------------- Acc < Gyro --------------------------------
                else
                {
                    _index = 0;
                    int maxCyclus = Acc.Count;
                    while (_index < maxCyclus)
                    {
                        ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();

                        string serial = _accnames[0];
                        _accnames.Remove(serial);

                        _localBuffer.Acc = new ActiveSensorStruct()
                        {
                            SensInfo = Acc[_index].SensInfo,
                            StartData = Acc[_index].StartData,
                            StopData = Acc[_index].StopData
                        };

                        int _realAccTempIndex = 0;
                        if (_tempIndexes.TryGetValue(_localBuffer.Acc.SensInfo.SensorName, out _realAccTempIndex))
                        {
                            if (_localBuffer.TempIndexes == null)
                                _localBuffer.TempIndexes = new List<int>();
                            _localBuffer.TempIndexes.Add(_realAccTempIndex);
                        }

                        int _gyrindex = 0;
                        if (!_gyronames.Contains(serial))
                        {
                            string _gyronameBuf = _gyronames[0];
                            serial = string.Format("{0}+{1}", serial, _gyronameBuf);
                            _gyronames.Remove(_gyronameBuf);

                            foreach (ActiveSensorStruct strc in Gyro)
                            {
                                if (strc.SensInfo.SensorName == _gyronameBuf)
                                    break;
                                else
                                    _gyrindex++;
                            }


                        }
                        else
                        {
                            _gyronames.Remove(serial);
                            foreach (ActiveSensorStruct strc in Gyro)
                            {
                                if (strc.SensInfo.SensorName == serial)
                                    break;
                                else
                                    _gyrindex++;
                            }
                        }

                        _localBuffer.Gyro = new ActiveSensorStruct()
                        {
                            SensInfo = Gyro[_gyrindex].SensInfo,
                            StartData = Gyro[_gyrindex].StartData,
                            StopData = Gyro[_gyrindex].StopData
                        };

                        int _realGyroTempIndex = 0;
                        if (_tempIndexes.TryGetValue(_localBuffer.Gyro.SensInfo.SensorName, out _realGyroTempIndex))
                        {
                            if (_localBuffer.TempIndexes == null)
                                _localBuffer.TempIndexes = new List<int>();
                            if (_realGyroTempIndex != _realAccTempIndex)
                                _localBuffer.TempIndexes.Add(_realGyroTempIndex);
                        }

                        AddNewSensor(serial);

                        this.ActiveSensors.Add(serial, _localBuffer);

                        _index++;
                    } //while (_index < Acc.Count)
                    maxCyclus = Gyro.Count;
                    while (_index < maxCyclus)
                    {
                        string serial = _gyronames[0];
                        _gyronames.Remove(serial);
                        AddNewSensor(serial);

                        ActiveAccGyroStruct _localBuffer = new ActiveAccGyroStruct();
                        _localBuffer.Gyro = new ActiveSensorStruct()
                        {
                            SensInfo = Gyro[_index].SensInfo,
                            StartData = Gyro[_index].StartData,
                            StopData = Gyro[_index].StopData
                        };
                        _localBuffer.Acc = new ActiveSensorStruct()
                        {
                            SensInfo = new ISTPConfiguration.Sensor_t(),
                            StartData = -1,
                            StopData = -1
                        };
                        int _realGyroTempIndex = 0;
                        if (_tempIndexes.TryGetValue(_localBuffer.Gyro.SensInfo.SensorName, out _realGyroTempIndex))
                        {
                            if (_localBuffer.TempIndexes == null)
                                _localBuffer.TempIndexes = new List<int>();
                            _localBuffer.TempIndexes.Add(_realGyroTempIndex);
                        }
                        this.ActiveSensors.Add(serial, _localBuffer);

                        _index++;
                    }
                }
                #endregion
            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Critical error during joining acc and gyro", ex.ToString());
            }
        }

        /// <summary>
        /// Repleace temperature index due to SPStempList.csv
        /// </summary>
        /// <param name="aConfig">Current configuration</param>
        private void ChangeTempIndex(ISTPConfiguration aConfig)
        {
            if (SPSscripts.TempList.Count > 0)
            {
                //Clear temp indexes
                for (int i = 0; i < SPSscripts.TempList.Count; i++)
                {
                    string _TempKey = SPSscripts.TempList.Keys.ElementAt(i);
                    string _TempVal;
                    if (SPSscripts.TempList.TryGetValue(_TempKey, out _TempVal))
                    {
                        //Find out if new thermomether is configured
                        int _confIn = 0;
                        int _newTempIn = -1;
                        while (aConfig.Sensors[_confIn].Active)
                        {
                            if (aConfig.Sensors[_confIn].SensorName.Contains(_TempVal))
                            {
                                _newTempIn = aConfig.Sensors[_confIn].QuantityIndexes[0];
                                break;
                            }
                            _confIn++;
                        }
                        if (_newTempIn < 0)
                            continue;

                        for (int y = 0; y < ActiveSensors.Count; y++)
                        {
                            string _key = ActiveSensors.Keys.ElementAt(y);

                            if (_key.Contains(_TempKey))
                            {
                                ActiveAccGyroStruct _val = new ActiveAccGyroStruct();
                                if (ActiveSensors.TryGetValue(_key, out _val))
                                {
                                    _val.TempIndexes.Clear();
                                }
                            }
                        }
                    }
                }
                //Add new indexes
                for (int i = 0; i < SPSscripts.TempList.Count; i++)
                {
                    string _TempKey = SPSscripts.TempList.Keys.ElementAt(i);
                    string _TempVal;
                    if (SPSscripts.TempList.TryGetValue(_TempKey, out _TempVal))
                    {
                        //Find out if new thermomether is configured
                        int _confIn = 0;
                        int _newTempIn = -1;
                        while (aConfig.Sensors[_confIn].Active)
                        {
                            if (aConfig.Sensors[_confIn].SensorName.Contains(_TempVal))
                            {
                                _newTempIn = aConfig.Sensors[_confIn].QuantityIndexes[0];
                                break;
                            }
                            _confIn++;
                        }
                        if (_newTempIn < 0)
                            continue;

                        for (int y = 0; y < ActiveSensors.Count; y++)
                        {
                            string _key = ActiveSensors.Keys.ElementAt(y);

                            if (_key.Contains(_TempKey))
                            {
                                ActiveAccGyroStruct _val = new ActiveAccGyroStruct();
                                if (ActiveSensors.TryGetValue(_key, out _val))
                                {
                                    _val.TempIndexes.Add(_newTempIn);
                                }
                            }
                        }
                    } //if (SPSscripts.TempList.TryGetValue(_TempKey, out _TempVal))
                } //for (int i = 0; i < SPSscripts.TempList.Count; i++)
            } //if (SPSscripts.TempList.Count > 0)
        }
        #endregion

        #region Public sector
        public void ResetCycleNumber()
        {
            CycleNumber = 0;
        }

        /// <summary>
        /// Reinitialize this class
        /// </summary>
        /// <param name="embConf">New embedded configuration</param>
        /// <returns></returns>
        public Dictionary<string, ActiveAccGyroStruct> Reconfigure(ISTPConfiguration embConf)
        {
            sensorNumber = 1;
            sensors = new List<string>();
            sensorsSerial = new List<string>();
            JoinAccAndGyro(embConf);
            ChangeTempIndex(embConf);
            EmbConfiguration = embConf;

            return this.ActiveSensors;
        }

        /// <summary>
        /// Runs threads for SPS iMode. Initialization is include.
        /// </summary>
        public void LogStart(float aTempNominal, bool isCompenActive)
        {
            Timestamp_TestStart = DateTime.UtcNow;
            isScriptsRun = false;
            TempNominal = aTempNominal;

            CycleNumber++;
            //Create Cxxx
            AddT3testFolder();
            //Create Avg,Raw,Res,Sim ... in Cxxx
            CreateT3folderInLastCxx();
            //Create all binary files;

            CreateNewRawLog();
            CreateNewAvgLog();
            if (isCompenActive)
                CreateNewUncompensatedLog();
            OpenBias();
            SaveSensorList();

            for (int y = 0; y < ActiveSensors.Count; y++)
            {
                string _key = ActiveSensors.Keys.ElementAt(y);
                SPSLogger.ActiveAccGyroStruct _val = new SPSLogger.ActiveAccGyroStruct();

                if (ActiveSensors.TryGetValue(_key, out _val))
                {
                    _val.BiasTime = new BiasTime_t()
                    {
                        BiasData = new float[4][],
                        BiasTemp = new float[4],
                        BiasTime = new float[4],
                        isInitCompl = false,
                        isOneHourCompl = false,
                        isTwoHourCompl = false,
                        OneHourTime = 3600,
                        TwoHourTime = 7200
                    };
                    _val.tempStabTimes.Finish = false;
                    _val.tempStabTimes.FirstPacketTimestamp = 0;
                }

                ActiveSensors[_key] = _val;
            }
            IsStabTimeLog = new bool[ActiveSensors.Count];
        }

        /// <summary>
        /// Saves data in Avg and Raw files. Checks time conditions.
        /// </summary>
        /// <param name="packet">Data packet</param>
        /// <param name="isCompenActive">State of Compensation</param>
        public void LogData(EmbeddedPacket packet, bool isCompenActive)
        {
            try
            {
                //Checked time 
                for (int i = 0; i < ActiveSensors.Count; i++)
                {
                    string _key = ActiveSensors.Keys.ElementAt(i);

                    if (!ActiveSensors[_key].tempStabTimes.Finish)
                    {
                        if (!IsStabTimeLog[i] && ActiveSensors[_key].tempStabTimes.isTimeActive && packet.MsgTimeStamp >= ActiveSensors[_key].tempStabTimes.Time)
                        {
                            SPSLogger.ActiveAccGyroStruct _val = new SPSLogger.ActiveAccGyroStruct();
                            if (ActiveSensors.TryGetValue(_key, out _val))
                            {
                                _val.tempStabTimes.Stopwatch = packet.MsgTimeStamp;
                                _val.tempStabTimes.Finish = true;
                                IsStabTimeLog[i] = true;
                            }
                            ActiveSensors[_key] = _val;
                        }
                    }
                }

                if (isCompenActive)
                {
                    LogUncompensatedData(packet);

                    packet = CompensatoryMode.Recalculate(packet);
                    LogAvgData(packet);
                    LogRawData(packet);
                }
                else
                {
                    LogAvgData(packet);
                    LogRawData(packet);
                }


            }
            catch (Exception ex)
            {
                DataProcessingError.Add("SPSLogger.cs", "Exception in log data controller", ex.ToString());
            }
        }

        /// <summary>
        /// Terminates Avg and raw files. Creates TempStabTime file. Create Bias file. Starts external scripts.
        /// </summary>
        /// <param name="skipProcessing">True = external scripts will not start</param>
        public void Terminate(bool skipProcessing = false)
        {
            if (!isScriptsRun)
            {
                TerminateAvg();
                TerminateRaw();
                TempStabTimeWriter();
                TerminateUncompensated();
                WriteBiasProvider();
                this.CloseBias();
                if (!skipProcessing)
                {
                    isScriptsRun = true;
                    SPSscript.Run(RootFolder, LastPath2Cycle, ActiveSet.scripts);
                    if (SPSscript.canRunLast(ActiveSet.scripts, CycleNumber))
                    {
                        SaveSensorListGlobal();
                        SPSscript.RunLast(RootFolder, LastPath2Cycle, ActiveSet.scripts, CycleNumber);
                        CycleNumber = 0;
                    }
                }
            }
        }

        public void UpdateSPSconfig(Dictionary<string, ActiveAccGyroStruct> modConf)
        {
            this.ActiveSensors = modConf;
        }
        #endregion

        public string CyclePath { get { return (string.Format("{0}\\{1}", RootFolder, LastPath2Cycle)); } }
        public string CyclePathActual { get { return ActualCyclePath; } }
    }
}
