using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using System.Threading;

namespace Robotics.Serial
{
    class SerialCom
    {
       public delegate void BotEventHandler(object sender, EventArgs ev);
       public static event BotEventHandler BotEvent;
        
       public string ComPort = "COM1";
       public static SerialPort serialPort1;// = new SerialPort();
       public bool isOpen = false;

       // ---------------------------------------------------------------------------
       /// <summary>
       /// Decode Message from the Roboboard
       /// </summary>
       /// <param name="message"></param>
       /// <returns></returns>
       // ---------------------------------------------------------------------------
       public static string DecodeMessage(string command)
       {
           string message = command.Substring(0, command.Length - 2);
           string target = "";
           target += (char)command[command.Length - 2];
           target += (char)command[command.Length - 1];
           //-----------------------------------------------------------
           // check CRC
           Int16 tmpcrc = 0;
           for (int i = 0; i < message.Length; i++)
           {
               tmpcrc += (Int16)message[i];
           }
           tmpcrc %= 4096;

           string crc = "";
           crc += (char)((byte)'=' + (tmpcrc / 64));
           crc += (char)((byte)'=' + (tmpcrc % 64));

           if (crc != target) return "";
           //-----------------------------------------------------------
           // decode message 
           target = "";
           message = command.Substring(3, command.Length - 5);

           int index = 0;
           uint a, b, c, d;
           uint x, y, z;
           int len = message.Length;
           message = message + "=====";

           while (index <= len)
           {
               a = (uint)message[index++] - '=';
               b = (uint)message[index++] - '=';
               c = (uint)message[index++] - '=';
               d = (uint)message[index++] - '=';

               x = (a << 2) | (b >> 4);
               y = ((b & 0x0f) << 4) | (c >> 2);
               z = ((c & 0x03) << 6) | d;

               target += (char)x;
               target += (char)y;
               target += (char)z;
           }
           return target;
       }

       // ---------------------------------------------------------------------------
       /// <summary>
       /// Encode Message to send to the Board
       /// </summary>
       /// <param name="message"></param>
       /// <returns></returns>
       // ---------------------------------------------------------------------------
       public static string EncodeMessage(string message)
       {
           string target = "";
           byte a, b, c;
           int len = message.Length;
           int ptr = 0;

           while (len > 0)
           {
               if (len > 0) { a = (byte)message[ptr++]; len--; } else a = 0;
               if (len > 0) { b = (byte)message[ptr++]; len--; } else b = 0;
               if (len > 0) { c = (byte)message[ptr++]; len--; } else c = 0;
               target += (char)((a >> 2) + (byte)'=');
               target += (char)((((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + (byte)'=');
               target += (char)((((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + (byte)'=');
               target += (char)((c & 0x3f) + (byte)'=');
           }
           return target;
       }

       // ---------------------------------------------------------------------------
       /// <summary>
       /// Creates a complete message including the command and the address byte 
       /// header. It calls thr EncodeMessage function to encode the message body.
       /// command and adress byte will not be encoded. An additional 2 Byte CRC will
       /// be added at the end of the message
       /// </summary>
       /// <param name="cmd"></param>
       /// <param name="adr"></param>
       /// <param name="message"></param>
       /// <returns></returns>
       // ---------------------------------------------------------------------------
       public string CreateMessage(char cmd, char adr, string message)
       {
           string target = "";
           target += '#';
           target += adr;
           target += cmd;
           target += EncodeMessage(message);

           //-----------------------------------------------------------
           //add CRC
           Int16 tmpcrc = 0;
           for (int i = 0; i < target.Length; i++)
           {
               tmpcrc += (Int16)target[i];
           }
           tmpcrc %= 4096;
           target += (char)((byte)'=' + (tmpcrc / 64));
           target += (char)((byte)'=' + (tmpcrc % 64));
           //-----------------------------------------------------------
           target += (char)'\r'; 
           return target;
       }

       // ---------------------------------------------------------------------------
       /// <summary>
       /// Initialize the COM Port. ComPort must be set before calling this function
       /// </summary>
       /// <returns></returns>
       // ---------------------------------------------------------------------------
       public int InitPort()
       {
            serialPort1 = new SerialPort();
            serialPort1.DataReceived += new System.IO.Ports.SerialDataReceivedEventHandler(serialPort1_DataReceived);
            try
            {
                serialPort1.PortName = ComPort;
                serialPort1.BaudRate = 57600;
                serialPort1.DataBits = 8;
                serialPort1.Open();
                serialPort1.DiscardInBuffer();
                serialPort1.DiscardOutBuffer();
                isOpen = true; 
            }
            catch
            {
                isOpen = false; 
                return 1;
            }
            return 0;
       }

       // ---------------------------------------------------------------------------
       /// <summary>
       /// Writes a string to the Com Port
       /// </summary>
       /// <param name="data"></param>
       // ---------------------------------------------------------------------------
       public void SerialWrite(string data)
       {
            serialPort1.Write(data);
       }

       // ---------------------------------------------------------------------------
       /// <summary>
       /// Writes a string to the comport and adds a CR/LF at the end
       /// </summary>
       /// <param name="data"></param>
       // ---------------------------------------------------------------------------
       public void SerialWriteLine(string data)
       {
            serialPort1.WriteLine(data);
       }

       // ---------------------------------------------------------------------------
       /// <summary>
       /// colse the COm port if open
       /// </summary>
       // ---------------------------------------------------------------------------
       public void ClosePort()
       {
           if (isOpen)
           {
               serialPort1.Close();
           }
       }

       // ---------------------------------------------------------------------------
       /// <summary>
       /// Message functions
       /// </summary>
       // ---------------------------------------------------------------------------       
        public string GetLastMessage()
        {
            if (messagevalid == true)
            {
                return botmessage;
            }
            else return "";
        }

        public bool IsMessageValid()
        {
            return messagevalid;
        }

        public bool IsMessageError()
        {
            return messageerror;
        }

        public bool IsMessageOverrun()
        {
            return messageoverrun; 
        }

        public void InvalidateMessage()
        {
            messagevalid = false;
            messageerror = false;
            messageoverrun = false;
        }

        public char GetMessageCmd()
        {
            return message_cmd;
        }

        public char GetMessageAdr()
        {
            return message_adr;
        }
        
        public string MessageAddByte(byte value, string command)
        {
            command = command + (char)value;
            return command;
        }

        public string MessageAddInt16(Int16 value, string command)
        {
            byte s = 0;
            s = (byte)value;
            command = command + (char)s;
            Int16 value1 = (Int16)(value >> 8);
            s = (byte)value1;
            command = command + (char)s;
            return command;
        }

        public string MessageAddInt32(Int32 value, string command)
        {
            byte s = 0;
            s = (byte)value;
            command = command + (char)s;
            Int32 value1 = (Int32)(value >> 8);
            s = (byte)value1;
            command = command + (char)s;
            value1 = (Int32)(value >> 8);
            s = (byte)value1;
            command = command + (char)s;
            value1 = (Int32)(value >> 8);
            s = (byte)value1;
            command = command + (char)s;
            return command;
        }

        public byte MessageReadByte(string command)
        {
            byte bvalue = (byte)command[0];
            return bvalue;
        }

        public Int16 MessageReadInt16(string command)
        {
            int index = 0;
            Int16 tmp2;
            Int16 result;

            Int16 tmp;
            tmp2 = (Int16)command[index++];
            result = (Int16)command[index++];
            result = (Int16)(result << 8);
            result += (Int16)tmp2;
            return result;
        }

        public Int32 MessageReadInt32(string command)
        {
            int index = 0;
            Int32 tmp0;
            Int32 tmp1;
            Int32 tmp2;
            Int32 result;

            Int16 tmp;
            tmp0 = (Int32)command[index++];
            tmp1 = (Int32)command[index++];
            tmp2 = (Int32)command[index++];
            result = (Int32)command[index++];
            result = (Int32)(result << 8);
            result += (Int32)tmp2;
            result = (Int32)(result << 8);
            result += (Int32)tmp1;
            result = (Int32)(result << 8);
            result += (Int32)tmp0;
            return result;
        }


       // ---------------------------------------------------------------------------
       /// <summary>
       /// serial data receive message handler.  
       /// </summary>
       /// <param name="sender"></param>
       /// <param name="e"></param>
       // ---------------------------------------------------------------------------
       private static string command;
       private static bool messagevalid = false;
       private static bool messageerror = false;
       private static bool messageoverrun = false;
       private static char message_cmd = (char)0;
       private static char message_adr = (char)0;
       private static int receiver_status = 0;
       private static string botmessage; 

       private static void serialPort1_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
       {
            int data;
            EventArgs ev = new EventArgs();
            while (serialPort1.BytesToRead > 0)
            {
                data = serialPort1.ReadChar();
                //Console.Write((char)data);
                switch (receiver_status)
                {
                    case 0:
                        if (data.CompareTo('#') == 0)
                        {
                            command = "#";
                            receiver_status++;
                        }
                        break;
                    case 1:
                        message_adr = (char)data;
                        command = command + (char)data;
                        receiver_status++;
                        break;
                    case 2:
                        message_cmd = (char)data;
                        command = command + (char)data;
                        receiver_status++;
                        break;
                    case 3:
                        if ((data.CompareTo(13) != 0) && (data.CompareTo(10) != 0))
                        {
                            command = command + (char)data;
                            if (command.Length > 350)
                            {
                                command = "";
                                serialPort1.DiscardInBuffer();
                            }
                        }
                        else
                        {
                            if (messagevalid == true)
                            {
                                messageoverrun = true;
                            }
                            else
                            {
                                botmessage = DecodeMessage(command);
                                if (botmessage == "")
                                {
                                    messageerror = true;
                                }
                                else
                                {
                                    messagevalid = true;
                                }
                            }
                            BotEvent(sender, ev);
                            receiver_status = 0;
                            command = "";
                        }
                        break;
                }
               
            }
       }
    }
}