using System; using System.Net; using System.Net.Sockets; using System.Threading; using MotoTRBO_GW; using SafeMobileLib; using SafeMobileLib.MessageDecoders; using System.Threading.Tasks; using System.Collections.Generic; namespace MotoTrbo_GW { class LocationThread { private static Int32 port; private byte[] data = new byte[1024]; private UdpMulticast udpMulticast; private static UdpClient udpClient; private String mIP; private Int32 mPort; private static byte reqID_lastByte=0; private Thread retryLocationRequest; private static Int32 numberOfPositions = 0; public static Int32 NumberOfPositions { get { return numberOfPositions; } set { numberOfPositions = value; numberOfPositionsSinceLastRead++; } } private static Int32 numberOfPositionsSinceLastRead = 0; public static Int32 NumberOfPositionsSinceLastRead { get { Int32 val = numberOfPositionsSinceLastRead; numberOfPositionsSinceLastRead = 0; return val; } set { numberOfPositionsSinceLastRead = value; } } private static Dictionary radioIdToLocReqTime = new Dictionary(); private static object lockDictionary = new object(); public LocationThread(Int32 locationPort, String multicastID, String multicastPort) { port = locationPort; mIP = multicastID; mPort = Int32.Parse(multicastPort); } private void RetryLocationRequestHandler() { while (Program.isRunning) { lock(lockDictionary) { Thread.Sleep(10); Dictionary copyDictionary = new Dictionary(radioIdToLocReqTime); foreach (KeyValuePair pair in copyDictionary) { if ((DateTime.Now - pair.Value.RequestTime).TotalSeconds > pair.Value.ReportingSeconds + 10) { SafeMobileLib.Utils.WriteLine($"Retrying Location Requests for {pair.Key}[{pair.Value.NrOfRetry}]", ConsoleColor.Yellow); sendTriggeredLocationReq(pair.Value.RadioID, pair.Value.ReportingSeconds); } if (pair.Value.NrOfRetry >= 3) { SafeMobileLib.Utils.WriteLine($"Stopping retrying Location Requests for {pair.Key}", ConsoleColor.Yellow); radioIdToLocReqTime.Remove(pair.Key); } } } } } public void handleConnection() { SafeMobileLib.Utils.WriteLine("Location Thread - Initialized on port " + port, ConsoleType.GPS); Task.Factory.StartNew(RetryLocationRequestHandler); try { udpClient = new UdpClient(); udpClient.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true); udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port)); } catch (Exception ex) { SafeMobileLib.Utils.WriteLine("LocationThread handleConnection exception while creating udpClient: " + ex.ToString(), ConsoleType.GPS); } try { udpMulticast = new UdpMulticast(mIP, mPort); SafeMobileLib.Utils.WriteLine("Location thread successfully registered to multicast group", ConsoleType.GPS); udpMulticast.OnNewDataRecv += new UdpMulticast.newData4Send(udpMulticast_OnNewDataRecv); udpMulticast.StartListen(Main.LocalIP); } catch (Exception ex) { SafeMobileLib.Utils.WriteLine("Location Thread exception while joining the multicast group: " + ex.ToString(), ConsoleType.GPS); } IPEndPoint remoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0); while (true) { try { // Blocks until a message returns on this socket from a remote host. Byte[] receivedBytes = udpClient.Receive(ref remoteIpEndPoint); char[] separator = { '.' }; string[] su = remoteIpEndPoint.Address.ToString().Split(separator); uint radioID = (Convert.ToUInt32(su[1])) * 256 * 256 + (Convert.ToUInt32(su[2])) * 256 + Convert.ToUInt32(su[3]); string suid = radioID.ToString(); SafeMobileLib.Utils.WriteLine("Location Thread received data from radio " + radioID, ConsoleColor.White, ConsoleType.GPS); //Utils.printBytesArray(receivedBytes); //put information on message bus try { LocationDecoder locDecoder = new LocationDecoder(suid, receivedBytes, Main.TESTMODE); try { Result_Codes_ENUM enumDisplayStatus = (Result_Codes_ENUM)locDecoder.result; string stringValue = enumDisplayStatus.ToString(); if(locDecoder.result != (byte)Result_Codes_ENUM.SUCCESS) SafeMobileLib.Utils.WriteLine($"Location Result code is {stringValue} for {radioID}", ConsoleColor.DarkYellow); } catch (Exception) { SafeMobileLib.Utils.WriteLine($"Location Result code is {locDecoder.result} for {radioID}", ConsoleColor.DarkYellow); } if (locDecoder.result == (byte)Result_Codes_ENUM.REPORTING_WILL_STOP || locDecoder.result == (byte)Result_Codes_ENUM.DUPLICATE_REQUEST_ID) { SafeMobileLib.Utils.WriteLine($"{(locDecoder.result == (byte)Result_Codes_ENUM.REPORTING_WILL_STOP ? "REPORTING_WILL_STOP" : "DUPLICATE_REQUEST_ID")} " + $"for {suid}!!!!!", ConsoleColor.Red); int requestingInterval = ARSThread.vehiclesManager.getReportingInterval(suid); if (requestingInterval < 5) { SafeMobileLib.Utils.WriteLine("Requesting interval for " + suid + " set to default value : 180 [found: " + requestingInterval + "]", ConsoleColor.DarkMagenta); requestingInterval = 180; } if (locDecoder.result == (byte)Result_Codes_ENUM.REPORTING_WILL_STOP) { SafeMobileLib.Utils.WriteLine($"Should request location updates again for {radioID}"); SendTriggeredLocationRequest(suid, requestingInterval); } else if (locDecoder.result == (byte)Result_Codes_ENUM.DUPLICATE_REQUEST_ID) { delayWaitingReponseForUnit(suid, requestingInterval); } } else if (locDecoder.result == (byte)Result_Codes_ENUM.SYNTAX_ERROR) { SafeMobileLib.Utils.WriteLine("SYNTAX_ERROR", ConsoleColor.Yellow); } else if (locDecoder.result == (byte)Result_Codes_ENUM.PROTOCOL_ELEMENT_NOT_SUPPORTED) { int requestingInterval = ARSThread.vehiclesManager.getReportingInterval(suid); if (requestingInterval < 5) { SafeMobileLib.Utils.WriteLine("Requesting interval for " + suid + " set to default value : 180 [found: " + requestingInterval + "]", ConsoleColor.DarkMagenta); requestingInterval = 180; } SafeMobileLib.Utils.WriteLine("PROTOCOL_ELEMENT_NOT_SUPPORTED " + (locDecoder.cell.isCSBK ? " [CSBK]" : "[LEV_OF_CONFIDENCE]")); try { Thread p = new Thread(delegate () { int coun = 0; while (Program.isRunning && (coun++) < 6) { Thread.Sleep(500); } // send Triggered Location Request which contains the level of confidence SendTriggeredLocationRequest(suid, requestingInterval); }); p.Start(); } catch (Exception) { } } else if(locDecoder.result == (byte)Result_Codes_ENUM.SUCCESS) { if (locDecoder.cell.lat == null) { int requestingInterval = ARSThread.vehiclesManager.getReportingInterval(suid); if (requestingInterval < 5) { SafeMobileLib.Utils.WriteLine("Requesting interval for " + suid + " set to default value : 180 [found: " + requestingInterval + "]", ConsoleColor.DarkMagenta); requestingInterval = 180; } delayWaitingReponseForUnit(suid, requestingInterval); } } if (locDecoder.cell.lat != null) { if (Main.TESTMODE) suid = locDecoder.RadioID; // remove the unit from the list of unanswered location triggers lock (lockDictionary) { if (radioIdToLocReqTime.ContainsKey(suid)) { SafeMobileLib.Utils.WriteLine($"Radio {suid} answered to location request, removing it for dictionary", ConsoleColor.Green); radioIdToLocReqTime.Remove(suid); } } // get the time difference between the GPS Position and the Global Time (which can be an internet time) TimeSpan locationDifference = (DateTime.Now.AddSeconds(MotoTrbo_GW.Main.timeDifference.TotalSeconds)).Subtract(locDecoder.cell.location_time); /* Console.ForegroundColor = ConsoleColor.Cyan; SafeMobileLib.Utils.WriteLine("CRT Time is " + DateTime.UtcNow.AddSeconds(MotoTrbo_GW.Main.timeDifference.TotalSeconds).ToString()); SafeMobileLib.Utils.WriteLine("Location Time is " + locDecoder.cell.location_time.ToString()); Console.ForegroundColor = ConsoleColor.Red; SafeMobileLib.Utils.WriteLine("POSITION DIFFERENCE IS " + locationDifference.Seconds); Console.ForegroundColor = ConsoleColor.Gray; */ // if position in the future with more than 2 days if (Math.Abs(locationDifference.TotalSeconds) > 172800) { // skip this position continue; } String[] toSendString = new String[4]; toSendString[0] = DateTo70Format(locDecoder.cell.location_time).ToString(); toSendString[1] = locDecoder.cell.spd.ToString(); toSendString[2] = locDecoder.cell.lat.ToString(); toSendString[3] = locDecoder.cell.lng.ToString(); // increment the number of positions received NumberOfPositions++; Int64 suId = 0; Int64.TryParse(suid, out suId); int speed = 0; int.TryParse(locDecoder.cell.spd, out speed); double lat = 0; double.TryParse(locDecoder.cell.lat, out lat); double lng = 0; double.TryParse(locDecoder.cell.lng, out lng); if (locDecoder.cell.triggered) { // trigger event if listener registered OnLocationReceived?.Invoke(suId, locDecoder.cell.location_time, speed, lat, lng); SafeMobileLib.Utils.WriteLine(String.Format("»»» Position [{0:0.0000},{1:0.0000}] from {2}", Math.Round(Double.Parse(locDecoder.cell.lat), 4), Math.Round(Double.Parse(locDecoder.cell.lng), 4), suid), ConsoleColor.White); Byte[] toSendMulticast = Utils.createLocationMessage(131, suid, toSendString); udpMulticast.Send(toSendMulticast, toSendMulticast.Length); //SafeMobileLib.Utils.WriteLine("Location thread successfully sent data to message bus: " + toSendString); } else { // trigger event if listener registered OnPollResponseReceived?.Invoke(suId, locDecoder.cell.location_time, speed, lat, lng, locDecoder.cell.seqID); SafeMobileLib.Utils.WriteLine(String.Format("»»» Poll Res [{0:0.0000},{1:0.0000}] from {2} [{3}]", Math.Round(Double.Parse(locDecoder.cell.lat), 4), Math.Round(Double.Parse(locDecoder.cell.lng), 4), suid, locDecoder.cell.seq_ID), ConsoleColor.White); Byte[] toSendMulticast = Utils.createLocationMessage(231, suid, toSendString); udpMulticast.Send(toSendMulticast, toSendMulticast.Length); //SafeMobileLib.Utils.WriteLine("Location thread->location poll response", ConsoleType.GPS); } } } catch (Exception ex) { SafeMobileLib.Utils.WriteLine("LocaltionThread [handleConnection]" + ex.ToString(), ConsoleColor.Red, ConsoleType.GPS); } Thread.Sleep(100); } catch (Exception e) { SafeMobileLib.Utils.WriteLine("##### LocationThread Exception #########\n" + e.ToString(), ConsoleType.GPS); } } } private void delayWaitingReponseForUnit(String radioId, int reportingInterval) { SafeMobileLib.Utils.WriteLine($"Should wait {reportingInterval + 15} seconds for location updates before requesting again for {radioId}", ConsoleColor.DarkYellow); lock (lockDictionary) { if (radioIdToLocReqTime.ContainsKey(radioId)) { radioIdToLocReqTime[radioId].NrOfRetry = 0; radioIdToLocReqTime[radioId].RequestTime = DateTime.Now.AddSeconds(reportingInterval + 15); } else { radioIdToLocReqTime.Add(radioId, new LocationRequest() { NrOfRetry = 0, RadioID = radioId, ReportingSeconds = reportingInterval, RequestTime = DateTime.Now.AddSeconds(75) }); } } } void udpMulticast_OnNewDataRecv(byte[] data, int dataLen) { try { string str = System.Text.Encoding.ASCII.GetString(data, 0, dataLen); String[] tempArray = str.Trim().Split('#'); if (tempArray.Length > 3) { if (tempArray[3].Equals("154")) { Int64 gwid_recv = Convert.ToInt64((tempArray[4].Split('.'))[0]); if (gwid_recv == Main.GWID) { SafeMobileLib.Utils.WriteLine("LocationThread received from multicast bus: " + str.Trim(), ConsoleType.GPS); string SUID = (tempArray[4].Split('.'))[2]; SafeMobileLib.Utils.WriteLine($"Poll location request for {SUID}", ConsoleColor.DarkCyan); SendPollRequest(SUID); } } } } catch (Exception ex) { SafeMobileLib.Utils.WriteLine(ex.ToString()); } } private uint DateTo70Format(DateTime param) { long nOfSeconds; //SafeMobileLib.Utils.WriteLine("DateTo70Format param=" + param); System.DateTime dt70 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc); TimeSpan span = param - dt70; nOfSeconds = (long)span.TotalSeconds; return ((uint)nOfSeconds); } private static void sendTriggeredLocationReq(string SUID, int report_time) { reqID_lastByte++; if (reqID_lastByte > 0xfe) reqID_lastByte = 0x00; reqID_lastByte = 0x05; Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Triggered_Location_Request_NoCDT, 0x0B, // length in Bytes (byte)Common_Element_Tokens_ENUM.request_id, //0x04, 0x24, 0x68, 0xAC, 0xE0, 0x04, 0x24, 0x68, 0xAC, reqID_lastByte, 0x51, (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 }; try { if (report_time < 0x80) { //sendBytes[10] = (byte)report_time; sendBytes[12] = (byte)report_time; } else { sendBytes[1] += 1; // adjust length int process = report_time; // MSB process >>= 7; process &= 0x007F; process |= 0x0080; sendBytes[12] = (byte)process; sendBytes[13] = (byte)(report_time & 0x007F); //LSB } udpClient.Client.SendTo(sendBytes, new IPEndPoint(IPAddress.Parse(Utils.ID2IP("12", SUID)), port)); //SafeMobileLib.Utils.WriteLine("Triggered location request sent to radio ID " + SUID, ConsoleType.GPS); SafeMobileLib.Utils.WriteLine("««« Triggered location request [" + report_time + " sec] for " + SUID, ConsoleColor.White, ConsoleType.GPS); //locationUdpClient.Close(); lock (lockDictionary) { if (radioIdToLocReqTime.ContainsKey(SUID)) { radioIdToLocReqTime[SUID].NrOfRetry++; radioIdToLocReqTime[SUID].RequestTime = DateTime.Now; } else { radioIdToLocReqTime.Add(SUID, new LocationRequest() { NrOfRetry = 0, RadioID = SUID, ReportingSeconds = report_time, RequestTime = DateTime.Now }); } } } catch (Exception ex) { SafeMobileLib.Utils.WriteLine("Location Thread SendTriggeredLocationRequest exception: " + ex.ToString(), ConsoleType.GPS); } } public static void SendTriggeredLocationRequest(string SUID, int report_time) { lock (lockDictionary) { // remove the previous requests because this is caused by a new ARS if (radioIdToLocReqTime.ContainsKey(SUID)) radioIdToLocReqTime.Remove(SUID); } // send the request sendTriggeredLocationReq(SUID, report_time); } public static void SendPollRequest(string SUID) { reqID_lastByte++; if (reqID_lastByte > 0xff) reqID_lastByte = 0x00; Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Immediate_Location_Request_NoCDT, 0x08, // length in Bytes (byte)Common_Element_Tokens_ENUM.request_id, //0x04, 0x24, 0x68, 0xAC, 0xE0, 0x04, 0x24, 0x68, 0xAC, reqID_lastByte, 0x51, //(byte)Query_Request_Messages_Tokens_ENUM.require_altitude}; (byte)Query_Request_Messages_Tokens_ENUM.request_speed_hor}; udpClient.Client.SendTo(sendBytes, new IPEndPoint(IPAddress.Parse(Utils.ID2IP("12", SUID)), port)); SafeMobileLib.Utils.WriteLine("««« POLL location request for " + SUID + " [" + reqID_lastByte + "]", ConsoleColor.White); //SafeMobileLib.Utils.WriteLine("POLL location request sent to radio ID " + SUID, ConsoleType.GPS); } public delegate void PollResponseReceived(Int64 radioID, DateTime locationTime, int speed, double latitude, double longitude, Int64 seqID); public event PollResponseReceived OnPollResponseReceived; public delegate void LocationReceived(Int64 radioID, DateTime locationTime, int speed, double latitude, double longitude); public event LocationReceived OnLocationReceived; } }