/***********************************************************************/
/**
* @file		    Form1.cs
* @brief		Class for GUI
* @version		1.0
* @date		    21. April. 2014
* @author		Michal Semrad
**************************************************************************/

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO.Ports;
using System.Runtime.InteropServices;





namespace GUIapp
{
    public partial class Form1 : Form
    {
        int x;
        byte[] bytes;
        int[] angles;
        int[] xyzabg;

        byte flag = 0;
        string message;
        SerialCOM sc;

        int operatingMode = 1;
        bool anglesFlag = false;
        bool xyzabgFlag = false;
        bool LoggerFlag = false;

        private enum ID
        {
            X=1,
            Y=2,
            Z=3,
            ALPHA=5,
            BETA=4,
            GRIP=6,
            Q0=7,
            Q1=8,
            Q2=9,
            Q3=10,
            STARTPOS = 11,
            INSTR = 12,
            SPEED=13,
            XYZABG = 21,
            ANGLES = 22,
            FREEHEAP = 30,
            TASKLIST = 31,
            QFREEHEAP = 32,
            QTASKLIST = 33,
            LOGGER = 40,
            RESET = 45,
            PAUSE = 46,
            CONTINUE = 47,
            CYCLIC = 51,
            ONCE = 52,
            SENDAGAIN = 53,
            PWMON = 54,
            PWMOFF = 55
        };


        public Queue<Int16[]> CommandQueue;

        public Form1()
        {
            InitializeComponent();
            timer1.Start();
            sc = new SerialCOM(57600);

            angles = new int[6];
            xyzabg = new int[6];

            CommandQueue = new Queue<Int16[]>();

            Application.ApplicationExit += new EventHandler(this.OnApplicationExit);
        }

        private void OnApplicationExit(object sender, EventArgs e)
        {

            if (buttonTurnOFF.Enabled == false)
                return;

            sc.SendPacket((byte)ID.PWMOFF, null, 0);
            sc.Disconnect();
        }


        private void Form1_Load(object sender, EventArgs e)
        {

            buttonStart.Enabled = false;

            buttonTurnOn.Enabled = true;

            tabControl1.Enabled = false;
            groupBox1.Enabled = false;
            groupBox2.Enabled = false;
            groupBox3.Enabled = false;
            groupBoxuCStats.Enabled = false;
            buttonSartPosition.Enabled = false;
            buttonContinue.Enabled = false;
            buttonPause.Enabled = false;
            buttonReset.Enabled = false;
            buttonTurnOFF.Enabled = false;
            buttonStart.Enabled = false;

        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            if (flag != 0)
            {
                switch (flag)
                {
                    case (byte)ID.X:
                        x = (int)this.numericUpDown1.Value;//x
                        break;
                    case (byte)ID.Y:
                        x = (int)this.numericUpDown2.Value;//y
                        break;
                    case (byte)ID.Z:
                        x = (int)this.numericUpDown3.Value;//z
                        break;
                    case (byte)ID.BETA:
                        x = (int)this.numericUpDown4.Value;//angle beta
                        break;
                    case (byte)ID.ALPHA:
                        x = (int)this.numericUpDown5.Value;//angle alpha
                        break;
                    case (byte)ID.GRIP:
                        x = (int)this.numericUpDown6.Value;//grip
                        break;
                    case (byte)ID.Q0:
                        x = (int)this.numericUpDown7.Value * 0x4000 / 180;  //q0
                        x = (x + 0x6000) % 0x8000;
                        break;
                    case (byte)ID.Q1:
                        x = (int)this.numericUpDown8.Value * 0x4000 / 180;  //q1
                        break;
                    case (byte)ID.Q2:
                        x = (int)this.numericUpDown9.Value * 0x4000 / 180; //q2
                        break;
                    case (byte)ID.Q3:
                        x = (int)this.numericUpDown10.Value * 0x4000 / 180;//q3
                        break;
                    case (byte)ID.SPEED:
                        x = (int)this.numericUpDown13.Value;//speed
                        break;

                    default:
                        break;
                }

                bytes = BitConverter.GetBytes(x);
                sc.SendPacket(flag, bytes, bytes.Length);//flag has role of ID
                flag = 0;

            }
            lock (this)
            {
                if (LoggerFlag)
                {
                    richTextBox2.AppendText(message + "\n");
                    if (message.Contains("Unreach"))
                    {
                        button3.Enabled = false;
                        this.BackColor = System.Drawing.Color.DarkRed;
                    }

                    message = "";
                    LoggerFlag = false;
                }

                if (anglesFlag)
                {
                    this.numericUpDown7.Value = (angles[0] + 90) % 180;
                    this.numericUpDown8.Value = angles[1];
                    this.numericUpDown9.Value = angles[2];
                    this.numericUpDown10.Value = angles[3];
                    button3.Enabled = true;
                    this.BackColor = System.Drawing.Color.WhiteSmoke;
                    anglesFlag = false;
                }
                if (xyzabgFlag)
                {
                    this.numericUpDown1.Value = xyzabg[0];
                    this.numericUpDown2.Value = xyzabg[1];
                    this.numericUpDown3.Value = xyzabg[2];
                    this.numericUpDown4.Value = xyzabg[4];
                    this.numericUpDown5.Value = xyzabg[3];
                    this.numericUpDown6.Value = xyzabg[5];
                    button3.Enabled = true;
                    this.BackColor = System.Drawing.Color.WhiteSmoke;
                    xyzabgFlag = false;
                }
            }
        }

        private void trackBar1_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown1.Value = this.trackBar1.Value;
        }

        private void numericUpDown1_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar1.Value = (int)this.numericUpDown1.Value;
            if (operatingMode == 1)
                flag = (byte)ID.X;
        }

        private void trackBar2_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown2.Value = this.trackBar2.Value;
        }

        private void numericUpDown2_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar2.Value = (int)this.numericUpDown2.Value;
            if (operatingMode == 1)
                flag = (byte)ID.Y;
        }

        private void trackBar3_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown3.Value = this.trackBar3.Value;
        }

        private void numericUpDown3_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar3.Value = (int)this.numericUpDown3.Value;
            if (operatingMode == 1)
                flag = (byte)ID.Z;
        }

        private void trackBar4_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown4.Value = this.trackBar4.Value;
        }

        private void numericUpDown4_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar4.Value = (int)this.numericUpDown4.Value;
            if (operatingMode == 1)
                flag = (byte)ID.BETA;
        }

        private void trackBar5_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown5.Value = this.trackBar5.Value;
        }

        private void numericUpDown5_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar5.Value = (int)this.numericUpDown5.Value;
            flag = (byte)ID.ALPHA;
        }

        private void trackBar6_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown6.Value = this.trackBar6.Value;
        }

        private void numericUpDown6_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar6.Value = (int)this.numericUpDown6.Value;
            flag = (byte)ID.GRIP;
        }

        private void trackBar7_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown7.Value = this.trackBar7.Value;
        }

        private void numericUpDown7_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar7.Value = (int)this.numericUpDown7.Value;
            if (operatingMode == 2)
                flag = (byte)ID.Q0;
        }

        private void trackBar8_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown8.Value = this.trackBar8.Value;
        }

        private void numericUpDown8_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar8.Value = (int)this.numericUpDown8.Value;
            if (operatingMode == 2)
                flag = (byte)ID.Q1;
        }

        private void trackBar9_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown9.Value = this.trackBar9.Value;
        }

        private void numericUpDown9_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar9.Value = (int)this.numericUpDown9.Value;
            if (operatingMode == 2)
                flag = (byte)ID.Q2;
        }

        private void trackBar10_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown10.Value = this.trackBar10.Value;
        }

        private void numericUpDown10_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar10.Value = (int)this.numericUpDown10.Value;
            if (operatingMode == 2)
                flag = (byte)ID.Q3;
        }

        private void trackBar13_Scroll(object sender, EventArgs e)
        {
            this.numericUpDown13.Value = this.trackBar13.Value;
        }
        private void numericUpDown13_ValueChanged(object sender, EventArgs e)
        {
            this.trackBar13.Value = (int)this.numericUpDown13.Value;
            flag = (byte)ID.SPEED;
        }





        private void buttonFreeHeap_Click(object sender, EventArgs e)
        {
            sc.SendPacket((byte)ID.QFREEHEAP, null, 0);//free heap query
        }

        private void buttonTaskList_Click(object sender, EventArgs e)
        {
            sc.SendPacket((byte)ID.QTASKLIST, null, 0);//task list query
        }

        private void richTextBox1_TextChanged(object sender, EventArgs e)//auto scroll down when new text appers
        {
            richTextBox1.SelectionStart = richTextBox1.Text.Length;
            richTextBox1.ScrollToCaret();
        }

        private void richTextBox2_TextChanged(object sender, EventArgs e)//auto scroll down when new text appers
        {
            richTextBox2.SelectionStart = richTextBox2.Text.Length;
            richTextBox2.ScrollToCaret();
        }


        private void tabControl1_SelectedIndexChanged(object sender, EventArgs e)//switching  between modes according tab selected
        {
            if (tabControl1.SelectedTab == tabControl1.TabPages[1])
                operatingMode = 2;
            else
                operatingMode = 1;
        }

        private void button3_Click(object sender, EventArgs e)      //add button
        {
            Int16[] instruct = new Int16[8];
            Int16[] previous = new Int16[8];
            instruct[0] = (Int16)this.numericUpDown1.Value;//x
            instruct[1] = (Int16)this.numericUpDown2.Value;//y
            instruct[2] = (Int16)this.numericUpDown3.Value;//z
            instruct[3] = (Int16)this.numericUpDown5.Value;//alpha
            instruct[4] = (Int16)this.numericUpDown4.Value;//beta
            instruct[5] = (Int16)this.numericUpDown6.Value;//grip
            instruct[6] = (Int16)(this.numericUpDown13.Value);//speed
            instruct[7] = (Int16)this.numericUpDelay.Value;//delay

            CommandQueue.Enqueue(instruct);

            //check if the new added command is not the same like the previous one
            int size = CommandQueue.Count();
            int count = 0;
            if (size > 1)
            {
                previous = CommandQueue.ElementAt(size - 2);
                for (int i = 0; i < 8; i++)
                {
                    if (instruct[i] == previous[i])
                        count++;
                }
                if (count == 8)
                {
                    RemoveLast(CommandQueue);
                    return;
                }
            }

            string instrS = string.Join(",", instruct);
            richTextBox1.AppendText(instrS + "\n");

            buttonStart.Enabled = true;
        }


        private void buttonStart_Click(object sender, EventArgs e)//Start button. Send whole command list to uC
        {
            byte[] instructions = new byte[2000];
            int cnt = 0;

            int len = CommandQueue.Count;
            for (int i = 0; i < len; i++)
            {
                Buffer.BlockCopy(CommandQueue.ElementAt(i), 0, instructions, i * 16, 16);
                cnt++;
            }

            tabControl1.Enabled = false;
            groupBox1.Enabled = false;
            groupBox2.Enabled = false;
            groupBox3.Enabled = false;
            buttonContinue.Enabled = false;

            buttonSartPosition.Enabled = true;
            buttonPause.Enabled = true;

            sc.SendPacket((byte)ID.INSTR, instructions, cnt * 16);
            //
        }


        private void button6_Click(object sender, EventArgs e)//save button
        {
            SaveFileDialog dlg = new SaveFileDialog();

            dlg.DefaultExt = ".txt";
            dlg.Filter = "Text Files (.txt)|*.txt|All Files (*.*)|*.*";


            if (dlg.ShowDialog() == DialogResult.OK)
            {
                using (System.IO.StreamWriter writer = new System.IO.StreamWriter(dlg.FileName))
                {
                    for (int i = 0; i < richTextBox1.Lines.Length; i++)
                    {
                        writer.WriteLine(richTextBox1.Lines[i]);
                    }
                    writer.Close();
                }
            }
        }

        private void button7_Click(object sender, EventArgs e)//open file
        {
            OpenFileDialog dlg = new OpenFileDialog();

            dlg.DefaultExt = ".txt";
            dlg.Filter = "Text Files (.txt)|*.txt|All Files (*.*)|*.*";


            if (dlg.ShowDialog() == DialogResult.OK)
            {
                richTextBox1.Clear();
                CommandQueue.Clear();

                using (System.IO.StreamReader reader = new System.IO.StreamReader(dlg.FileName))
                {
                    string temp;
                    temp = reader.ReadLine();
                    while (temp.Length > 1)
                    {
                        richTextBox1.AppendText(temp + "\n");
                        Int16[] instruct = new Int16[8];
                        instruct = tointarray(temp, ',');
                        if (instruct == null || instruct.Length != 8)
                        {
                            reader.Close();
                            richTextBox1.Clear();
                            CommandQueue.Clear();
                            MessageBox.Show("File, which you are trying to open, does not have a suitable format", "Error Message", MessageBoxButtons.OK, MessageBoxIcon.Error);
                            return;

                        }
                        CommandQueue.Enqueue(instruct);
                        temp = reader.ReadLine();
                    }
                    reader.Close();
                    //if opened succesfull
                    buttonStart.Enabled = true;
                }
            }
        }




        private void button8_Click(object sender, EventArgs e)//Clear Command List button
        {
            richTextBox1.Clear();
            CommandQueue.Clear();
            buttonStart.Enabled = false;
        }

        private void button9_Click(object sender, EventArgs e)//clear messages
        {
            richTextBox2.Clear();
        }

        private void buttonReset_Click(object sender, EventArgs e)//reset
        {
            sc.SendPacket((byte)ID.RESET, null, 0);// call method NVIC_SystemReset(); in uC
            Application.Restart();

        }

        private void buttonPause_Click(object sender, EventArgs e)//pause
        {
            sc.SendPacket((byte)ID.PAUSE, null, 0);
            buttonContinue.Enabled = true;
            buttonPause.Enabled = false;

        }

        private void buttonContinue_Click(object sender, EventArgs e)//continue
        {
            sc.SendPacket((byte)ID.CONTINUE, null, 0);
            buttonPause.Enabled = true;
            buttonContinue.Enabled = false;
        }

        private void button13_Click(object sender, EventArgs e)//remove last button
        {
            if (CommandQueue.Count < 2)
                return;                 //dont remove last standing element

            RemoveLast(CommandQueue);

            richTextBox1.Clear();
            int len = CommandQueue.Count;
            for (int i = 0; i < len; i++)
            {
                string instrS = string.Join(",", CommandQueue.ElementAt(i));
                richTextBox1.AppendText(instrS + "\n");
            }
        }

        private void buttonStartPosition_Click(object sender, EventArgs e)
        {
            tabControl1.Enabled = true;
            groupBox1.Enabled = true;
            groupBox2.Enabled = true;
            groupBox3.Enabled = true;

            buttonContinue.Enabled = false;
            buttonPause.Enabled = false;
            sc.SendPacket((byte)ID.STARTPOS, null, 0);
        }

        private void radioButtonCyclic_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButtonCyclic.Checked == true)
                sc.SendPacket((byte)ID.CYCLIC, null, 0);
            else
                sc.SendPacket((byte)ID.ONCE, null, 0);
        }

        private void buttonTurnOn_Click(object sender, EventArgs e)
        {
            if (sc.Connect() != 1)
                return;

            tabControl1.Enabled = true;
            groupBox1.Enabled = true;
            groupBox2.Enabled = true;
            groupBox3.Enabled = true;
            groupBoxuCStats.Enabled = true;
            buttonSartPosition.Enabled = true;
            buttonTurnOFF.Enabled = true;
            buttonReset.Enabled = true;
            buttonTurnOFF.Enabled = true;

            buttonContinue.Enabled = false;
            buttonPause.Enabled = false;
            buttonTurnOn.Enabled = false;


            sc.SendPacket((byte)ID.PWMON, null, 0);
        }

        private void buttonTurnOFF_Click(object sender, EventArgs e)
        {
            sc.SendPacket((byte)ID.PWMOFF, null, 0);
            sc.Disconnect();

            buttonTurnOn.Enabled = true;

            tabControl1.Enabled = false;
            groupBox1.Enabled = false;
            groupBox2.Enabled = false;
            groupBox3.Enabled = false;
            groupBoxuCStats.Enabled = false;
            buttonSartPosition.Enabled = false;
            buttonContinue.Enabled = false;
            buttonPause.Enabled = false;
            buttonReset.Enabled = false;
            buttonTurnOFF.Enabled = false;
        }

        private void numericUpDelay_ValueChanged(object sender, EventArgs e)
        {
            trackBarDelay.Value = (int)numericUpDelay.Value;
        }

        private void trackBarDelay_Scroll(object sender, EventArgs e)
        {
            numericUpDelay.Value = trackBarDelay.Value;
        }

        /*********************************************************************************************
         * 
         * 
         * 
         ******************************************methods ******************************************* 
         *
         * 
         * 
         *********************************************************************************************/


        Int16[] tointarray(string value, char sep)
        {
            string[] sa = value.Split(sep);
            Int16[] ia = new Int16[sa.Length];
            for (Int16 i = 0; i < ia.Length; ++i)
            {
                int j;
                string s = sa[i];
                if (int.TryParse(s, out j))
                {
                    ia[i] = (Int16)j;
                }
                else
                {
                    return null;
                }
            }
            return ia;
        }

        public void SerialProcessPacket(byte[] packet, int received)
        {

            if (packet == null)
            {
                return;
            }

            switch (packet[0])
            {

                case (byte)ID.XYZABG://in joint mode. Receive x,y,z values to update trackbars
                    lock (this)
                    {
                        for (int i = 0; i < 6; i++)
                        {
                            xyzabg[i] = System.BitConverter.ToInt16(packet, i * 2 + 1);
                        }
                        Console.WriteLine(string.Join(",", xyzabg));
                        xyzabgFlag = true;

                        break;
                    }

                case (byte)ID.ANGLES://in x,y,z mode. Receive angles values to update trackbars
                    lock (this)
                    {
                        for (int i = 0; i < 6; i++)
                        {
                            angles[i] = System.BitConverter.ToUInt16(packet, i * 2 + 1) * 180 / 0x4000;
                        }
                        Console.WriteLine(string.Join(",", angles));
                        anglesFlag = true;

                        break;
                    }
                case (byte)ID.LOGGER://messages
                    lock (this)
                    {
                        message += System.Text.ASCIIEncoding.ASCII.GetString(packet, 1, received - 3);//log message
                        message += "\n";
                        Console.WriteLine(message);
                        LoggerFlag = true;
                        break;
                    }

                case (byte)ID.FREEHEAP:
                    lock (this)
                    {
                        int freeHeap = System.BitConverter.ToInt32(packet, 1);//Free heap info
                        message += "FreeHeap is " + freeHeap.ToString();
                        message += "\n";
                        Console.WriteLine("FreeHeap is  {0}", freeHeap);
                        LoggerFlag = true;
                        break;
                    }

                case (byte)ID.TASKLIST:
                    lock (this)
                    {
                        message += "Name\t\tStatus\tPrior\tStack res\tNumber\n";
                        message += System.Text.ASCIIEncoding.ASCII.GetString(packet, 1, received - 3);//TASK LIST
                        message += "\n";
                        Console.WriteLine(message);
                        LoggerFlag = true;
                        break;
                    }

                case (byte)ID.SENDAGAIN:
                    sc.SendPacket(sc.ident_previous, sc.data_previous, sc.length_previous);
                    break;


                default:
                    break;
            }
        }



        object RemoveLast(Queue<Int16[]> q)
        {
            Int16[] first = q.Peek();
            Int16[] current = null;
            while (true)
            {
                current = q.Dequeue();
                if (q.Peek() == first)
                {
                    break;
                }
                q.Enqueue(current);
            }
            return current;
        }

    }
}
