using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Net;
using System.Threading;
using System.Net.Sockets;
using System.Collections;
using SafeMobileLib;

namespace MotoTRBO_XNL_Cmd
{
    public class RadioConnection
    {
        private bool connected;
        public bool Connected
        {
            get { return connected; }
            set { connected = value; }
        }

        private bool authDone;
        public bool AuthDone
        {
            get { return authDone; }
            set { authDone = value; }
        }

        //private bool bussy ;
        private string radioIP;
        public string RadioIP
        {
            get { return radioIP; }
            set { radioIP = value; }
        }

        private int radioPort;
        public int RadioPort
        {
            get { return radioPort; }
            set { radioPort = value; }
        }

        private string firmware_version;
        public string Firmware_version
        {
            get { return firmware_version; }
        }

        private TcpClient tcp = null;
        private NetworkStream ns = null;
        private Thread connectionThread;

        private RadioMessage rmAuth;
        private string user="";

        public RadioConnection()
        {
            //Console.WriteLine("Radio connection class - constructor starteed");
            connected = false;
            //bussy = false;
            connectionThread = new Thread(new ThreadStart(HandleConnection));
            connectionThread.IsBackground = true;
        }

        public RadioConnection(string ip, int port) : this()
        {
            radioIP = ip;
            radioPort = port;
        }

        public RadioConnection(string ip, int port,string user)
            : this()
        {
            radioIP = ip;
            radioPort = port;
            this.user = user;
        }

        public bool Start()
        {
            try
            {
                IPEndPoint ipEnd = GetInterface(radioIP);
                if (ipEnd != null)
                {
                    if (connected)
                    {
                        Stop();
                    }
                    //init com  GW -> radio station
                    Utils.WriteLine("Connecting to radio on port: " + radioPort + " with ip :" + radioIP + " on interface " + ipEnd.ToString()+ " for:"+user, ConsoleType.XNL);
                    //tcp = new TcpClient(radioIP, radioPort);
                    tcp = new TcpClient(ipEnd);
                    tcp.Connect(radioIP, radioPort);
                    tcp.ReceiveTimeout = 1000;
                    tcp.SendTimeout = 1000;
                    ns = tcp.GetStream();
                    connected = true;
                    Utils.WriteLine("Connection established with " + radioIP + " starting message handle thread " + " for:" + user, ConsoleType.XNL);
                    if (connectionThread != null)
                    {
                        connectionThread.Start();
                    }
                    else
                    {
                        connectionThread = new Thread(new ThreadStart(HandleConnection));
                        connectionThread.IsBackground = true;
                        connectionThread.Start();
                    }
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                connected = false;
                Utils.WriteLine(ex.ToString(), ConsoleType.XNL);
                return false;
            }
        }

        public void Stop()
        {
            connected = false;
            AuthDone = false;
            if (tcp != null)
            {
                tcp.Close();
                tcp = null;
            }
            if (ns != null)
            {
                ns.Dispose();
                ns = null;
            }
            if (connectionThread != null)
            {
                if (connectionThread.IsAlive)
                {
                    connectionThread.Abort();
                    connectionThread = null;
                }
            }
            rmAuth = null;
            Utils.WriteLine("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", ConsoleType.XNL);
            Utils.WriteLine("Communication with " + radioIP + " STOPED!!!!" + " for:" + user, ConsoleType.XNL);
            Utils.WriteLine("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", ConsoleType.XNL);
        }
        
        private void HandleConnection()
        {
            byte[] receiveBytesRadio;
            RadioMessage rm = new RadioMessage();
            rm.OnAuthDone += new RadioMessage.AuthDone(rm_OnAuthDone);
            rm.OnSpeakersEvent += new RadioMessage.SpeakersEvent(rm_OnSpeakersEvent);
            rm.OnCallEvent += new RadioMessage.CallEvent(rm_OnCallEvent);
            rm.OnChannelEvent += new RadioMessage.ChannelEvent(rm_OnChannelEvent);
            rm.OnRemoteControlEvent += new RadioMessage.RemoteControlEvent(rm_OnRemoteControlEvent);
            rm.OnEmergencyEvent += new RadioMessage.EmergencyEvent(rm_OnEmergencyEvent);
            rm.OnTransmitEvent += new RadioMessage.TransmitEvent(rm_OnTransmitEvent);
            rm.OnRestartEvent += new RadioMessage.RestartEvent(rm_OnRestartEvent);
            rm.OnVersionEvent += new RadioMessage.VersionEvent(rm_OnVersionEvent);
            rm.OnSendEmergencyEvent += new RadioMessage.SendEmergencyEvent(rm_OnSendEmergencyEvent);
            rm.OnNavRepEvent += new RadioMessage.NavRepEvent(rm_OnNavRepEvent);


            while (true)
            {
                if (connected)
                {
                    try
                    {
                        Thread.Sleep(1);
                        receiveBytesRadio = new byte[1024];
                        if (ns == null) break;
                        if (ns.DataAvailable)
                        {
                            
                            int recv = ns.Read(receiveBytesRadio, 0, receiveBytesRadio.Length);
                            

                            Queue<RawMessage> queue = SplitMessages(receiveBytesRadio, recv);

                            /*
                            Console.WriteLine("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% " + recv);
                            Console.WriteLine("%%%%%%%%%%%%% Message received %%%%%%%%%%%%% recv=" + recv);
                            Console.Write("RX Data raw: ");
                            for (int i = 0; i < recv; i++)
                                Console.Write(" 0x" + receiveBytesRadio[i].ToString("X"));
                            Console.WriteLine("");
                            Console.WriteLine("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ");
                             */


                            while (queue.Count > 0)
                            {
                                RawMessage rawmsg = queue.Dequeue();

                               /*
                                Console.WriteLine("******************************************** " + rawmsg.len);
                                Console.WriteLine("************ Message feed ************** recv=" + rawmsg.len);
                                Console.Write("Data raw feed: ");
                                for (int i = 0; i < rawmsg.len; i++)
                                    Console.Write(" 0x" + rawmsg.data[i].ToString("X2"));
                                Console.WriteLine("");
                                Console.WriteLine("******************************************** ");
                                */


                                bool messageStatus = rm.processMessage(rawmsg.data, rawmsg.len);
                                if (messageStatus)
                                {
                                    //Console.WriteLine("@@@@@@@@@@@@@ if (messageStatus) @@@@@@");
                                    byte[] msg = new byte[1024];
                                    msg = rm.genResponseMessage();

                                    Utils.WriteLine("Msg response: ", ConsoleType.XNL);
                                    for (int i = 0; i < msg.Length; i++)
                                        Utils.Write(" 0x" + msg[i].ToString("X2"), ConsoleType.XNL);
                                    Utils.Write("\n", ConsoleType.XNL);
                                    if ((msg[0] != 0) || msg[1] != 0) //message length >0
                                    {
                                        Utils.WriteLine("sending message", ConsoleType.XNL);                                        
                                        ns.Write(msg, 0, msg.Length);
                                    }
                                    messageStatus = false;
                                    if (rm.connectionStatus)
                                    {
                                        rmAuth = rm;
                                    }
                                }
                            }
                        }
                        if (!ns.CanRead && !ns.CanWrite && !ns.CanTimeout)
                        {
                            Utils.WriteLine("ns.CanRead:" + ns.CanRead
                                + "ns.CanWrite:" + ns.CanWrite
                                + "ns.CanTimeout:" + ns.CanTimeout, ConsoleType.XNL);
                        }
                    }
                    catch (Exception ex)
                    {
                        Utils.WriteLine("Exception in HandleConnection of radio:" + RadioIP + " for:" + user, ConsoleType.XNL);
                        Utils.WriteLine(ex.ToString(), ConsoleType.XNL);
                        Connected = false;
                        break;
                    }
                }
                Thread.Sleep(1);
            }
        }            
        
        #region EVENTS HANDLER
        void rm_OnAuthDone(RadioMessage rm)
        {
            /*
            Console.WriteLine("========================================");
            Console.WriteLine("Auth done with :" + radioIP);
            Console.WriteLine("========================================");
             */
            AuthDone = true;
            if (rm.connectionStatus)
            {
                rmAuth = rm;
            }
            if (this.OnAuthDone != null)
            {
                this.OnAuthDone();
            }

            if (this.OnAuthDone2 != null)
            {
                this.OnAuthDone2(user);
            }
            //send version request
            VersionRequest();

            //send 'Disable Display Messages' Request
            disableDisplayBcstRequest();
        }

        void rm_OnSpeakersEvent(bool onOFF)
        {
            if (onOFF)
            {
                //Console.WriteLine("Speakers ON on:" + radioIP);
                if (this.OnSpeakersEvent != null)
                {
                    this.OnSpeakersEvent(onOFF);
                }
            }
            else
            {
                //Console.WriteLine("Speakers OFF on:" + radioIP);
                if (this.OnSpeakersEvent != null)
                {
                    this.OnSpeakersEvent(onOFF);
                }
            }
        }

        void rm_OnCallEvent(CALL_STATUS status, int callerID, int callType, int groupID)
        {
            if (this.OnCallEvent != null)
            {
                this.OnCallEvent(status, callerID, callType, groupID);
            }

        }

        void rm_OnTransmitEvent(bool onOFF)
        {
            if (this.OnTransmitEvent != null)
            {
                this.OnTransmitEvent(onOFF);
            }
        } 

        void rm_OnChannelEvent(int zoneNr,int channelNr)
        {
            if (this.OnChannelEvent != null)
            {
                this.OnChannelEvent(zoneNr,channelNr);
            }
        }       

        void rm_OnRemoteControlEvent(bool status, REMOTE_RADIO_FUNCS func, int radioId)
        {
            //Console.WriteLine("Response form 'remote radio control' status:{0} func:{1} radioID:{2}",status,func,radioId);
            if (this.OnRemoteControlEvent != null)
            {
                this.OnRemoteControlEvent(status,func,radioId);
            }
        }

        void rm_OnEmergencyEvent(int radioId,bool status)
        {
            if (this.OnEmergencyEvent != null)
            {
                this.OnEmergencyEvent(radioId,status);

                if (status)
                {
                    //send ACk
                    SendEmergencyACK();
                }
            }
        }

        void rm_OnRestartEvent()
        {
            if (this.OnRestartEvent != null)
            {
                this.OnRestartEvent();
            }
        }

        void rm_OnVersionEvent(string version)
        {
            firmware_version = version;
            if (this.OnVersionEvent != null)
            {
                this.OnVersionEvent(version);
            }
        }

        void rm_OnSendEmergencyEvent(int function, int status)
        {
            if (this.OnSendEmergencyEvent != null)
            {
                this.OnSendEmergencyEvent(function, status);
            }
        }

        void rm_OnNavRepEvent(int function, int status)
        {
            if (this.OnNavRepEvent != null)
            {
                this.OnNavRepEvent(function, status);
            }
        }
        #endregion

        #region COMMANDS

        public bool SendPTTon()
        {
            bool ret = false;
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                //msg1 = rmAuth.pttONOFF(true);
                msg1 = rmAuth.pttWithPUI(true);
                //Console.WriteLine("sending PTT ON");
                ret = SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"PTT ON\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
            return ret;
        }
        public bool SendPTToff()
        {
            bool ret = false;
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                //msg1 = rmAuth.pttONOFF(false);
                msg1 = rmAuth.pttWithPUI(false);
                //Console.WriteLine("sending PTT off");
                ret = SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"PTT OFF\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
            return ret;
        }
        /// <summary>
        /// send call type 
        /// </summary>
        /// <param name="type">call type</param>
        /// <param name="id">subscriber or group id</param>
        public void SendCallType(byte type, int id)
        {
            if (rmAuth != null && connected)
            {
                //Console.WriteLine("Sending call type to radio");
                byte[] msg = new byte[1024];
                msg = rmAuth.callType(type, id);
                SendCommand2Radio(msg);
            }
            else
            {
                Utils.WriteLine("Could not send command \"call type\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        /// <summary>
        /// send a zone & channel nr request
        /// if zone = 0 and channel = 0 then a queery request is made
        /// </summary>
        /// <param name="channelNr"></param>
        /// <param name="zoneNr"></param>
        /// <returns></returns>
        public bool SendChannelChange(int zoneNr,int channelNr)
        {
            bool ret = false;
            if (rmAuth != null && connected)
            {
                byte[] msg = new byte[1024];
                msg = rmAuth.channelSelect(channelNr, zoneNr);
                //Console.WriteLine("Sending channel selection on " + radioIP);
                if (SendCommand2Radio(msg)) ret = true;
            }
            else
            {
                Utils.WriteLine("Could not send command \"channel selection\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
            return ret;
        }


        public void SendEmergencyAck(int radioId)
        {
            /*
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.SendEmergencyToRadio(radioId, false);
                Console.WriteLine("sending emergency ack to:" + radioId);
                SendCommand2Radio(msg1);
            }
            else
            {
                Console.WriteLine("Could not send command \"SendEmergencyAck\" radio: " + radioIP + " not connected" + " for:" + user);
            }*/

            // nothing to do here because there is no emergency stop for a field radio
        }



        public void SendRemoteDisable(int radioId)
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.remoteRadioControl(radioId, REMOTE_RADIO_FUNCS.Disable);
                Utils.WriteLine("sending radio disable to:"+radioId, ConsoleType.XNL);
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendRemoteDisable\" radio: " + radioIP + " not connected" + " for:" + user, ConsoleType.XNL);
            }
        }

        public void SendRemoteEnable(int radioId)
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.remoteRadioControl(radioId, REMOTE_RADIO_FUNCS.Enable);
                //Console.WriteLine("sending radio enable to:" + radioId); 
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendRemoteEnable\" radio: " + radioIP + " not connected" + " for:" + user, ConsoleType.XNL);
            }
        }

        public void SendRemoteMonitor(int radioId)
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.remoteRadioControl(radioId, REMOTE_RADIO_FUNCS.Monitor);
                //Console.WriteLine("sending radio Monitor to:" + radioId);
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendRemoteMonitor\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void SendRemoteCheck(int radioId)
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.remoteRadioControl(radioId, REMOTE_RADIO_FUNCS.Check);
                //Console.WriteLine("sending radio check to:" + radioId);
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendRemoteCheck\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void SendDeKey()
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.remoteRadioControl(0, REMOTE_RADIO_FUNCS.DeKey);
                //Console.WriteLine("sending radio DeKey");
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendDeKey\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void SendEmergency(bool onOFF)
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.SendEmergency(onOFF);
                //Console.WriteLine("sending radio DeKey");
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendDeKey\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void Restart()
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.RestartRequest();
                //Console.WriteLine("sending radio DeKey");
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"Restart\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void VersionRequest()
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.VersionRequest();
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"VersionRequest\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }


        /**
         * Send 'Disable Display Messages' Request 
         * This message is used to prevent DM4600 series from reporting any
         * changes of the Display [Display_NONE as mentioned in the docs]
         */
        public void disableDisplayBcstRequest()
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.disableDisplayBcstRequest();
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"disableDisplayBcstRequest\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void DispalayTest(string txt)
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.DisplayText(txt);
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"VersionRequest\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void SendEmergencyACK()
        {
            if (rmAuth != null && connected)
            {
                /*
                Console.WriteLine("sending back button short press(on)!!!");
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.backBtnShortPress(true);                
                SendCommand2Radio(msg1);
                Thread.Sleep(100);
                Console.WriteLine("sending back button short press(off)!!!");
                msg1 = rmAuth.backBtnShortPress(false);
                SendCommand2Radio(msg1);
                Thread.Sleep(100);
                
                //sending P4 long press!!!
                msg1 = rmAuth.btnP4longPress(true);
                Console.WriteLine("sending P4 button long press(on)!!!");
                SendCommand2Radio(msg1);
                Thread.Sleep(1100);
                msg1 = rmAuth.btnP4longPress(false);
                Console.WriteLine("sending P4 button long press(off)!!!");
                SendCommand2Radio(msg1);
                */
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.SendEmergencyAck();
                Utils.WriteLine("sending P4 button long press(off)!!!", ConsoleType.XNL);
                Thread.Sleep(200);
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendEmergencyACK\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void SendNavReq()
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];
                msg1 = rmAuth.sendNavReq();  //remoteRadioControl(0, REMOTE_RADIO_FUNCS.DeKey);
                //Console.WriteLine("sending radio DeKey");
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendDeKey\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        public void SendNavReqQuery()
        {
            if (rmAuth != null && connected)
            {
                byte[] msg1 = new byte[1024];     
                msg1 = rmAuth.sendNavReqQueryList();  //remoteRadioControl(0, REMOTE_RADIO_FUNCS.DeKey);
                //Console.WriteLine("sending radio DeKey");
                SendCommand2Radio(msg1);
            }
            else
            {
                Utils.WriteLine("Could not send command \"SendDeKey\" radio: " + radioIP + " not connected", ConsoleType.XNL);
            }
        }

        private bool SendCommand2Radio(byte[] data)
        {
            bool ret = false;
            int length = 2;
            length += data[0] << 8;
            length += data[1];
            Utils.WriteLine("### LENGTH ###" + length, ConsoleType.XNL);

            try
            {
                ns.Write(data, 0, length);
                ret = true;
            }
            catch (Exception ex)
            {
                Utils.WriteLine("Error in SendCommand2Radio " + radioIP, ConsoleType.XNL);
                Utils.WriteLine(ex.ToString(), ConsoleType.XNL);
            }
            return ret;
        }

        #endregion

        public IPEndPoint GetInterface(string IPstr)
        {
            string strInterface = "";
            IPEndPoint ipend = null;
            try
            {
                string[] arrStr = IPstr.Split('.');
                int lastPartOfIP = Convert.ToInt32(arrStr[3]);
                arrStr[3] = (lastPartOfIP + 1).ToString();
                strInterface = arrStr[0] + "." + arrStr[1] + "." + arrStr[2] + "." + arrStr[3];
                ipend = new System.Net.IPEndPoint(IPAddress.Parse(strInterface), 0);
            }
            catch (Exception ex)
            {
                Utils.WriteLine("Error GEtting interface", ConsoleType.XNL);
                Utils.WriteLine(ex.ToString(), ConsoleType.XNL);
            }
            return ipend;
        }

        #region EVENTS
        public delegate void AuthDoneDEl();
        public event AuthDoneDEl OnAuthDone;

        public delegate void AuthDoneDEl2(string user);
        public event AuthDoneDEl2 OnAuthDone2;

        public delegate void SpeakersEvent(bool onOFF);
        public event SpeakersEvent OnSpeakersEvent;

        public delegate void CallEvent(CALL_STATUS status, int callerID, int callType, int groupID);
        public event CallEvent OnCallEvent;

        public delegate void ChannelEvent(int zoneNr, int channelNr);
        public event ChannelEvent OnChannelEvent;

        public delegate void RemoteControlEvent(bool status, REMOTE_RADIO_FUNCS func, int radioId);
        public event RemoteControlEvent OnRemoteControlEvent;

        public delegate void EmergencyEvent(int radioId, bool status);
        public event EmergencyEvent OnEmergencyEvent;

        public delegate void TransmitEvent(bool onOFF);
        public event TransmitEvent OnTransmitEvent;

        public delegate void RestartEvent();
        public event RestartEvent OnRestartEvent;

        public delegate void VersionEvent(string version);
        public event VersionEvent OnVersionEvent;

        public delegate void ConnectionDropedEvent(string radioIP);
        public event ConnectionDropedEvent OnConnectionDropedEvent;

        public delegate void SendEmergencyEvent(int function, int status);
        /// <summary>
        /// function: 0x01 - Enter Emergency
        ///           0x03 - Exit emrgency 
        /// status: 0x00 - success
        ///         0x01 - failure
        ///         0x03 - Emergency feature not available
        /// </summary>
        public event SendEmergencyEvent OnSendEmergencyEvent;

        public delegate void NavRepEvent(int function, int status);
        public event NavRepEvent OnNavRepEvent;

        #endregion

        //aux
        public Queue<RawMessage> SplitMessages(byte[] data, int len)
        {
            Queue<RawMessage> queue = new Queue<RawMessage>();

            //get len from first 2 bytes;
            int msg_len = RadioMessage.bytes2ushort(data[0], data[1]);
            msg_len += 2;
            if (msg_len == len)
            {
                RawMessage raw = new RawMessage();
                raw.data = data;
                raw.len = len;

                queue.Enqueue(raw);
            }
            else
            {
                if (msg_len > len)
                {
                    Utils.WriteLine(String.Format("Incomplite message ..drop it!!! msq_len:{0}  recv_len:{1}", msg_len, len), ConsoleType.XNL);
                }
                else
                {
                    try
                    {
                        /*
                        Console.WriteLine(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
                        Console.WriteLine(">>>>>>>>>>>>>>>> processing mutliple messages >>>>>>>>>>>>>>>>>>");
                        
                         */
                        byte[] data4pars = data;
                        int len4parse = len;

                        /*
                        Console.WriteLine("");
                        Console.Write("Initial data: ");
                        for (int i = 0; i < len4parse; i++)
                            Console.Write(" 0x" + data4pars[i].ToString("X"));
                        Console.WriteLine("");
                        Console.WriteLine("");
                         */

                        int count = 0;
                        while (len4parse > 0)
                        {
                            //Console.WriteLine("data4pars.Length:" + data4pars.Length);
                            //Console.WriteLine("len4parse:" + len4parse);
                            msg_len = RadioMessage.bytes2ushort(data4pars[0], data4pars[1]);
                            msg_len += 2;
                            //Console.WriteLine("msg_len:" + msg_len);
                            RawMessage raw = new RawMessage();
                            //add first message to queue
                            raw.data = new byte[msg_len];
                            for (int i = 0; i < msg_len; i++)
                            {
                                raw.data[i] = data4pars[i];
                            }
                            
                            raw.len = msg_len;
                            /*
                            Console.WriteLine("");
                            Console.Write("Message {0}: ",count);
                            for (int i = 0; i < raw.len; i++)
                                Console.Write(" 0x" + raw.data[i].ToString("X"));
                            Console.WriteLine("");
                            Console.WriteLine("");
                             */

                            queue.Enqueue(raw);                                                        
                            
                            byte[] datatemp = new byte[len4parse];
                            
                            for (int i = msg_len; i < len4parse; i++)
                            {
                                datatemp[i - msg_len] = data4pars[i];
                            }
                             
                            
                            data4pars = datatemp;
                            len4parse = len4parse - msg_len;
                            count++;
                        }
                    }
                    catch (Exception ex)
                    {
                        Utils.WriteLine("Error in SplitMessages.. when multiple messsages present!!\n" + ex.ToString(), ConsoleType.XNL);
                    }
                    //Console.WriteLine(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
                }

            }

            return queue;
        }
    }
}