using System;
using System.Collections;
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.Threading;
using System.IO;
using System.Diagnostics;
using System.Collections.Concurrent;


namespace TestUartForm
{
    public partial class Form1 : Form
    {
         enum State
        {
            CALIBRATION_ACC = 0,
            CALIBRATION_MAG,
            READ_DATA
        };

        // This delegate enables asynchronous calls for setting
        // the text property on a TextBox control.
        delegate void SetTextCallback(string text);

        //Background Worker
        private BackgroundWorker ReceiveProcess = new BackgroundWorker();
        Semaphore ReceiveEndSync;
        public bool ReceiveInfinite = false;
        private int state = (int)State.CALIBRATION_ACC;

        //private const byte [] header = new const byte[2]{0x55, 0xAA};
        private Boolean receiving;
        private SerialPort comport = new SerialPort();
        private Int32 totalLength = 0;
        private Thread t, t1, d;
        private int cnt = 1;

        private const Byte head = 0x55;

        private DateTime time_start, time_end;
        delegate void Display(Byte[] buffer);

        private int indexofmatch = 0;
        private int Size = 100000;
        private int ContentLength = 0;
        private Byte[] circular;
        private int index = 0;
        private int index_get = 0;
        private Byte[] tempArray;

        private int now, past_time, delt = 0;

        private IMU sensor = new IMU();

        //Store raw binary data
        private byte[] AccRaw = new byte[6];
        private byte[] GyroRaw = new byte[6];
        private byte[] MagRaw = new byte[6];
        private byte[] TimeRaw = new byte[5];

        private float[] acc = new float[3];
        private float[] gyro = new float[3];
        private float[] mag = new float[3];
        private float[] AccOffset = new float[3];
        private float[] GyroOffset = new float[3];
        private float[] MagOffset = new float[3];

        private float[] euler = new float[3];
        private float[] q = new float[4];
        private double AEq_1 = 1, AEq_2 = 0, AEq_3 = 0, AEq_4 = 0;
        private int time = 0;

        private static Mutex mut = new Mutex();
        private object locker = new Object(); // 用於獨佔鎖定的物件
        private byte[] test = new byte[26] { 0x55, 0xAA, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04,
                                             0x01, 0x28, 0x02, 0x78, 0x1F, 0x18,
                                             0xFF, 0xDF, 0x00, 0x10, 0x00, 0x02,
                                             0xFB, 0x00, 0x1F, 0x01, 0x63, 0x00};

        private byte[] checkCommand = new byte[11] { 0x55, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA };

        private ConcurrentQueue<byte> myQueue = new ConcurrentQueue<byte>();

        private int processed = 0;

        public Form1()
        {
            InitializeComponent();

            ReceiveEndSync = new Semaphore(0, 1);
            //Backgroundworker
            ReceiveProcess.WorkerReportsProgress = true;
            ReceiveProcess.WorkerSupportsCancellation = true;
            ReceiveProcess.DoWork += new DoWorkEventHandler(Parse);
            ReceiveProcess.ProgressChanged += new ProgressChangedEventHandler(bw_ReceiveProcessProgressChanged);
            ReceiveProcess.RunWorkerCompleted += new RunWorkerCompletedEventHandler(bw_ReceiveProcessRunWorkerCompleted);

        }

        private void Form1_Load(object sender, EventArgs e)
        {
            comport.PortName = "COM42";
            comport.BaudRate = 921600;
            comport.DataBits = 8;
            comport.StopBits = StopBits.One;

            comport.ReadBufferSize = 4096 * 100;

            comport.Close();

            try
            {
                comport.Open();
            }
            catch (Exception exp)
            {
                Debug.Print(exp.ToString());
            }
            while (!comport.IsOpen) { }
            comport.DiscardInBuffer();
            byte[] wdata = { 0x55, 0x01, 0x01, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0xAA };

            comport.Write(wdata, 0, 11);
            int reslen = 0;
            while (reslen < 7)
            {
                reslen = comport.BytesToRead;
            }
            byte[] resbuf = new byte[7];
            comport.Read(resbuf, 0, reslen);

            receiving = true;
            t = new Thread(DoReceive);
            t.IsBackground = true;

            ReceiveProcess.RunWorkerAsync();
            t.Start();
        }

        private void DoReceive()
        {
            time_start = DateTime.Now;
            while (receiving)
            {
                int length = comport.BytesToRead;
                byte[] buf = new byte[length];
                byte[] test = new byte[length];

                if (length > 0)
                {
                    //ContentLength += length;

                    Array.Resize(ref buf, length);
                    comport.Read(buf, 0, length);
                    //fs_test.Write(buf, 0, length);
                    //lock (myQueue)
                    //{
                        for (int j = 0; j < length; j++ )
                           myQueue.Enqueue(buf[j]);
                    //}
                        Thread.Sleep(1);
                }
            }
        }

        private void bw_ReceiveProcessRunWorkerCompleted(object sender, RunWorkerCompletedEventArgs e)
        {

        }

        private void bw_ReceiveProcessProgressChanged(object sender, ProgressChangedEventArgs e)
        {

            //DateTime dt = DateTime.Now;
            double ESq_1, ESq_2, ESq_3, ESq_4;                              // quaternion describing orientation of sensor relative to earth
            double ASq_1, ASq_2, ASq_3, ASq_4;                              // quaternion describing orientation of sensor relative to auxiliary frame
            double r_11, r_12, r_13, r_21, r_22, r_23, r_31, r_32, r_33;    // rotation matrix elements
            double phi, theta, psi;                                         // Euler angles
            Graphics graphics = this.CreateGraphics();                      // graphics object for visualisation
            Pen penB; ;                                                     // blue pen for x-axis
            Pen penR;                                                       // blue pen for x-axis
            Pen penG;                                                       // blue pen for x-axis
            Font font = new Font("Arial", 12);                              // font object for graphics text
            int origin_x = 620;                                             // x origin of axis in form
            int origin_y = 300;                                             // y origin of axis in form
            int axisLength = (int)(0.6 * origin_y);                         // length of axis displayed in graphics
            int axisWidth = (int)(0.075 * axisLength);                      // width of the axis displayed in graphics

            // compute the quaternion conjugate
            ESq_1 = q[0];
            ESq_2 = -q[1];
            ESq_3 = -q[2];
            ESq_4 = -q[3];

            // compute the quaternion product
            ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
            ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
            ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
            ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;

            // compute the Euler anles from the quaternion
            //phi = Math.Atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
            //theta = -Math.Asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
            //psi = Math.Atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);

            //sw.WriteLine(phi + "," + theta + "," + psi);

            // compute rotation matrix from quaternion
            r_11 = 2 * ASq_1 * ASq_1 - 1 + 2 * ASq_2 * ASq_2;
            r_12 = 2 * (ASq_2 * ASq_3 + ASq_1 * ASq_4);
            r_13 = 2 * (ASq_2 * ASq_4 - ASq_1 * ASq_3);
            r_21 = 2 * (ASq_2 * ASq_3 - ASq_1 * ASq_4);
            r_22 = 2 * ASq_1 * ASq_1 - 1 + 2 * ASq_3 * ASq_3;
            r_23 = 2 * (ASq_3 * ASq_4 + ASq_1 * ASq_2);
            r_31 = 2 * (ASq_2 * ASq_4 + ASq_1 * ASq_3);
            r_32 = 2 * (ASq_3 * ASq_4 - ASq_1 * ASq_2);
            r_33 = 2 * ASq_1 * ASq_1 - 1 + 2 * ASq_4 * ASq_4;

            // draw sensor frame axes to graphics object in correct order
            penB = new Pen(Color.Blue, axisWidth);
            penR = new Pen(Color.Red, axisWidth);
            penG = new Pen(Color.Green, axisWidth);
            graphics.Clear(SystemColors.Control);
            if ((r_21 > r_22) && (r_21 > r_23))
                graphics.DrawLine(penB, origin_x, origin_y, (int)(origin_x + (axisLength * r_11)), (int)(origin_y + (axisLength * -r_31)));
            else if ((r_22 > r_21) && (r_22 > r_23))
                graphics.DrawLine(penR, origin_x, origin_y, (int)(origin_x + (axisLength * r_12)), (int)(origin_y + (axisLength * -r_32)));
            else if ((r_23 > r_21) && (r_23 > r_22))
                graphics.DrawLine(penG, origin_x, origin_y, (int)(origin_x + (axisLength * r_13)), (int)(origin_y + (axisLength * -r_33)));

            if (((r_21 > r_22) && (r_21 < r_23)) || ((r_21 < r_22) && (r_21 > r_23)))
                graphics.DrawLine(penB, origin_x, origin_y, (int)(origin_x + (axisLength * r_11)), (int)(origin_y + (axisLength * -r_31)));
            else if (((r_22 > r_21) && (r_22 < r_23)) || ((r_22 < r_21) && (r_22 > r_23)))
                graphics.DrawLine(penR, origin_x, origin_y, (int)(origin_x + (axisLength * r_12)), (int)(origin_y + (axisLength * -r_32)));
            else if (((r_23 > r_21) && (r_23 < r_22)) || ((r_23 < r_21) && (r_23 > r_22)))
                graphics.DrawLine(penG, origin_x, origin_y, (int)(origin_x + (axisLength * r_13)), (int)(origin_y + (axisLength * -r_33)));

            if ((r_21 < r_22) && (r_21 < r_23))
                graphics.DrawLine(penB, origin_x, origin_y, (int)(origin_x + (axisLength * r_11)), (int)(origin_y + (axisLength * -r_31)));
            else if ((r_22 < r_21) && (r_22 < r_23))
                graphics.DrawLine(penR, origin_x, origin_y, (int)(origin_x + (axisLength * r_12)), (int)(origin_y + (axisLength * -r_32)));
            else if ((r_23 < r_21) && (r_23 < r_22))
                graphics.DrawLine(penG, origin_x, origin_y, (int)(origin_x + (axisLength * r_13)), (int)(origin_y + (axisLength * -r_33)));

            graphics.DrawString("x", font, Brushes.Black, (int)(origin_x + (axisLength * r_11)), (int)(origin_y + (axisLength * -r_31)));
            graphics.DrawString("y", font, Brushes.Black, (int)(origin_x + (axisLength * r_12)), (int)(origin_y + (axisLength * -r_32)));
            graphics.DrawString("z", font, Brushes.Black, (int)(origin_x + (axisLength * r_13)), (int)(origin_y + (axisLength * -r_33)));

            proc.Text = e.ProgressPercentage.ToString();
            AX.Text = acc[0].ToString();
            AY.Text = acc[1].ToString();
            AZ.Text = acc[2].ToString();
            GX.Text = gyro[0].ToString();
            GY.Text = gyro[1].ToString();
            GZ.Text = gyro[2].ToString();
            MX.Text = mag[0].ToString();
            MY.Text = mag[1].ToString();
            MZ.Text = mag[2].ToString();
            roll.Text = euler[0].ToString();
            pitch.Text = euler[1].ToString();
            yaw.Text = euler[2].ToString();
            AccXOffset.Text = AccOffset[0].ToString();
            AccYOffset.Text = AccOffset[1].ToString();
            AccZOffset.Text = AccOffset[2].ToString();

            GyroXOffset.Text = GyroOffset[0].ToString();
            GyroYOffset.Text = GyroOffset[1].ToString();
            GyroZOffset.Text = GyroOffset[2].ToString();

            MagXOffset.Text = MagOffset[0].ToString();
            MagYOffset.Text = MagOffset[1].ToString();
            MagZOffset.Text = MagOffset[2].ToString();
            graphics.Dispose();
            Thread.Sleep(1);
        }

        private void Parse(object sender, DoWorkEventArgs e)
        {
            BackgroundWorker ReceiveWorker = sender as BackgroundWorker;

            while (receiving)
            {
                //try
                //{
                if (ReceiveWorker.CancellationPending == true)
                {
                    e.Cancel = true;
                    ReceiveEndSync.Release();
                    break;
                }
                else
                {
                    int startindex = 0;

                    tempArray = new Byte[26];
                    byte get;
                    //lock (myQueue)
                    //{
                        if (myQueue.Count > 26)
                        {
                            if ((byte)myQueue.ToArray()[index_get] == 0x55)
                            {
                                index_get++;
                                if ((byte)myQueue.ToArray()[index_get] == 0xAA)
                                {
                                    index_get--;
                                    startindex = (index_get + Size) % Size;
                                    if (index_get > 0)
                                    {
                                        int i = 0;
                                        while ((i < index_get) && (myQueue.TryDequeue(out get)))
                                        {
                                            i++;
                                        }
                                    }
                                    for (int j = 0; j < 26; j++)
                                    {
                                         myQueue.TryDequeue(out tempArray[j]);
                                    }
                                    for (int k = 0; k < 6; k++)
                                    {
                                        AccRaw[k] = tempArray[k + 8];
                                        GyroRaw[k] = tempArray[k + 14];
                                        MagRaw[k] = tempArray[k + 20];
                                    }

                                    for (int index = 0; index < 5; index++)
                                        TimeRaw[index] = tempArray[index + 2];

                                    if (state == (int)State.CALIBRATION_ACC)
                                    {
                                        sensor.SetData(AccRaw, GyroRaw, MagRaw, TimeRaw);
                                        sensor.UpdateAGOffset();
                                        processed++;
                                        if (processed == 1000)
                                        {
                                            AccOffset = sensor.GetAccOffset();
                                            GyroOffset = sensor.GetGyroOffset();
                                            processed = 0;
                                            state = (int)State.CALIBRATION_MAG;
                                        }
                                    }
                                    else if (state == (int)State.CALIBRATION_MAG)
                                    {
                                        sensor.SetData(AccRaw, GyroRaw, MagRaw, TimeRaw);
                                        sensor.UpdateMOffset();
                                        processed++;
                                        if (processed == 1000)
                                        {
                                            MagOffset = sensor.GetMagOffset();
                                            processed = 0;
                                            state = (int)State.READ_DATA;
                                        }

                                    }
                                    else if (state == (int)State.READ_DATA)
                                    {
                                        sensor.SetData(AccRaw, GyroRaw, MagRaw, TimeRaw);
                                        sensor.update();
                                        acc = sensor.GetAcc();
                                        gyro = sensor.GetGyro();
                                        mag = sensor.GetMag();
                                        euler = sensor.GetEuler();
                                        q = sensor.GetQuaternion();

                                        sensor.Save();
                                        processed++;
                                        //Thread.Sleep(10);

                                    }
                                    if (processed % 10 == 0)
                                    {
                                        ReceiveWorker.ReportProgress(processed);
                                    }
                                    index_get = 0;

                                    Thread.Sleep(1);
                                }
                                else
                                {
                                    index_get++;
                                }
                            }
                            else
                            {
                                index_get++;
                            }
                        }
                    //}
                }
             }
        }

        private void Button_ClearTextbox_Click(object sender, EventArgs e)
        {
            TextBox1.Clear();
        }

        private void Button_AbortThread_Click(object sender, EventArgs e)
        {
            Close();
        }

        private void Close()
        {
            receiving = false;

            time_end = DateTime.Now;
            comport.Close();
            sensor.WriteToFile((time_end - time_start).TotalMilliseconds.ToString());
            sensor.Close();
            t.Abort();
            ReceiveProcess.CancelAsync();
            //this.Dispose();
        }

    }
}