using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.IO;

namespace iSTP_DataProcessing
{
    /// <summary>
    /// Virtual sensors
    /// </summary>
    public class VirtualSensors
    {
        #region --------------------------- Private global values ---------------------------
        static protected string[] ACCquantities = new string[] 
                {
                    "acc1",
                    "acc2",
                    "acc3"
                };
        static protected string[] GYROquantities = new string[] 
                {
                    "rate1",
                    "rate2",
                    "rate3"
                };

        private string SourceFilePath;

        private Calibration.Database_t MyDatabase;

        private float Act_Acc1 = 0;
        private float Act_Acc2 = 0;
        private float Act_Acc3 = 0;

        private int[] AccIndex;

        private float Act_Rate1 = 0;
        private float Act_Rate2 = 0;
        private float Act_Rate3 = 0;

        private int[] GyroIndex;

        #endregion

        #region --------------------------- Public global values ---------------------------
        #endregion

        #region --------------------------- Structures & Enums & Internal classes ---------------------------
        public enum Type_e
        { 
            Acc,
            Gyro
        }

        /// <summary>
        /// Internal calibration class
        /// </summary>
        [Serializable]
        private class Calibration
        {
            List<string> acc1 = new List<string>();
            List<string> acc2 = new List<string>();
            List<string> acc3 = new List<string>();
            List<string> rate1 = new List<string>();
            List<string> rate2 = new List<string>();
            List<string> rate3 = new List<string>();

            public Calibration(string path)
            {
                StreamReader rfile = new StreamReader(path);
                while (!rfile.EndOfStream)
                {
                    string line = rfile.ReadLine();
                    string[] lines = line.Split(',');
                    switch (lines[0])
                    { 
                        case "acc1":
                            for (int i = 1; i < lines.Length; i++)
                                acc1.Add(lines[i]);
                            break;
                        case "acc2":
                            for (int i = 1; i < lines.Length; i++)
                                acc2.Add(lines[i]);
                            break;
                        case "acc3":
                            for (int i = 1; i < lines.Length; i++)
                                acc3.Add(lines[i]);
                            break;
                        case "rate1":
                            for (int i = 1; i < lines.Length; i++)
                                rate1.Add(lines[i]);
                            break;
                        case "rate2":
                            for (int i = 1; i < lines.Length; i++)
                                rate2.Add(lines[i]);
                            break;
                        case "rate3":
                            for (int i = 1; i < lines.Length; i++)
                                rate3.Add(lines[i]);
                            break;
                        default: break;
                    }
                }
            }

            private bool FindSensor(iSTP_Embedded.ISTPConfiguration embconf, string sens ,out string val, out string sensorname, out int index, out bool oposite)
            {
                int i = 0;
                bool _res = false;
                val = string.Empty; 
                sensorname = string.Empty;
                oposite = false;
                index = 0;

                while (embconf[i].Active)
                {
                    sensorname = string.Empty;

                    char[] chars = sens.Split('_')[0].ToCharArray();
                    if (chars[0] == '-')
                    {
                        oposite = true;
                        for (int n = 1; n < chars.Length; n++)
                            sensorname += chars[n];
                    }
                    else
                    {
                        for (int n = 0; n < chars.Length; n++)
                            sensorname += chars[n];
                    }

                    val = sens.Split('_')[sens.Split('_').Length - 1];

                    if (embconf[i].SensorName == sensorname)
                    {
                        _res = true;
                        for(int m = 0; m < embconf[i].Quantities.Length; m++)
                            if (embconf[i].Quantities[m] == val)
                            {
                                index = embconf[i].QuantityIndexes[m];
                                break;
                            }
                        break;
                    }

                    i++;
                }

                return _res;
            }

            public struct SensInfo_t
            {
                public string valueName;
                public int Index;
                public bool MinusReguested;
            }

            public struct Database_t
            {
                public Dictionary<string, SensInfo_t> Acc1;
                public Dictionary<string, SensInfo_t> Acc2;
                public Dictionary<string, SensInfo_t> Acc3;
                public Dictionary<string, SensInfo_t> Rate1;
                public Dictionary<string, SensInfo_t> Rate2;
                public Dictionary<string, SensInfo_t> Rate3;
            }

            private void GetIndexes(iSTP_Embedded.ISTPConfiguration embconf, string[] typov,out Dictionary<string, SensInfo_t> output)
            {
                string quantities;
                string sensorname;
                int index;
                bool oposite;
                output = new Dictionary<string,SensInfo_t>();

                foreach (string s in typov)
                {
                    if (FindSensor(embconf, s, out quantities, out sensorname, out index, out oposite))
                    {
                        output.Add(sensorname,
                            new SensInfo_t()
                            {
                                Index = index,
                                MinusReguested = oposite,
                                valueName = quantities
                            });
                    }
                }
            }

            public Database_t GetDatabase(iSTP_Embedded.ISTPConfiguration embconf)
            {
                Database_t _res = new Database_t();

                GetIndexes(embconf, acc1.ToArray(), out _res.Acc1);
                GetIndexes(embconf, acc2.ToArray(), out _res.Acc2);
                GetIndexes(embconf, acc3.ToArray(), out _res.Acc3);
                GetIndexes(embconf, rate1.ToArray(), out _res.Rate1);
                GetIndexes(embconf, rate2.ToArray(), out _res.Rate2);
                GetIndexes(embconf, rate3.ToArray(), out _res.Rate3);

                return _res;
            }
        } // private class Calibration

        #endregion

        #region --------------------------- Construcotrs&Destructor ---------------------------
        /// <summary>
        /// Creates one virtual sensor according to file
        /// </summary>
        /// <param name="filepath">Name of file</param>
        /// <param name="embconf">Current configuration</param>
        public VirtualSensors(string filepath, ref iSTP_Embedded.ISTPConfiguration embconf)
        {
            AccIndex = new int[3];
            GyroIndex = new int[3];

            SourceFilePath = filepath;

            Calibration calib = new Calibration(SourceFilePath);
            MyDatabase = calib.GetDatabase(embconf);
            calib = null;

            string sensname = filepath.Split(Path.DirectorySeparatorChar)[filepath.Split(Path.DirectorySeparatorChar).Length - 1];
            sensname = sensname.Split('.')[0];

            if (MyDatabase.Acc1.Count > 0 || MyDatabase.Acc2.Count > 0 || MyDatabase.Acc3.Count > 0)
            {
                int lastindex = LastIndex(embconf);
                embconf[embconf.Size] = CreateSensor(lastindex, string.Format("{0}_acc", sensname), ACCquantities);
                AccIndex = new int[] { lastindex + 1, lastindex + 2, lastindex + 3 };
            }

            if (MyDatabase.Rate1.Count > 0 || MyDatabase.Rate2.Count > 0 || MyDatabase.Rate3.Count > 0)
            {
                int lastindex = LastIndex(embconf);
                embconf[embconf.Size] = CreateSensor(lastindex, string.Format("{0}_gyro", sensname), GYROquantities);
                GyroIndex = new int[] { lastindex + 1, lastindex + 2, lastindex + 3 };
            }
        }
        #endregion

        #region --------------------------- Private sector ---------------------------
        /// <summary>
        /// Creates virtual sensor into current configuration database
        /// </summary>
        /// <param name="index"></param>
        /// <param name="sensorname"></param>
        /// <param name="quantities"></param>
        /// <returns></returns>
        private iSTP_Embedded.ISTPConfiguration.Sensor_t CreateSensor(int index, string sensorname, string[] quantities)
        {
            return new iSTP_Embedded.ISTPConfiguration.Sensor_t()
            {
                Active = true,
                BoardIndex = 6,
                Comment = "Virtual Sensor",
                Axis = 0,
                ConfigurationStatus = new iSTP_Embedded.ISTPConfiguration.Board_t()
                {
                    Name = "Virtual",
                    Parameters = new iSTP_Embedded.ISTPConfiguration.Parameter_t[]
                                {
                                    new iSTP_Embedded.ISTPConfiguration.Parameter_t(){Options = new string[0]}
                                }
                },
                SensorName = sensorname,
                SensorNumber = 20,
                QuantityIndexes = new int[] { index + 1, index + 2, index + 3 },
                Quantities = quantities
            };
        }

        private int LastIndex(iSTP_Embedded.ISTPConfiguration embconf)
        {
            return embconf[embconf.Size - 1].QuantityIndexes[embconf[embconf.Size - 1].QuantityIndexes.Length - 1];
        }

        private void CalcAcc(int axis, iSTP_Embedded.EmbeddedPacket datapck, out float avg)
        {
            avg = 0;

            switch (axis)
            { 
                case 0:
                    for (int i = 0; i < MyDatabase.Acc1.Count; i++)
                    {
                        Calibration.SensInfo_t _val = MyDatabase.Acc1.ElementAt(i).Value;

                        if(_val.MinusReguested)
                            avg -= datapck[_val.Index];
                        else
                            avg += datapck[_val.Index];
                    }
                    avg /= (MyDatabase.Acc1.Count == 0) ? 1 : MyDatabase.Acc1.Count;
                    break;
                case 1:
                    for (int i = 0; i < MyDatabase.Acc2.Count; i++)
                    {
                        Calibration.SensInfo_t _val = MyDatabase.Acc2.ElementAt(i).Value;

                        if(_val.MinusReguested)
                            avg -= datapck[_val.Index];
                        else
                            avg += datapck[_val.Index];
                    }
                    avg /= (MyDatabase.Acc2.Count == 0) ? 1 : MyDatabase.Acc2.Count;
                    break;
                case 2:
                    for (int i = 0; i < MyDatabase.Acc3.Count; i++)
                    {
                        Calibration.SensInfo_t _val = MyDatabase.Acc3.ElementAt(i).Value;

                        if(_val.MinusReguested)
                            avg -= datapck[_val.Index];
                        else
                            avg += datapck[_val.Index];
                    }
                    avg /= (MyDatabase.Acc3.Count == 0) ? 1 : MyDatabase.Acc3.Count;
                    break;
            }
        }

        private void CalcGyro(int axis, iSTP_Embedded.EmbeddedPacket datapck, out float avg)
        {
            avg = 0;

            switch (axis)
            {
                case 0:
                    for (int i = 0; i < MyDatabase.Rate1.Count; i++)
                    {
                        Calibration.SensInfo_t _val = MyDatabase.Rate1.ElementAt(i).Value;

                        if(_val.MinusReguested)
                            avg -= datapck[_val.Index];
                        else
                            avg += datapck[_val.Index];
                    }
                    avg /= (MyDatabase.Rate1.Count == 0) ? 1 : MyDatabase.Rate1.Count;
                    break;
                case 1:
                    for (int i = 0; i < MyDatabase.Rate2.Count; i++)
                    {
                        Calibration.SensInfo_t _val = MyDatabase.Rate2.ElementAt(i).Value;

                        if(_val.MinusReguested)
                            avg -= datapck[_val.Index];
                        else
                            avg += datapck[_val.Index];
                    }
                    avg /= (MyDatabase.Rate1.Count == 0) ? 1 : MyDatabase.Rate1.Count;
                    break;
                case 2:
                    for (int i = 0; i < MyDatabase.Rate3.Count; i++)
                    {
                        Calibration.SensInfo_t _val = MyDatabase.Rate3.ElementAt(i).Value;

                        if(_val.MinusReguested)
                            avg -= datapck[_val.Index];
                        else
                            avg += datapck[_val.Index];
                    }
                    avg /= (MyDatabase.Rate1.Count == 0) ? 1 : MyDatabase.Rate1.Count;
                    break;
            }
        }

        private void CalcAcc(ref iSTP_Embedded.EmbeddedPacket datapck)
        {           
            float element;

            if (datapck.MsgNumber > 1000)
            {
                int n = 1;
            }

            if (MyDatabase.Acc1 != null)
            {
                CalcAcc(0, datapck, out element);
                Act_Acc1 = element;
            }
            else
            {
                Act_Acc1 = 0;
            }

            if (MyDatabase.Acc2 != null)
            {
                CalcAcc(1, datapck, out element);
                Act_Acc2 = element; 
            }
            else
            {
                Act_Acc2 = 0;
            }

            if (MyDatabase.Acc3 != null)
            {
                CalcAcc(2, datapck, out element);
                Act_Acc3 = element; 
            }
            else
            {
                Act_Acc3 = 0;
            }
        }

        private void AddNewElement(ref iSTP_Embedded.EmbeddedPacket datapck)
        {
            if (datapck.MsgLength != datapck.MsgData.Length)
                datapck.MsgLength = datapck.MsgData.Length;

            if (AccIndex[2] < (datapck.MsgLength / 4))
            {
                datapck[AccIndex[0]] = Act_Acc1;
                datapck[AccIndex[1]] = Act_Acc2;
                datapck[AccIndex[2]] = Act_Acc3;
            }
            else
            {
                int i = 0;
                int max = 0;
                if (AccIndex[0] >= (datapck.MsgLength / 4))
                {
                    i = datapck.MsgLength / 4;
                    max = 3;

                    datapck.MsgLength += max * sizeof(float);

                    byte[] src = datapck.MsgData;
                    datapck.MsgData = new byte[datapck.MsgLength];
                    Buffer.BlockCopy(src, 0, datapck.MsgData, 0, src.Length);

                    datapck[i++] = Act_Acc1;
                    datapck[i++] = Act_Acc2;
                    datapck[i++] = Act_Acc3;
                }
                else if (AccIndex[1] >= (datapck.MsgLength / 4))
                {
                    datapck[datapck.MsgLength / 4 - 1] = Act_Acc1;
                    max = 2;
                    datapck.MsgLength += max * sizeof(float);
                    i = datapck.MsgLength / 4;

                    byte[] src = datapck.MsgData;
                    datapck.MsgData = new byte[datapck.MsgLength];
                    Buffer.BlockCopy(src, 0, datapck.MsgData, 0, src.Length);

                    datapck[i++] = Act_Acc2;
                    datapck[i++] = Act_Acc3;
                }
                else
                {
                    datapck[datapck.MsgLength / 4 - 2] = Act_Acc1;
                    datapck[datapck.MsgLength / 4 - 1] = Act_Acc2;
                    max = 1;
                    datapck.MsgLength += max * sizeof(float);
                    i = datapck.MsgLength / 4;

                    byte[] src = datapck.MsgData;
                    datapck.MsgData = new byte[datapck.MsgLength];
                    Buffer.BlockCopy(src, 0, datapck.MsgData, 0, src.Length);

                    datapck[i++] = Act_Acc3;
                }       
            }

            if (GyroIndex[2] < (datapck.MsgLength / 4))
            {
                datapck[GyroIndex[0]] = Act_Rate1;
                datapck[GyroIndex[1]] = Act_Rate2;
                datapck[GyroIndex[2]] = Act_Rate3;
            }
            else
            {
                int i = 0;
                int max = 0;
                if (GyroIndex[0] >= (datapck.MsgLength / 4))
                {
                    i = datapck.MsgLength / 4;
                    max = 3;
                    datapck.MsgLength += max * sizeof(float);

                    byte[] src = datapck.MsgData;
                    datapck.MsgData = new byte[datapck.MsgLength];
                    Buffer.BlockCopy(src, 0, datapck.MsgData, 0, src.Length);

                    datapck[i++] = Act_Rate1;
                    datapck[i++] = Act_Rate2;
                    datapck[i++] = Act_Rate3;
                }
                else if (GyroIndex[1] >= (datapck.MsgLength / 4))
                {
                    datapck[datapck.MsgLength / 4 - 1] = Act_Rate1;
                    max = 2;
                    datapck.MsgLength += max * sizeof(float);
                    i = datapck.MsgLength / 4;

                    byte[] src = datapck.MsgData;
                    datapck.MsgData = new byte[datapck.MsgLength];
                    Buffer.BlockCopy(src, 0, datapck.MsgData, 0, src.Length);

                    datapck[i++] = Act_Rate2;
                    datapck[i++] = Act_Rate3;
                }
                else
                {
                    datapck[datapck.MsgLength / 4 - 2] = Act_Rate1;
                    datapck[datapck.MsgLength / 4 - 1] = Act_Rate2;
                    max = 1;
                    datapck.MsgLength += max * sizeof(float);
                    i = datapck.MsgLength / 4;

                    byte[] src = datapck.MsgData;
                    datapck.MsgData = new byte[datapck.MsgLength];
                    Buffer.BlockCopy(src, 0, datapck.MsgData, 0, src.Length);

                    datapck[i++] = Act_Rate3;
                }
            }
        }

        private void CalcGyro(ref iSTP_Embedded.EmbeddedPacket datapck)
        {
            float element;

            if (MyDatabase.Rate1 != null)
            {
                CalcGyro(0, datapck, out element);
                Act_Rate1 = element;
            }
            else
            {
                Act_Rate1 = 0;
            }

            if (MyDatabase.Rate2 != null)
            {
                CalcGyro(1, datapck, out element);
                Act_Rate2 = element;
            }
            else
            {
                Act_Rate2 = 0;
            }

            if (MyDatabase.Rate3!= null)
            {
                CalcGyro(2, datapck, out element);
                Act_Rate3 = element;
            }
            else
            {
                Act_Rate3 = 0;
            }
        }
        #endregion

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

        #region --------------------------- Public sector ---------------------------
        public void CalculateAverage(ref iSTP_Embedded.EmbeddedPacket datapck)
        {
            CalcAcc(ref datapck);
            CalcGyro(ref datapck);
            AddNewElement(ref datapck);
        }
        #endregion
    }

    /// <summary>
    /// Find and parse all *virtual*.csv files
    /// </summary>
    static class VirtualSensorProfile
    {
        public static bool CheckProfile(string path)
        {
            bool _res = false;

            StreamReader rfile = new StreamReader(path);

            List<string> myarr = (from data in new string[] 
                {
                    "acc1",
                    "acc2",
                    "acc3"
                } select data).ToList();
            List<string> myrate = (from data in new string[] 
                {
                    "rate1",
                    "rate2",
                    "rate3"
                } select data).ToList();
            List<string> Lelem = new List<string>();

            while (!rfile.EndOfStream)
            {
                string[] elements = rfile.ReadLine().Split(',');

                Lelem.Add(elements[0]);

                if (myarr.Contains(elements[0]))
                    myarr.Remove(elements[0]);
                else if (myrate.Contains(elements[0]))
                    myrate.Remove(elements[0]);
            }

            if (myarr.Count == 0 && myrate.Count == 0)
                _res = true;

            return _res;
        }

        public static List<string> FindAllVirtualCSV(string path)
        {
            List<string> _res = new List<string>();

            DirectoryInfo hdDirectoryInWhichToSearch = new DirectoryInfo(path);
            if (hdDirectoryInWhichToSearch.Exists)
            {
                FileInfo[] filesInDir = hdDirectoryInWhichToSearch.GetFiles(string.Format("*{0}*.csv", "virtual"));

                foreach (FileInfo foundFile in filesInDir)
                {
                    _res.Add(foundFile.FullName);
                }
            }

            List<string> newvar = new List<string>();
            string newpath = string.Empty;

            if (path.Contains("SW"))
            {
                foreach (string element in path.Split(Path.DirectorySeparatorChar))
                {
                    if (element == "SW")
                        break;
                    else
                        newvar.Add(element);
                }
            }

            if (newvar.Count > 0)
            {
                newvar.Add("TestSettings");
                foreach (string element in newvar)
                    newpath += element + Path.DirectorySeparatorChar;
            }

            if (Directory.Exists(newpath))
            {
                hdDirectoryInWhichToSearch = new DirectoryInfo(newpath);
                if (hdDirectoryInWhichToSearch.Exists)
                {
                    FileInfo[] filesInDir = hdDirectoryInWhichToSearch.GetFiles(string.Format("*{0}*.csv", "virtual"));

                    foreach (FileInfo foundFile in filesInDir)
                    {
                        _res.Add(foundFile.FullName);
                    }
                }

                foreach (string dirs in Directory.GetDirectories(newpath))
                {
                    hdDirectoryInWhichToSearch = new DirectoryInfo(dirs);
                    if (hdDirectoryInWhichToSearch.Exists)
                    {
                        FileInfo[] filesInDir = hdDirectoryInWhichToSearch.GetFiles(string.Format("*{0}*.csv", "virtual"));

                        foreach (FileInfo foundFile in filesInDir)
                        {
                            _res.Add(foundFile.FullName);
                        }
                    }  
                }
            }

            return _res;
        }
    }
}
