// =========================================================================================
// Altino Robot Car Test module 
// =========================================================================================
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.IO;
using static AltinoSerial.ALTINO;
// =========================================================================================
// 2023.07.13 : Initial version
// 2023.07.14 : Read & parse Sensor Value
// =========================================================================================
namespace AltinoSerial
{
    public partial class Form1 : Form
    {
        private SerialPort AltinoSerial;
        private int SerialPortNo = 0;
        byte[] SendBufMain = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
        byte[] SenData56 = Enumerable.Repeat<byte>(0, 56).ToArray<byte>();
        byte[] SenData31A = Enumerable.Repeat<byte>(0, 31).ToArray<byte>();
        byte[] SenData31B = Enumerable.Repeat<byte>(0, 31).ToArray<byte>();
        private ALTINO.AltinoSensor01 atinoSen01 = new ALTINO.AltinoSensor01();
        private ALTINO.AltinoSensor02 atinoSen02 = new ALTINO.AltinoSensor02();
        public static string LogFileName;
        private int SensorInterval = 0;     // Sensor Reading interval (ms)
        private bool fReadSensor = false;
        // -------------------------------------------------------------------------------------
        // main 에서 만든 UI or Control을 다른 Thread가 접근하면 CrossThread Error 발생
        // -------------------------------------------------------------------------------------
        delegate void MyDelegate();        // 델리게이트 선언(크로스 쓰레드 해결)
        // -------------------------------------------------------------------------------------
        public Form1()
        {
            InitializeComponent();
            timerAltino.Tick += timerPortScan_Tick;
            timerAltino.Interval = 10;
            timerAltino.Start();
            LogFileName = "Log" + DateTime.Now.ToString("-yyyyMMdd-HHmmss") + ".txt";
        }
        // -------------------------------------------------------------------------------------
        private void Form1_Load(object sender, EventArgs e)
        {
            AltinoSerial = new SerialPort();
            cBoxSerial.DataSource = SerialPort.GetPortNames();
            string[] serial_list = SerialPort.GetPortNames();
            SerialPortNo = 0;
            foreach (string name in serial_list)
            {
                SerialPortNo++;
            }
        }
        // -------------------------------------------------------------------------------------
        // Relase Data Handler
        // -------------------------------------------------------------------------------------
        private void Form1_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (AltinoSerial.IsOpen)
            {
                AltinoSerial.DataReceived -= new SerialDataReceivedEventHandler(AltinoDataReceived);
                AltinoSerial.Close();
            }
        }
        // -------------------------------------------------------------------------------------
        private void btnPortReload_Click(object sender, EventArgs e)
        {
            cBoxSerial.DataSource = null;
            cBoxSerial.Items.Clear();
            cBoxSerial.DataSource = SerialPort.GetPortNames();
            string[] serial_list = SerialPort.GetPortNames();
            SerialPortNo = 0;
            foreach (string name in serial_list)
            {
                SerialPortNo++;
            }
        }
        // -------------------------------------------------------------------------------------
        private void comboBox1_SelectedIndexChanged(object sender, EventArgs e)
        {

        }
         // -------------------------------------------------------------------------------------
        // Open COM port - prepare to use DataReceived Handler
        // -------------------------------------------------------------------------------------
        private void btnPortOpen_Click(object sender, EventArgs e)
        {
            if (AltinoSerial.IsOpen) // 특정 포트가 이미 열려 있다면 설정이 되지 않기 때문에 우선 닫는다.
            {
                AltinoSerial.Close();
            }
            if (SerialPortNo <= 0)
            {
                MessageBox.Show("열린 COM 포트가 없습니다.");
                return;
            }
            if (!AltinoSerial.IsOpen)  //시리얼포트가 닫혀있을 때만
            {
                AltinoSerial.PortName = cBoxSerial.Text;    // 선택된 combobox 의 이름으로 포트명
                AltinoSerial.BaudRate = 115200;             // 아두이노에서 사용할 전송률
                AltinoSerial.DataBits = 8;
                AltinoSerial.StopBits = StopBits.One;
                AltinoSerial.Parity = Parity.None;
                AltinoSerial.DataReceived += new SerialDataReceivedEventHandler(AltinoDataReceived);
                AltinoSerial.ReceivedBytesThreshold = 31;   // Min 31 & Max 56
                try
                {
                    AltinoSerial.Open();  //시리얼포트 열기
                }
                catch (Exception ex)
                {
                    MessageBox.Show("COM 포트 Open 오류");
                }
            }
            else
            {
                MessageBox.Show("해당포트가 이미 열려 있습니다.");
            }
        }
        // -------------------------------------------------------------------------------------
        // Altino Control
        // -------------------------------------------------------------------------------------
        private void ControlSpeed()
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            int left = Convert.ToInt16(tBoxGoLeft.Text);
            int right = Convert.ToInt16(tBoxGoRight.Text);

            int temp = 32768;
            int left_int, right_int;

            if (left < 0) left_int = temp - left;
            else left_int = left;
            if (right < 0) right_int = temp - right;
            else right_int = right;

            Sendbuf[6] = 0;
            Sendbuf[9] = 0;
            Sendbuf[7] = (byte)(right_int / 256);
            Sendbuf[8] = (byte)(right_int % 256);
            Sendbuf[10] = (byte)(left_int / 256);
            Sendbuf[11] = (byte)(left_int % 256);
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
            SendBufMain = Sendbuf;          // Save & Keep current Speed
        }
        // -------------------------------------------------------------------------------------
        // Enumerable.Repeat<byte>(0, 28).ToArray<byte>() : 28개 byte를 '0'로 채움
        private void btnGo_Click(object sender, EventArgs e)
        {
            ControlSpeed();
        }
        // -------------------------------------------------------------------------------------
        private void btnSpeedInc_Click(object sender, EventArgs e)
        {
            int left = Convert.ToInt16(tBoxGoLeft.Text);
            int right = Convert.ToInt16(tBoxGoRight.Text);
            int SpeedStep = Convert.ToInt16(tBoxSpeedStep.Text);
            tBoxGoLeft.Text = Convert.ToString(left + SpeedStep);
            tBoxGoRight.Text = Convert.ToString(right + SpeedStep);
            ControlSpeed();
        }
        // -------------------------------------------------------------------------------------
        private void btnSpeedDec_Click(object sender, EventArgs e)
        {
            int left = Convert.ToInt16(tBoxGoLeft.Text);
            int right = Convert.ToInt16(tBoxGoRight.Text);
            int SpeedStep = Convert.ToInt16(tBoxSpeedStep.Text);
            tBoxGoLeft.Text = Convert.ToString(left - SpeedStep);
            tBoxGoRight.Text = Convert.ToString(right - SpeedStep);
            ControlSpeed();
        }
        // -------------------------------------------------------------------------------------
        private void btnStop_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            SendBufMain = Sendbuf;          // Save & Keep current Speed
            // Sendbuf[6-11] are already all '0'
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        // Enumerable.Repeat<byte>(0, 28).ToArray<byte>() : 28개 byte를 '0'로 채움
        private void btnSteer_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;              // Keep current Speed
            int Steer = Convert.ToByte(tBoxStreer.Text);
            if (Steer > 127) Steer = 127;
            if (Steer < -127) Steer = -127;

            if (Steer < 0) Steer = 128 - Steer;

            Sendbuf[5] = Convert.ToByte(Steer);
            Sendbuf[24] = 0;        // Steering Mode =0 상대위치 제어
            Sendbuf[25] = (byte)0;  // Mode = 1 --> (Left)128 0 127(Right): 
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        private void btnSteerLeft_Click(object sender, EventArgs e)
        {
            byte SteerMode = Convert.ToByte(tBoxSteerMode.Text);
            SendSteer(SteerMode, 1);
        }
        // -------------------------------------------------------------------------------------
        private void btnSteerCenter_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;              // Keep current Speed
            Sendbuf[5] = 2;                     // Steer Center
            Sendbuf[24] = 0;                    // Steering Mode =1 절대위치 제어
            Sendbuf[25] = (byte)0;
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
            tBoxStreer.Text = Convert.ToString(2);
        }
        // -------------------------------------------------------------------------------------
        private void btnSteerRight_Click(object sender, EventArgs e)
        {
            byte SteerMode = Convert.ToByte(tBoxSteerMode.Text);
            SendSteer(SteerMode, 3);
        }
        // -------------------------------------------------------------------------------------
        // Sendbuf[24]
        // Steering Mode = 0 (3 stage) : Left -> 1 Center -> 2 Right -> 3
        // Steering Mode = 1 (Abs)     : Left -> 128 + (0~127) Right -> 0~127
        // Steering Mode = 2 (Relative): angle incease by time
        // -------------------------------------------------------------------------------------
        private void SendSteer(byte SteerMode, byte Direction)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            int Steer = 0;
            if (SteerMode != 0)
            {
                int SteerStep = Convert.ToByte(tBoxSteerStep.Text);
                Steer = Convert.ToByte(tBoxStreer.Text) + SteerStep;
                if (Steer > 127) Steer = 127;
                if (Steer < -127) Steer = -127;
                if (Steer < 0) Steer = 128 - Steer;
            }
            if (SteerMode == 0) Sendbuf[5] = Direction; // Steer Left
            else Sendbuf[5] = (byte)Steer;
            tBoxStreer.Text = Convert.ToString(Direction);
            Sendbuf[24] = SteerMode;
            Sendbuf[25] = (byte)0;
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        private void btnText_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            byte[] ASC2 = Encoding.ASCII.GetBytes(tBoxDisplay.Text);

            int ASCII = ASC2[0];
            if (ASCII > 128) ASCII = ASCII - 128;
            else if (ASCII > 0 && ASCII < 128) ASCII = 128 + ASCII;
            Sendbuf = SendBufMain;                  // Keep current Speed
            Sendbuf[12] = Convert.ToByte(ASCII);
            // Sendbuf[13-20] are already all '0'
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        private void btnLED_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            Sendbuf[12] = 0;
            Sendbuf[13] = (byte)Convert.ToInt32(tBoxLED0.Text, 16);
            Sendbuf[14] = (byte)Convert.ToInt32(tBoxLED1.Text, 16);
            Sendbuf[15] = (byte)Convert.ToInt32(tBoxLED2.Text, 16);
            Sendbuf[16] = (byte)Convert.ToInt32(tBoxLED3.Text, 16);
            Sendbuf[17] = (byte)Convert.ToInt32(tBoxLED4.Text, 16);
            Sendbuf[18] = (byte)Convert.ToInt32(tBoxLED5.Text, 16);
            Sendbuf[19] = (byte)Convert.ToInt32(tBoxLED6.Text, 16);
            Sendbuf[20] = (byte)Convert.ToInt32(tBoxLED7.Text, 16);
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        private void btnLEDOff_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            // Sendbuf[13-20] are already all '0'
            for (int i = 12; i <= 20; i++) Sendbuf[i] = 0;
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        // Altino Firmware 4.0 looks having BUG : Speed increase if sound on
        // -------------------------------------------------------------------------------------
        private void btnSound_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            Sendbuf[22] = Convert.ToByte(tBoxSound.Text);
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        // Enumerable.Repeat<byte>(0, 28).ToArray<byte>() : 28개 byte를 '0'로 채움
        // Send Command ---> 10, 1, 2
        // option : 0 --> clear command buffer, 1 = keep old command
        // -------------------------------------------------------------------------------------
        public void SendData(byte[] Sendbuf1, byte SendCommand, byte option)
        {
            byte[] tx_d = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            tx_d[0] = STX;
            tx_d[1] = 28;

            tx_d[3] = 1;
            tx_d[4] = SendCommand;
            for (int i = 5; i < 25; i++)
            {
                tx_d[i] = Sendbuf1[i];
            }
            //tx_d[21] = (byte)(Sendbuf1[21] | 0x01);
            tx_d[21] = (byte)(Sendbuf1[21] | 0x01);     // Turn ON IR sensor
            tx_d[26] = 255;
            tx_d[27] = ETX;

            int u16_tx_check_sum = 0;
            int u16_tx_cnt;
            int u16_cnt = 27;
            u16_tx_check_sum = u16_tx_check_sum + tx_d[1];
            for (u16_tx_cnt = 3; u16_tx_cnt <= u16_cnt; u16_tx_cnt++)
            {
                u16_tx_check_sum = u16_tx_check_sum + tx_d[u16_tx_cnt];
            }
            u16_tx_check_sum = u16_tx_check_sum % 256;
            tx_d[2] = (byte)(u16_tx_check_sum);
            AltinoSerial.Write(tx_d, 0, tx_d.Length);

            rBuffString = BitConverter.ToString(tx_d).Replace("-", " ");
            using (StreamWriter file = new StreamWriter(LogFileName, true))
            {
                file.WriteLine("Send: " + rBuffString);
            }
            btnTextSend.Text = "Command: " + rBuffString;
        }
        // -------------------------------------------------------------------------------------
        //private ALTINO.AltinoSensor10 aParseSensor56(byte[] senData56)
        public const byte STX = 0x02;                       // Altino STX
        public const byte ETX = 0x03;                       // Altino ETX
        private static string rTextParse = "";              // all text to be proceed
        private static string rBuffString = "";             // byte2string 시리얼포트 데이터 수신
        private static byte[] rByteAll = new byte[1024];
        private static byte[] rBuffByte31 = new byte[31];
        private static byte[] rBuffByte56 = new byte[56];
        // -------------------------------------------------------------------------------------
        // // Method 1 : Timer based - NOT used this version, see Method 2
        // -------------------------------------------------------------------------------------
        private void timerPortScan_Tick(object sender, EventArgs e)
        {
            if (!AltinoSerial.IsOpen) return;
            SensorInterval += timerAltino.Interval;
            if (SensorInterval >= Convert.ToInt16(tBoxInterval.Text))
            {
                SensorInterval = 0;
                aSendSenorCommand(10);
            }
            //ReadSerialPort();
        }
        // -------------------------------------------------------------------------------------
        // delay from :: www.bluene.net/blog/550 - Thread.Sleep 사용시 COM 포트 값 읽기 문제있음
        // -------------------------------------------------------------------------------------
        private static DateTime Delay(int MS)
        {
            DateTime ThisMoment = DateTime.Now;
            TimeSpan duration = new TimeSpan(0, 0, 0, 0, MS);
            DateTime AfterWards = ThisMoment.Add(duration);

            while (AfterWards >= ThisMoment)
            {
                System.Windows.Forms.Application.DoEvents();
                ThisMoment = DateTime.Now;
            }
            return DateTime.Now;
        }
        // -------------------------------------------------------------------------------------
        // AVR에서 라인별로 송신해도 C#에서 읽은 시리얼 포트 데이터는 붙어있다.
        // 즉 receiveBuffer 안에는 "\r"로 구분된 여러개의 데이터 포함됨
        // https://wowon.tistory.com/232 : C# 시리얼통신 데이터 잘려서 읽히는 경우
        // -------------------------------------------------------------------------------------
        private void ReadSerialPort()
        {
            if (AltinoSerial.BytesToRead < 31) return;
            if (AltinoSerial.BaseStream.CanRead == false) return;
            int textLen = AltinoSerial.BytesToRead;
            AltinoSerial.BaseStream.Read(rByteAll, 0, textLen);
            for (int i = 0; i < 56; i++) rBuffByte56[i] = rByteAll[i];
            rBuffString = BitConverter.ToString(rBuffByte56).Replace("-", " ");

            string mBlock;
            int endOfLineIndex;
            do
            {   // if No return '-1' from IndexOf
                endOfLineIndex = rTextParse.IndexOf("\n");
                if (endOfLineIndex == 0)
                {
                    rTextParse = rTextParse.Remove(0, 1);     // remove the "\r" at the head
                }
                else if (endOfLineIndex > 0)
                {
                    mBlock = rTextParse.Substring(0, endOfLineIndex);
                    rTextParse = rTextParse.Remove(0, endOfLineIndex);
                }
            } while (endOfLineIndex >= 0);
        }
        // -------------------------------------------------------------------------------------
        // Method 2 : This version is using DataReceived
        // -------------------------------------------------------------------------------------
        private void AltinoDataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            if (AltinoSerial.BytesToRead < 56) return;
            if (AltinoSerial.BaseStream.CanRead == false) return;

            int textLen = AltinoSerial.BytesToRead;
            AltinoSerial.BaseStream.Read(rByteAll, 0, textLen);
            for (int i = 0; i < 56; i++)    rBuffByte56[i] = rByteAll[i];
            rBuffString = BitConverter.ToString(rBuffByte56).Replace("-", " ");
            using (StreamWriter file = new StreamWriter(LogFileName, true))
            {
                file.WriteLine("Recv: " + rBuffString);
            }
            if (this.btnTextRecv.InvokeRequired == true)
            {
                this.btnTextRecv.Invoke((MethodInvoker)delegate
                {
                    this.btnTextRecv.Text = "Altino: " + rBuffString.ToString();
                });
            }
            else 
            {
                btnTextRecv.Text = rBuffString;
            }            
            if (rBuffByte56[0] != STX || rBuffByte56[55] != ETX || rBuffByte56[1] != 56) return;
            if (ALTINO.CheckSum56(rBuffByte56) != rBuffByte56[2]) return; // Check sum error
            ALTINO.AltinoSensor10 _atinoSen10 = new ALTINO.AltinoSensor10();
            _atinoSen10 = ALTINO.aParseSensor56(rBuffByte56);
            UpdateAltinoSensor(_atinoSen10);
        }
        // -------------------------------------------------------------------------------------
        private void UpdateAltinoSensor(ALTINO.AltinoSensor10 _atinoSen10)
        {
            MyDelegate delgateUI = delegate ()                  // Delegation function begine
            {
                btnTextIR1.Text = _atinoSen10.IR[0].ToString();
                btnTextIR2.Text = _atinoSen10.IR[1].ToString();
                btnTextIR3.Text = _atinoSen10.IR[2].ToString();
                btnTextIR4.Text = _atinoSen10.IR[3].ToString();
                btnTextIR5.Text = _atinoSen10.IR[4].ToString();
                btnTextIR6.Text = _atinoSen10.IR[5].ToString();
                btnTextGyroX.Text = _atinoSen10.Gyro[0].ToString();
                btnTextGyroY.Text = _atinoSen10.Gyro[1].ToString();
                btnTextGyroZ.Text = _atinoSen10.Gyro[2].ToString();
                btnTextAccGX.Text = _atinoSen10.AccG[0].ToString();
                btnTextAccGY.Text = _atinoSen10.AccG[1].ToString();
                btnTextAccGZ.Text = _atinoSen10.AccG[2].ToString();
                btnTextMagX.Text = _atinoSen10.Mag[0].ToString();
                btnTextMagY.Text = _atinoSen10.Mag[1].ToString();
                btnTextMagZ.Text = _atinoSen10.Mag[2].ToString();
                btnTextCDS.Text = _atinoSen10.CDS.ToString();
                btnTextBatt.Text = _atinoSen10.Batt.ToString();
                btnTextSteerTQ.Text = _atinoSen10.SteerMoTQ.ToString();
                btnTextSteerVal.Text = _atinoSen10.SteerVal.ToString();
                btnTextMotoTQR.Text = _atinoSen10.MoTQR.ToString();
                btnTextMotoTQL.Text = _atinoSen10.MoTQL.ToString();
                btnTextTemp.Text = _atinoSen10.Temp.ToString();
                btnTextMTemp.Text = _atinoSen10.MTemp.ToString();   
            };                                                  // Delegation function end
            this.Invoke(delgateUI);
        }
        // -------------------------------------------------------------------------------------
        // Send read Sensor command, then Atino responds sensor data
        // However Altino responds sensor data to any command
        // -------------------------------------------------------------------------------------
        private void aSendSenorCommand(int command)
        {
            if (fReadSensor != true) return;
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            SendData(Sendbuf, 10, 0);
        }
        private void btnReadSensor_Click(object sender, EventArgs e)
        {
            fReadSensor = !fReadSensor;
            if (fReadSensor) btnReadSensor.Text = "Stop Reading";
            else btnReadSensor.Text = "Read Sensor";
            aSendSenorCommand(10);
        }

        private void btnPortClose_Click(object sender, EventArgs e)
        {
            if (AltinoSerial.IsOpen)
            {
                AltinoSerial.DataReceived -= new SerialDataReceivedEventHandler(AltinoDataReceived);
                AltinoSerial.Close();
            }
        }
        // -------------------------------------------------------------------------------------
    }
    // =========================================================================================
}
