using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;

using System.Text;

using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.Collections;
using SafeNetLib;

namespace ConnectPlus_SOC
{

    class LocationThread
    {
        public int port;
        //public MotoTRBOGW parent;
        public static Hashtable ht_location_hash;
        public static Hashtable ht_pendingMsg;
        private byte[] data = new byte[1024];
        private static UdpClient udpClient;
        private Int32 reqID_send = 0x01;
        private string reqID;

        // default request id for Location REQUEST
        public static readonly byte REQ_ID = 0xEE;
        private int ACCEPTED_LEVEL_OF_CONFIDENCE = 20;


        private Int32 reqID_recv;
        // -------------------------------------------------------------------
        // Main
        // -------------------------------------------------------------------
        public LocationThread()
        {
            Random rnd1 = new Random();
            reqID_send = (byte)rnd1.Next(0xffffff);
        }
        private bool start_read = true;

        private bool startAsyncRead()
        {
            EndPoint reponseEndPoint = new IPEndPoint(0, 0);
            try
            {
                if (udpClient == null)
                {
                    udpClient = new UdpClient(ConnectPlus_GW.cfg.locPort_r);
                    udpClient.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
                    //udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, ConnectPlus_GW.cfg.locPort_r));
                    //Console.WriteLine("ReBinding location udpClient");
                }
                IPEndPoint remoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0);

                // Blocks until a message returns on this socket from a remote host.
                udpClient.Client.BeginReceiveFrom(data, 0, data.Length,
                    SocketFlags.None, ref reponseEndPoint,
                    new AsyncCallback(AsyncRead),
                    (object)this);
                Thread.Sleep(100);
                //Console.WriteLine("AsyncRead started for Location thread on port:" + ConnectPlus_GW.cfg.locPort_r);
            }
            catch (Exception ex)
            {
                Utils.ConsWrite(DebugMSG_Type.always, "HandleConnection(): exception " + ex);
            }
            return true;
        }

        public void HandleConnection()
        {
            ht_location_hash = new Hashtable();
            ht_pendingMsg = new Hashtable();
            //TestPacket();
            //return;   
            start_read = false;          
            startAsyncRead();
            DateTime lastHashVerify = DateTime.Now;
            while (true)
            {                
                try
                {
                    MotoTRBOcmdMsg msg = SN_Queues.locationQueue.GetItem(100);
                     //add data to ht_SUInfo

                    if (msg != null)
                    {
                        ProcessCommand(msg); //this will update the m_seqID and m_time
                        if (!ht_pendingMsg.ContainsKey(msg.m_seqID))
                            ht_pendingMsg.Add(msg.m_seqID, msg);
                    }

                    //is now answer within 30 secconds resend
                    if (ht_pendingMsg.Count > 0)
                        if ((DateTime.Now.Ticks - lastHashVerify.Ticks) > 30 * 1000 * 10000)
                        {
                            LOGS.LOG("Debug: LOCK1");
                            lock (ht_pendingMsg.SyncRoot)
                            {
                                try
                                {
                                    Hashtable tmp = new Hashtable();
                                    Utils.ConsWrite(DebugMSG_Type.GPS, "Controler answers check at :" + DateTime.Now);
                                    foreach (DictionaryEntry de in ht_pendingMsg)
                                    {
                                        MotoTRBOcmdMsg m = (MotoTRBOcmdMsg)de.Value;

                                        //if not send remove from pendingMsg and add back to location Queue
                                        if (DateTime.Now.Ticks - m.m_time.Ticks > 60 * 1000 * 10000)
                                            if (m.m_cmd == (int)MotoTRBOcmd.SET_REPORT_INTERVAL) //put back in queue ONLY LRRP set requests
                                            {
                                                SN_Queues.locationQueue.PostItem(m);
                                                Thread.Sleep(1000);
                                            }
                                            else if (!tmp.ContainsKey(m.m_seqID))
                                                tmp.Add(m.m_seqID, m);

                                    }
                                    ht_pendingMsg = tmp;
                                    lastHashVerify = DateTime.Now;
                                }
                                catch (Exception e)
                                {
                                    LOGS.LOG("Error: cannot reset pending messages HashTable" + e.ToString());
                                }
                            }//release lock
                            LOGS.LOG("Debug: UN-LOCK1");
                        }
                    
                    if (start_read)
                    {
                        start_read = false;
                        LOGS.LOG("&GPS& " + "1.startAsyncRead();");
                        startAsyncRead();
                    }
                   
                    
                }
                catch (Exception ex)
                {
                    Console.ForegroundColor = ConsoleColor.DarkRed;
                    Utils.ConsWrite(DebugMSG_Type.always, "Exception in LocationThread:HandleConnection(): " + ex.ToString());
                    Console.ForegroundColor = ConsoleColor.Gray;
                }
            } // end while (true)
        }
        

        void AsyncRead(IAsyncResult result)
        {
            EndPoint remoteEndPoint = new IPEndPoint(0, 0);
            try
            {
                // read bytes from UDP and decode them if length is grater than 1 char
                int bytesRead = udpClient.Client.EndReceiveFrom(result, ref remoteEndPoint);
                LOGS.LOG("&GPS& " + "2.AsyncRead(); got GPS message from controller");
                if (bytesRead > 1)
                {
                    // update lastActivityTime
                    ConnectPlus_GW.lastActivityTime = DateTime.Now;
                    //Console.WriteLine("\n--------------------");
                    //Console.WriteLine("AsyncRead endded processing data!!!");
                    //Console.WriteLine("GPS Received: " + Byte2String(data,0,bytesRead));
                    DecodePacket_Connect_plus(data);
                    LOGS.LOG("&GPS& " + "3.AsyncRead(); GPS message decoded");
                }

                // flag that a new message can be read
                start_read = true;
                LOGS.LOG("&GPS& " + "4.AsyncRead(); start_read = true;");
            }
            catch (Exception ex)
            {
                Console.WriteLine("AsyncRead Exception : " + ex.ToString());
                start_read = true;
                LOGS.LOG("&GPS& " + "4.AsyncRead(); start_read = true;");
            }
        }

        public void DecodePacket_Connect_plus(Byte[] data)
        {
            int i = 0, pdata;

            htCell_t cell = new htCell_t();
            cell.lat = "";
            cell.lng = "";
            cell.di = "0";
            cell.spd = "0";
            cell.spd_v = "0";
            cell.suid = "";
            cell.location_time = DateTime.MinValue;
            cell.activity_time = DateTime.MinValue;
            cell.triggered = false;
            cell.level_confidence = -1;

            switch (data[0])
            {
                // -----------------------------------------------------------------------------------------------------------------------
                case (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT:
                case (byte)Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT:
                    
                   if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT)
                        Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT);
                    else Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT);
                    
                    pdata = (int)data[1];
                    string result="";
                    //Console.WriteLine("Length =" + pdata + "(0x" + pdata.ToString("X") + ")");
                    for (i = 2; i < data[1] + 2; i++)
                    {
                        switch (data[i])
                        {
                            case (byte)Common_Element_Tokens_ENUM.request_id:
                                //Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + Byte2String(data, i + 1, 5));
                                 
                                //Console.WriteLine(data[i + 5]);
                                byte[] bytes = { data[i + 5], data[i + 4], data[i + 3], data[i + 2] };
                                reqID = Byte2String(data, i + 1, 5);
                                reqID_recv = BitConverter.ToInt32(bytes, 0);

                                cell.seq_ID = Byte2String(data, i + 2, data[i+1]) + "";
                                cell.requestID = data[i + 5];
                                i += 5;
                                break;

                            case (byte)Report_Messages_Tokens_ENUM.result:
                            case (byte)Report_Messages_Tokens_ENUM.result2:
                                //Console.WriteLine("Result: " + Byte2String(data, i + 1, 1));
                                if (data[i + 1] != 0)
                                {
                                    Utils.ConsWrite(DebugMSG_Type.GPS, "GPS returned with » " + ProcessError(data[i + 1]));
                                    result = ProcessError(data[i + 1]);
                                }
                                //Console.WriteLine("OpaqueData: " + Byte2String(data, i + 2, data[1] - i));
                                i = data[1] + 1; // exit
                                break;
                            case (byte)Report_Messages_Tokens_ENUM.result1:
                                //Console.WriteLine("Result: OK");
                                result = "OK";
                                break;
                            case (byte)Report_Messages_Tokens_ENUM.SU_IPv4:
                                string suid = ProcessSU_ID(data, i + 1).ToString();
                                cell.suid = suid;
                                i += 4;
                                //Console.Write("SUIDc: " + suid + " ### ");
                                break;
                        }
                    }

                    LOGS.LOG("Debug: LOCK3");
                    //remove message from pending
                    lock (ht_pendingMsg.SyncRoot)
                    {
                        try
                        {
                            if (reqID_recv!= null && ht_pendingMsg.ContainsKey(reqID_recv))
                            {
                                //LOGS.LOG(ConnectPlus_GW.cfg.gw_id + cell.suid + " removing from pending \t regID:" + reqID);
                                ht_pendingMsg.Remove(reqID_recv);

                            }
                        }catch (Exception e)
                                { 
                                    LOGS.LOG("Error: cannot delete pending messages HashTable"+e.ToString());
                                }
                    }
                    LOGS.LOG("Debug: UN-LOCK3");

                    if(data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT)
                        LOGS.LOG(ConnectPlus_GW.cfg.gw_id + cell.suid + " Triggered_Location_Stop_Answer_NoCDT \t regID:" + reqID + " \t result:" + result);
                    else
                        LOGS.LOG(ConnectPlus_GW.cfg.gw_id + cell.suid + " Triggered_Location_Answer_NoCDT \t regID:" + reqID + " \t result:" + result);
                    break;

                // -----------------------------------------------------------------------------------------------------------------------
                case (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT:
                case (byte)Document_Identifiers_ENUM.Immediate_Location_Report_NoCDT:
                    {
                        /*
                        if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT)
                            Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT);
                        else
                            Console.WriteLine(Document_Identifiers_ENUM.Immediate_Location_Report_NoCDT);
                        */
                        if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT)
                        {
                            cell.triggered = true;
                        }
                        else
                        {
                            //Utils.ConsWrite(DebugMSG_Type.DEV, "Got request id:" + cell.requestID);
                        }

                        pdata = (int)data[1];
                        Boolean parsingError = false;
                        byte parsErrorBYte = (byte)Result_Codes_ENUM.SYNTAX_ERROR;
                        DateTime gps_time = DateTime.MinValue;
                        for (i = 2; i < data[1] + 2; i++)
                        {
                            switch (data[i])
                            {
                                case (byte)Report_Messages_Tokens_ENUM.point_2d:
                                    cell.lat = ProcessLat(data, i + 1, 4).ToString();
                                    cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
                                    cell.d_lat = ProcessLat(data, i + 1, 4);
                                    cell.d_lng = ProcessLng(data, i + 1 + 4, 4);
                                    if (gps_time == DateTime.MinValue)
                                        cell.location_time = DateTime.Now.ToUniversalTime();
                                    /*
                                    Console.WriteLine("Point_2d: " + data[i].ToString("X"));
                                    Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
                                    Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
                                     */
                                    i += 8;
                                    break;

                                case (byte)Report_Messages_Tokens_ENUM.point_3d:
                                    //Console.WriteLine("### Point 3d ###");
                                    cell.lat = ProcessLat(data, i + 1, 4).ToString();
                                    cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
                                    cell.d_lat = ProcessLat(data, i + 1, 4);
                                    cell.d_lng = ProcessLng(data, i + 1 + 4, 4);
                                    if (gps_time == DateTime.MinValue)
                                        cell.location_time = DateTime.Now.ToUniversalTime();
                                    
                                    i += 11;
                                    break;

                                case (byte)Report_Messages_Tokens_ENUM.circle_2d:
                                    cell.lat = ProcessLat(data, i + 1, 4).ToString();
                                    cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
                                    cell.d_lat = ProcessLat(data, i + 1, 4);
                                    cell.d_lng = ProcessLng(data, i + 1 + 4, 4);
                                    if (gps_time == DateTime.MinValue)
                                        cell.location_time = DateTime.Now.ToUniversalTime();
                                    cell.radius = ProcessUFloat2B(data, i + 1 + 8);
                                    /*
                                    Console.WriteLine("Circle_2d: " + data[i].ToString("X"));
                                    Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
                                    Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
                                    Console.WriteLine("Rad: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.radius);
                                     */
                                    i += 10;
                                    break;

                                case (byte)Report_Messages_Tokens_ENUM.circle_3d:
                                case (byte)Report_Messages_Tokens_ENUM.circle_3d1:
                                    cell.lat = ProcessLat(data, i + 1, 4).ToString();
                                    cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
                                    cell.d_lat = ProcessLat(data, i + 1, 4);
                                    cell.d_lng = ProcessLng(data, i + 1 + 4, 4);
                                    if (gps_time == DateTime.MinValue)
                                        cell.location_time = DateTime.Now.ToUniversalTime();
                                    cell.radius = ProcessUFloat2B(data, i + 1 + 8);
                                    /*
                                    Console.WriteLine("Circle_3d: " + data[i].ToString("X"));
                                    Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
                                    Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
                                    Console.WriteLine("Rad: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.radius);
                                    Console.WriteLine("Alt: " + Byte2String(data, i + 1 + 10, 2));
                                    Console.WriteLine("Ala: " + Byte2String(data, i + 1 + 12, 2));
                                     */
                                    //if (!cell.triggered) Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
                                    //if (!cell.triggered) Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
                                    i += 14;
                                    break;

                                case (byte)Report_Messages_Tokens_ENUM.speed_vrt: //!! not tested
                                    cell.spd_v = Math.Round((ProcessUFloat2B(data, i + 1) * 3.6)).ToString();
                                    //Console.WriteLine(Report_Messages_Tokens_ENUM.speed_vrt + ": " + Byte2String(data, i + 1, 2) + " =" + cell.spd_v);
                                    i += 2;
                                    break;
                                case (byte)Report_Messages_Tokens_ENUM.speed_hor:
                                    cell.spd = Math.Round((ProcessUFloat2B(data, i + 1) * 3.6)).ToString();
                                    //Console.WriteLine(Report_Messages_Tokens_ENUM.speed_hor + ": " + Byte2String(data, i + 1, 2) + " =" + cell.spd);
                                    i += 2;
                                    break;
                                case (byte)Report_Messages_Tokens_ENUM.lev_conf: //!! not tested
                                    cell.level_confidence = (int)data[i + 1];
                                    //Console.WriteLine(Report_Messages_Tokens_ENUM.lev_conf + ": " + Byte2String(data, i + 1, 1) + " =" + cell.level_confidence) ;
                                    i += 1;
                                    break;

                                case (byte)Common_Element_Tokens_ENUM.request_id:
                                    cell.seq_ID = Byte2String(data, i + 2, data[i+1]) + "";
                                    cell.requestID = data[i + 1 + data[i + 1]];
                                    //if (!cell.triggered) Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + Byte2String(data, i + 1, 5));
                                    i += 5;
                                    break;
                                case (byte)Report_Messages_Tokens_ENUM.info_time:
                                case (byte)Report_Messages_Tokens_ENUM.info_time1:
                                    gps_time = ProcessTime(data, i + 1, 5);
                                    //Console.WriteLine("INFO_TIME: " + Report_Messages_Tokens_ENUM.info_time + ": " + Byte2String(data, i + 1, 5) + " =" + gps_time);
                                    i += 5;
                                    break;
                                case (byte)Report_Messages_Tokens_ENUM.SU_IPv4:
                                    string suid = ProcessSU_ID(data, i + 1).ToString();
                                    cell.suid = suid;
                                    //Utils.ConsWrite(DebugMSG_Type.GPS, "SUID: " + suid + " ### ");
                                    //if (!cell.triggered) 
                                       // Utils.ConsWrite(DebugMSG_Type.DEV, "SUID: " + suid + " ### ");
                                    i += 4;
                                    break;
                                // RESULTS
                                case (byte)Report_Messages_Tokens_ENUM.result:
                                case (byte)Report_Messages_Tokens_ENUM.result2:
                                    //Console.WriteLine("Result: " + Byte2String(data, i + 1, 1));
                                    if (data[i + 1] != 0)
                                    {
                                        parsErrorBYte = data[i + 1];

                                        if (data[i + 1] == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE ||
                                            data[i + 1] == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_CURRENTLY_ATTAINABLE)
                                        {
                                            cell.lat = "0";
                                            cell.lng = "0";
                                            cell.spd = "0";
                                            gps_time = DateTime.UtcNow;
                                            //Console.WriteLine("INFO got QUERY_INFO_NOT_ATTAINABLE ...adding values lat=0,lng=0,time =DateTime.UtcNow");
                                        }
                                        else
                                        {
                                            Utils.ConsWrite(DebugMSG_Type.GPS, "GPS returned with »» " + ProcessError(data[i + 1]));
                                            parsingError = true;
                                        }

                                    }
                                    //Console.WriteLine("OpaqueData: " + Byte2String(data, i + 2, data[1] - i));
                                    i = data[1] + 1; // exit
                                    break;
                                case (byte)Report_Messages_Tokens_ENUM.result1:
                                    //Console.WriteLine("Result: OK");
                                    break;
                                
                                case (byte)Report_Messages_Tokens_ENUM.direction_hor:
                                    i += 1;
                                    break;

                                default:
                                    Utils.ConsWrite(DebugMSG_Type.GPS, "Unknown: 0x" + data[i].ToString("X"));
                                    break;
                            }
                        }

                        if (gps_time == DateTime.MinValue)
                            Utils.ConsWrite(DebugMSG_Type.GPS, "GPS gps_time == DateTime.MinValue");    
                        else if (!parsingError || 
                            (((parsErrorBYte == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE) ||
                                (parsErrorBYte == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_CURRENTLY_ATTAINABLE)) && (!cell.triggered)))
                        {
                            //Utils.ConsWrite(DebugMSG_Type.DEV, "Got location from" + cell.suid);
                            cell.location_time = DateTime.UtcNow;

                            // check if the position fulfilled the requirements  for LoC
                            if (cell.level_confidence == -1)
                            {
                                // message didn't contained any level of confidence information
                            }
                            else if (cell.level_confidence < ACCEPTED_LEVEL_OF_CONFIDENCE)
                            {
                                cell.lat = "0";
                                cell.lng = "0";
                                cell.spd = "0";
                                cell.location_time = DateTime.UtcNow;
                            }

                            try
                            {

                                // update last GPS list if the unit exists in the hashtable
                                if (cell.suid!= null && SN_Queues.ht_SUInfo.ContainsKey(cell.suid))
                                {
                                    ((SUinfo)SN_Queues.ht_SUInfo[cell.suid]).GPSlasttime = DateTime.UtcNow;
                                }
                                else
                                {
                                    // break if received possition is not from a unit assigned to this gateway

                                    Utils.ConsWrite(DebugMSG_Type.GPS, "Got GPS position from " + cell.suid + " which is not assigned to this GW.");
                                    return;
                                }

                                if (cell.triggered)//if location is "triggered" then proceed as usual  
                                {
                                    //Utils.ConsWrite(DebugMSG_Type.DEV, "Got triggered location from" + cell.suid);
                                    if (cell.suid != null && ht_location_hash.ContainsKey(cell.suid))
                                    {
                                        if (gps_time != ((DateTime)ht_location_hash[cell.suid]))
                                        {
                                            //Console.WriteLine("SUID:" + cell.suid + "GPS time != last GPS time... Adding it to DB!!!");
                                            cell.location_time = gps_time;
                                            SN_Queues.DBQueueLocation.PostItem(cell);
                                            ht_location_hash.Remove(cell.suid);
                                            ht_location_hash.Add(cell.suid, gps_time);
                                            
                                            string consoleMsg = "";
                                            Utils.ConsWrite(DebugMSG_Type.GPS, consoleMsg = String.Format("»»» Position [{0:0.0000},{1:0.0000}] from {2}", Math.Round(Double.Parse(cell.lat), 4),
                                                                        Math.Round(Double.Parse(cell.lng), 4), cell.suid));

                                            LOGS.LOG(ConnectPlus_GW.cfg.gw_id + " " + consoleMsg);
                                            //LOGS.LOG("&GPS& " + "3.5.DecodePacket_Connect_plus(Byte[] data)(); Added to queue for DB");
                                        }
                                        else
                                        {
                                            //Utils.ConsWrite(DebugMSG_Type.GPS, "SUID:" + cell.suid + " GPS time already in DB ...time:" + gps_time);
                                        }
                                    }//if (location_hash.ContainsKey(cell.suid))

                                    else
                                    {
                                        //Utils.ConsWrite(DebugMSG_Type.GPS, "SUID:" + cell.suid + " First GPS adding it to DB!!!");
                                        cell.location_time = gps_time;
                                        SN_Queues.DBQueueLocation.PostItem(cell);
                                        ht_location_hash.Add(cell.suid, gps_time);
                                        string consoleMsg = "";
                                        Utils.ConsWrite(DebugMSG_Type.GPS, consoleMsg = String.Format("»»» Position [{0:0.0000},{1:0.0000}] from {2}'st", Math.Round(Double.Parse(cell.lat), 4),
                                                                    Math.Round(Double.Parse(cell.lng), 4), cell.suid));

                                        LOGS.LOG(ConnectPlus_GW.cfg.gw_id + " " + consoleMsg);
                                    }
                                }
                                    
                                /// cell. triggered = false => poll response was received
                                else
                                {
                                    string consoleMsg = "";
                                    Utils.ConsWrite(DebugMSG_Type.GPS, consoleMsg = String.Format("»»» Poll Res [{0:0.0000},{1:0.0000}] from {2} [{3}]", Math.Round(Double.Parse(cell.lat), 4),
                                                                        Math.Round(Double.Parse(cell.lng), 4), cell.suid, cell.seq_ID));

                                    try
                                    {
                                        lock (SN_Queues.ht_POLL_List.SyncRoot)
                                        {
                                            foreach (DictionaryEntry item in SN_Queues.ht_POLL_List)
                                            {
                                                if (((POLLmsg)item.Value).suid == cell.suid)
                                                {
                                                    ((POLLmsg)item.Value).response = cell.location_time;
                                                    ((POLLmsg)item.Value).lat = cell.lat;
                                                    ((POLLmsg)item.Value).lng = cell.lng;
                                                    ((POLLmsg)item.Value).speed = cell.spd;
                                                    SN_Queues.DBQueueLocation.PostItem(cell);
                                                    SN_Queues.recvPOLLQueue.PostItem((POLLmsg)item.Value);

                                                    LOGS.LOG(ConnectPlus_GW.cfg.gw_id + " " + consoleMsg);
                                                    break;
                                                }
                                            }
                                        }
                                    }
                                    catch (Exception ex)
                                    {
                                        Utils.ConsWrite(DebugMSG_Type.always, "[Location Thread] Could not process poll msg suid:" + cell.suid);
                                    }
                                }
                            }
                            catch (Exception ex)
                            {
                                Utils.ConsWrite(DebugMSG_Type.always, "[Location Thread]: " + ex.ToString());
                            }
                        }
                        else if (parsErrorBYte == (byte)Result_Codes_ENUM.PROTOCOL_ELEMENT_NOT_SUPPORTED)
                        {
                            // this is error is generated whenever a location request sent to a field radio contains 
                            // required information that are not supported by that station
                            //SendTriggeredLocationSTOP(cell.suid, cell.requestID);

                            // send Triggered Location Request which contains the level of confidence
                            reqID_send++;
                            if (reqID_send == Int32.MaxValue)
                                reqID_send = 1;
                            SendTriggeredLocationRequest(cell.suid, (uint)((SUinfo)SN_Queues.ht_SUInfo[cell.suid]).repInterval, reqID_send);
                        }
                    }
                    break;

                // -----------------------------------------------------------------------------------------------------------------------
                default:
                    Utils.ConsWrite(DebugMSG_Type.GPS, "UNDEFINED 0x" + data[0].ToString("X"));
                    break;
            }
        }


        void ProcessCommand(MotoTRBOcmdMsg p_msg)
        {
            try
            {                
                reqID_send++;
                if (reqID_send == Int32.MaxValue)
                    reqID_send = 1;

                // hijack the reqID to my default value
                //reqID_send = REQ_ID;

                p_msg.m_seqID = reqID_send;
                p_msg.m_time = DateTime.Now;
                if (p_msg.m_cmd == (byte)MotoTRBOcmd.SET_REPORT_INTERVAL)
                {
                    // this is error is generated whenever a location request sent to a field radio contains 
                    // required information that are not supported by that station
                    //SendTriggeredLocationSTOP(p_msg.m_suid, REQ_ID);

                    // send a poll request with the default request ID
                    SendTriggeredLocationRequest(p_msg.m_suid, Convert.ToUInt32(p_msg.m_payload), reqID_send);
                    //SendTriggeredLocationRequest(p_msg.m_suid, Convert.ToUInt32(p_msg.m_payload), reqID_send); 
                }
                if (p_msg.m_cmd == (byte)MotoTRBOcmd.SEND_POLL)
                {
                    // send a poll request with the default request ID
                    SendPollLocationRequest(p_msg.m_suid, reqID_send);
                    //SendPollLocationRequest(p_msg.m_suid, reqID_send);
                    Utils.ConsWrite(DebugMSG_Type.GPS, "Poll request sent for unit:" + p_msg.m_suid);
                    //Utils.ConsWrite(DebugMSG_Type.DEV, "Poll request sent for unit:" + p_msg.m_suid);
                    lock (SN_Queues.ht_POLL_List.SyncRoot)
                    {
                        foreach (DictionaryEntry item in SN_Queues.ht_POLL_List)
                        {

                            if (((POLLmsg)item.Value).suid == p_msg.m_suid && ((POLLmsg)item.Value).DBid.ToString() == p_msg.m_payload)
                            {
                                ((POLLmsg)item.Value).sent = DateTime.Now.ToUniversalTime();
                                ((POLLmsg)item.Value).requestID = reqID_send;
                                Utils.ConsWrite(DebugMSG_Type.DEV, "Poll request found in MotoTRBOGW.ht_POLL_List for unit:" + p_msg.m_suid);
                                Utils.ConsWrite(DebugMSG_Type.DEV, "reqID_send:" + reqID_send);
                                SN_Queues.sentPOLLQueue.PostItem((POLLmsg)item.Value);
                                break;
                            }
                        }
                    }
                }
            }
            catch (Exception e)
            {
                Utils.ConsWrite(DebugMSG_Type.always, "Could not send Location Request to unit");
                Utils.ConsWrite(DebugMSG_Type.always, e.Message);
            }

        }


        /// <summary>
        /// Request a field radio to report its location on a periodical basis. The location will contain Level Of Confidence item
        /// </summary>
        /// <param name="SUID">The field radio ID which needs to send its location</param>
        /// <param name="p_reportSec">The number of seconds at which the field radio will resend its position</param>
        /// <param name="reqID">The triggered location ID for this location request</param>
        public void SendTriggeredLocationRequestwithLoC(string SUID, uint p_reportSec, Int32 reqID)
        {
            byte[] arr_radioID = Utils.ID2ByteARR("12", SUID);

            //get reqid bytes
            byte[] intBytes = BitConverter.GetBytes(reqID);
            if (BitConverter.IsLittleEndian)
                Array.Reverse(intBytes);

            Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Triggered_Location_Request_NoCDT, 
                                    0x12, // length in Bytes
                                    (byte)Common_Element_Tokens_ENUM.request_id, 
                                    0x04, intBytes[0], intBytes[1], intBytes[2], intBytes[3], 
                                    (byte)Query_Request_Messages_Tokens_ENUM.SU_IPv4,
                                    arr_radioID[0],
                                    arr_radioID[1],
                                    arr_radioID[2],
                                    arr_radioID[3],
                                    (byte)Query_Request_Messages_Tokens_ENUM.ret_info1,                                     
                                    (byte)Query_Request_Messages_Tokens_ENUM.request_speed_hor,
                                    (byte)Query_Request_Messages_Tokens_ENUM.request_lev_conf,      // level of confidence
                                    (byte)0x00,
                                    (byte)Query_Request_Messages_Tokens_ENUM.periodic_trigger, 
                                    (byte)Query_Request_Messages_Tokens_ENUM.interval, 
                                    0x00, 0x00 //(byte)report_time// in seconds                    
            };

            int report_time = (int)p_reportSec;

            if (report_time < 0x80)
            {
                //sendBytes[10] = (byte)report_time;
                sendBytes[19] = (byte)report_time;
            }
            else
            {
                sendBytes[1] += 1; // adjust length

                int process = report_time;                   // MSB
                process >>= 7;
                process &= 0x007F;
                process |= 0x0080;

                sendBytes[19] = (byte)process;
                sendBytes[20] = (byte)(report_time & 0x007F); //LSB
            }

            bool result = SendOnUDP(sendBytes, sendBytes.Length);
            string consoleMessage = String.Format("{0} Triggered location request LoC [{1}] for {2}", (result ? "««« Sent" : "ooo Not Sent "), p_reportSec, SUID);
            
            LOGS.LOG(ConnectPlus_GW.cfg.gw_id + " " + consoleMessage + " [" + reqID + "]");

            Utils.ConsWrite(DebugMSG_Type.DB, consoleMessage);
        }


        /// <summary>
        /// Request a field radio to report its location on a periodical basis
        /// </summary>
        /// <param name="SUID">The field radio ID which needs to send its location</param>
        /// <param name="p_reportSec">The number of seconds at which the field radio will resend its position</param>
        /// <param name="reqID">The triggered location ID for this location request</param>
        public void SendTriggeredLocationRequest(string SUID, uint p_reportSec,Int32 reqID)
        {
            byte[] arr_radioID = Utils.ID2ByteARR("12", SUID); 
           
            //get reqid bytes
            byte[] intBytes = BitConverter.GetBytes(reqID);
            if (BitConverter.IsLittleEndian)
                Array.Reverse(intBytes);

            Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Triggered_Location_Request_NoCDT, 
                                    0x10, // length in Bytes
                                    (byte)Common_Element_Tokens_ENUM.request_id, 
                                    0x04, 0x00, 0x00, intBytes[2], intBytes[3], 
                                    (byte)Query_Request_Messages_Tokens_ENUM.SU_IPv4,
                                    arr_radioID[0],
                                    arr_radioID[1],
                                    arr_radioID[2],
                                    arr_radioID[3],
                                    (byte)Query_Request_Messages_Tokens_ENUM.ret_info1,                                   
                                    (byte)Query_Request_Messages_Tokens_ENUM.request_speed_hor,

                                    (byte)Query_Request_Messages_Tokens_ENUM.periodic_trigger, 
                                    (byte)Query_Request_Messages_Tokens_ENUM.interval, 
                                    0x00, 0x00 //(byte)report_time// in seconds                    
            };

            int report_time = (int)p_reportSec;

            if (report_time < 0x80)
            {
                //sendBytes[10] = (byte)report_time;
                sendBytes[17] = (byte)report_time;
            }
            else
            {
                sendBytes[1] += 1; // adjust length

                int process = report_time;                   // MSB
                process >>= 7;
                process &= 0x007F;
                process |= 0x0080;

                sendBytes[17] = (byte)process;
                sendBytes[18] = (byte)(report_time & 0x007F); //LSB
            }

            bool result = SendOnUDP(sendBytes, sendBytes.Length);
            string consoleMessage = String.Format("{0} Triggered location request [{1}] for {2}", (result ? "««« Sent" : "ooo Not Sent "), p_reportSec, SUID);
            LOGS.LOG(ConnectPlus_GW.cfg.gw_id + " " + consoleMessage + " [" + reqID + "]");

            Utils.ConsWrite(DebugMSG_Type.DB, consoleMessage);
        }


        /// <summary>
        /// Request a field radio to report immediate its location [also know as poll]
        /// </summary>
        /// <param name="SUID">The field radio ID which needs to send imeediate its location</param>
        /// <param name="reqID">The triggered location ID for this poll request</param>
        public void SendPollLocationRequest(string SUID, Int32 reqID)
        {
            byte[] arr_radioID = Utils.ID2ByteARR("12", SUID);
            //get reqid bytes
            byte[] intBytes = BitConverter.GetBytes(reqID);
            if (BitConverter.IsLittleEndian)
                Array.Reverse(intBytes);

            Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Immediate_Location_Request_NoCDT, 
                                    0x0D, // length in Bytes
                                    (byte)Common_Element_Tokens_ENUM.request_id, 
                                    0x04, intBytes[0], intBytes[1], intBytes[2], intBytes[3],
                                    (byte)Query_Request_Messages_Tokens_ENUM.SU_IPv4,
                                    arr_radioID[0],
                                    arr_radioID[1],
                                    arr_radioID[2],
                                    arr_radioID[3],
                                    0x51, 
                                    (byte)Query_Request_Messages_Tokens_ENUM.request_speed_hor                  
            };

            bool result = SendOnUDP(sendBytes, sendBytes.Length);
            string consoleMessage = String.Format("{0} POLL location request for {1} [{2}]", (result ? "««« Sent" : "ooo Not Sent "), SUID, reqID);
            LOGS.LOG(ConnectPlus_GW.cfg.gw_id + " " + consoleMessage + " [" + reqID + "]");

            Utils.ConsWrite(DebugMSG_Type.DB, consoleMessage);
        }


        /// <summary>
        /// Request the stop of a Triggered Location from a field radio
        /// </summary>
        /// <param name="SUID">The field radio ID which needs to stop sending location at a schedule time</param>
        /// <param name="reqID">The triggered location ID that needs to be stopped [each field radio can have 4 Triggered Locations]</param>
        public void SendTriggeredLocationSTOP(string SUID, byte reqID)
        {
            byte[] arr_radioID = Utils.ID2ByteARR("12", SUID);
            Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Request_NoCDT, 
                                    0x0B, // length in Bytes
                                    (byte)Common_Element_Tokens_ENUM.request_id, 
                                    0x04, 0x00, 0x00, 0x00, reqID, 
                                    (byte)Query_Request_Messages_Tokens_ENUM.SU_IPv4,
                                    arr_radioID[0],
                                    arr_radioID[1],
                                    arr_radioID[2],
                                    arr_radioID[3]                 
            };

            bool result = SendOnUDP(sendBytes, sendBytes.Length);
            string consoleMessage = String.Format("{0} Triggered location STOP request for {1} [{2}]", (result ? "««« Sent" : "ooo Not Sent "), SUID, reqID);
            LOGS.LOG(ConnectPlus_GW.cfg.gw_id + " " + consoleMessage);

            Utils.ConsWrite(DebugMSG_Type.DB, consoleMessage);
        }


        /// <summary>
        /// Send a byte array on UDP
        /// </summary>
        /// <param name="sendBytes">The byte array which needs to be sent on UDP</param>
        /// <param name="length"></param>
        /// <returns></returns>
        private bool SendOnUDP(byte[] sendBytes, int length)
        {
            try
            {
                UdpClient udpClient = new UdpClient(ConnectPlus_GW.cfg.locPort_r);
                //udpClient.Connect(ConnectPlus_GW.cfg.ctrlIP, ConnectPlus_GW.cfg.locPort);

                IPAddress ctrIP = null;
                if (!IPAddress.TryParse(ConnectPlus_GW.cfg.ctrlIP, out ctrIP))
                {
                    ctrIP = Dns.GetHostAddresses(ConnectPlus_GW.cfg.ctrlIP)[0];
                }
                if (ctrIP == null)
                {
                    Utils.ConsWrite(DebugMSG_Type.always, "[Location Thread] Invalid host address for:  " + ConnectPlus_GW.cfg.ctrlIP);
                }
                
                udpClient.Send(sendBytes, length, new IPEndPoint(ctrIP, ConnectPlus_GW.cfg.locPort));
                udpClient.Close();
                udpClient = null;

                return true;
            }
            catch (Exception ex)
            {
                Utils.ConsWrite(DebugMSG_Type.always, "[Location Thread] Send on UDP exception: " + ex.ToString());
                return false;
            }
        }


        #region AUXILIARY FUNCTIONS

        /// <summary>
        /// Convert a received byte array to a string representation of hex elements
        /// </summary>
        /// <param name="data">The byte array that needs to be converted</param>
        /// <returns>A hex based representation of the byte array</returns>
        static public string Byte2String(byte[] data)
        {
            return Byte2String(data, 0, data.Length);
        }


        /// <summary>
        /// Convert a portion of a byte array to a string representation of hex elements
        /// </summary>
        /// <param name="data">The byte array that needs to be converted<</param>
        /// <param name="startIndex">Index from which the hex conversion begins</param>
        /// <param name="len">The number of bytes which will be converted</param>
        /// <returns>A hex based representation of the byte array</returns>
        static public string Byte2String(byte[] data, int startIndex, int len)
        {
            string sdata = "";
            int i;

            if (startIndex > data.Length)
                return "";

            for (i = startIndex; i < startIndex + len && i < data.Length; i++)
            {
                int ii;
                ii = (int)data[i];
                sdata += "0x" + ii.ToString("X") + " ";
            }
            return sdata;
        }


        /// <summary>
        /// Return a human readable error based on the error received OTA from the field radio
        /// </summary>
        /// <param name="err">Error received from the field radio</param>
        /// <returns>A human readable error name</returns>
        static string ProcessError(Byte err)
        {
            switch (err)
            {
                case (byte)Result_Codes_ENUM.SUCCESS: return "" + Result_Codes_ENUM.SUCCESS;
                case (byte)Result_Codes_ENUM.UNSUPPORTED_VERSION: return "" + Result_Codes_ENUM.UNSUPPORTED_VERSION;
                case (byte)Result_Codes_ENUM.SYNTAX_ERROR: return "" + Result_Codes_ENUM.SYNTAX_ERROR;
                case (byte)Result_Codes_ENUM.PROTOCOL_ELEMENT_NOT_SUPPORTED: return "" + Result_Codes_ENUM.PROTOCOL_ELEMENT_NOT_SUPPORTED;
                case (byte)Result_Codes_ENUM.PROTOCOL_ELEMENT_VALUE_OUT_OF_RANGE: return "" + Result_Codes_ENUM.PROTOCOL_ELEMENT_VALUE_OUT_OF_RANGE;
                case (byte)Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE: return "" + Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE;
                case (byte)Result_Codes_ENUM.NO_SUCH_REQUEST: return "" + Result_Codes_ENUM.NO_SUCH_REQUEST;
                case (byte)Result_Codes_ENUM.DUPLICATE_REQUEST_ID: return "" + Result_Codes_ENUM.DUPLICATE_REQUEST_ID;
            }
            return "Unknown err";

        }


        /// <summary>
        /// Convert a byte array to a double value
        /// </summary>
        /// <param name="data">The byte array which contains the double value</param>
        /// <param name="startIndex">The index from which the double value is stored</param>
        /// <returns>The double value that resides in the byte array</returns>
        static public double ProcessUFloat2B(Byte[] data, int startIndex)
        {
            int aux = data[startIndex];
            aux &= 0xff;
            int int_part = 0;
            int i=0;

            // get the int part
            while ((aux & 0x80) != 0)
            {
                //Console.Write("<" + aux.ToString("X") + ">");
                int_part |= (aux ^ 0x80);
                int_part <<= 7;
                i++;
                aux = data[startIndex+i];
                aux &= 0xff;
                //Console.Write("[" + int_part.ToString("X") + "]");
            }
            //Console.Write("<" + aux.ToString("X") + ">");
            int_part |= aux;
            //Console.Write("[" + int_part.ToString("X") + "]");

            // ignore the float part
            i++;
            while ((data[startIndex + i] & 0x80) != 0)
            {
                //Console.Write("<" + data[startIndex + i] + ">");
                i++;
            }
            return (double)int_part;
        }

        /// <summary>
        /// Convert a byte array to a dateTime value
        /// </summary>
        /// <param name="data">The byte array which contains the dateTime value</param>
        /// <param name="startIndex">The index from which the dateTime value is stored</param>
        /// <param name="len">The number of bytes which repesents the dateTime value</param>
        /// <returns>The dateTime value that resides in the byte array</returns>
        static DateTime ProcessTime(Byte[] data, int startIndex, int len)
        {
            // (yyyy*2^26) + (MM*2^22) + (DD*2^17) + (hh*2^12) + (mm*2^6) + ss
            Byte B1 = data[startIndex];
            Byte B2 = data[startIndex + 1];
            Byte B3 = data[startIndex + 2];
            Byte B4 = data[startIndex + 3];
            Byte B5 = data[startIndex + 4];

            int hh, mm, ss, YY, MM, DD;
            ss = B5 & 0x3F; // remove first 2b

            mm = B4 & 0x0F;
            mm <<= 2;
            mm |= ((B5 >> 6) & 0x03);

            hh = B3 & 0x01;
            hh <<= 4;
            hh |= ((B4 & 0xf0) >> 4);

            DD = B3 & 0x3E;
            DD >>= 1;

            MM = B2 & 0x03;
            MM <<= 2;
            MM |= ((B3 >> 6) & 0x03);

            YY = B1;
            YY <<= 6;
            YY |= ((B2 >> 2) & 0x3F);

            //Console.WriteLine("YY=" + YY.ToString("X"));
            //Console.WriteLine("MM=" + MM.ToString("X"));
            //Console.WriteLine("DD=" + DD.ToString("X"));
            //Console.WriteLine("hh=" + hh.ToString("X"));
            //Console.WriteLine("mm=" + mm.ToString("X"));
            //Console.WriteLine("ss=" + ss.ToString("X"));

            //Utils.ConsWrite(DebugMSG_Type.GPS, "GPSTime : " + YY + "/" + MM + "/" + DD + " " + hh + ":" + mm + ":" + ss);
            return new DateTime(YY, MM, DD, hh, mm, ss, DateTimeKind.Utc);
        }


        /// <summary>
        /// Gets the Latitude from the byte array
        /// </summary>
        /// <param name="data">The byte array which contains the Latitude value</param>
        /// <param name="startIndex">The index from which the Latitude value is stored</param>
        /// <param name="len">The number of bytes which repesents the Latitude value</param>
        /// <returns>The Latitude value that resides in the byte array</returns>
        static double ProcessLat(Byte[] data, int startIndex, int len)
        {
            bool sign = false;
            if ((data[startIndex] & 0x80) != 0)
            {
                sign = true;
            }
            ulong l = (ulong)data[startIndex];
            if (sign)
                l ^= 0x80; // remove the sign

            l <<= 8;
            l |= (ulong)data[startIndex + 1];
            l <<= 8;
            l |= (ulong)data[startIndex + 2];
            l <<= 8;
            l |= (ulong)data[startIndex + 3];


            //Console.WriteLine("lat ulong=0x" + l.ToString("X"));

            double ld = (double)l;
            ld *= 90;
            ld /= 1024;
            ld /= 1024;
            ld /= 1024;
            ld /= 2;
            if (sign)
                ld *= -1;

            return ld;
        }


        /// <summary>
        /// Gets the Longitude from the byte array
        /// </summary>
        /// <param name="data">The byte array which contains the Longitude value</param>
        /// <param name="startIndex">The index from which the Longitude value is stored</param>
        /// <param name="len">The number of bytes which repesents the Longitude value</param>
        /// <returns>The Longitude value that resides in the byte array</returns>
        static double ProcessLng(Byte[] data, int startIndex, int len)
        {
            bool sign = false;
            if ((data[startIndex] & 0x80) != 0)
            {
                sign = true;
            }
            //Console.WriteLine("data[]=" + data[startIndex].ToString("X") + " sign=" + sign);
            ulong l = (ulong)data[startIndex];
            if (sign)
                l ^= 0x80; // remove the sign

            l <<= 8;
            l |= (ulong)data[startIndex + 1];
            l <<= 8;
            l |= (ulong)data[startIndex + 2];
            l <<= 8;
            l |= (ulong)data[startIndex + 3];


            //Console.WriteLine("lng ulong=0x" + l.ToString("X"));

            double ld = (double)l;
            ld *= 360;
            ld /= 1024;
            ld /= 1024;
            ld /= 1024;
            ld /= 4;

            if (sign)
            {
                ld = 180 - ld;
                ld *= -1;
            }
            return ld;
        }


        /// <summary>
        /// Gets the field radio ID from the byte array
        /// </summary>
        /// <param name="data">The byte array which contains the radioID value</param>
        /// <param name="startIndex">The index from which the radioID value is stored</param>
        /// <returns>The field radio ID value that resides in the byte array</returns>
        private int ProcessSU_ID(Byte[] data, int startIndex)
        {
            Byte B1 = data[startIndex];
            Byte B2 = data[startIndex + 1];
            Byte B3 = data[startIndex + 2];
            Byte B4 = data[startIndex + 3];

            int radioID = B4 + B3 * 256 + B2 * 256 * 256;
            return radioID;
        }


        /// <summary>
        /// Print a byte array on the console as a Hex representation
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <param name="inOut"></param>
        private static void Print(byte[] data, int length, bool inOut)
        {
            //Console.Clear();
            Console.WriteLine("--------------------------------------------------------------------------- " + length);
            Console.Write("Data (" + ((inOut) ? "RECEIVED" : "SENT") + "): ");
            for (int i = 0; i < length; i++)
                Console.Write(" 0x" + data[i].ToString("X2"));
            Console.WriteLine("");
            Console.WriteLine("--------------------------------------------------------------------------- ");
        }

        #endregion
    }
}