using System;
using System.Net;
using System.Net.Sockets;
using SafeMobileLib;
using System.Threading;
namespace MotoTrbo_GW
{
class TallysmanReceiveThread
{
private static UdpClient udpClient;
private static UdpMulticast udpMulticast;
private Int32 Port = 0;
private String mIP;
private Int32 mPort;
private static GeneralLocationManager locManager;
public TallysmanReceiveThread(Int32 port, String multicastID, String multicastPort)
{
Port = port;
mIP = multicastID;
mPort = Int32.Parse(multicastPort);
locManager = new GeneralLocationManager(mIP, mPort);
}
public void handleConnection()
{
SafeMobileLib.Utils.WriteLine("TallysmanReceiveThread initialized on port " + Port, ConsoleColor.Yellow);
udpClient = null;
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("TallysmanReceiveThread handleConnection exception: " + ex.ToString());
}
try
{
udpMulticast = new UdpMulticast(mIP, mPort);
//SafeMobileLib.Utils.WriteLine("TallysmanReceiveThread successfully registered to multicast group");
}
catch (Exception ex)
{
SafeMobileLib.Utils.WriteLine("TallysmanReceiveThread exception while joining the multicast group: " + ex.ToString());
}
IPEndPoint remoteIpEndPoint = new IPEndPoint(IPAddress.Any, Port);
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();
int seqNo = receivedBytes[0] * 256 + receivedBytes[1];
String seqID = TallysmanConfirm.getFromConfirmationQueue(seqNo);
SafeMobileLib.Utils.WriteLine(DateTime.Now.ToString() + " TallysmanReceiveThread received data from radio " + radioID + ", with seq ID " + seqID);
//Utils.printBytesArray(receivedBytes);
// process the message
Byte toreturn = ProcessPacket2(receivedBytes, receivedBytes.Length, radioID.ToString().Trim());
//put information on message bus
//string sequenceID = "";
//Byte[] toSendMulticast = Utils.createMulticastMessage(233, suid, receivedBytes, out sequenceID);
//udpMulticast.Send(toSendMulticast, toSendMulticast.Length);
SafeMobileLib.Utils.WriteLine("TallysmanReceiveThread successfully sent data to message bus");
Thread.Sleep(100);
}
catch (Exception e)
{
SafeMobileLib.Utils.WriteLine("##### TallysmanReceiveThread Exception #########\n" + e.ToString());
}
}
}
public static Byte ProcessPacket2(byte[] data, int len, String imei)
{
#region variable declaration
Byte numberofMessByte =0;
String pLat = "0.0", pLong = "0.0";
Int64 itime70 = 0;
double iSpeed = 0;
int altitude = 0;
#region fields for safenet db
int bearing = -1;
int levelOfConfidence = -1;
int accuracy_horizontal = -1;
int accuracy_vertical = -1;
int odometer = -1;
int vio_status = -1;
int vio_changed = -1;
float rssi = -1;
int vital_id = -1;
int runtime, idletime = -1;
int eventTime = -1;
long eventField = -1;
string firmware_version = string.Empty;
double average_speed = 0;
#endregion
#region correction length variables
int start_parse = 0;
int delta = 0;
int sum = 0;
#endregion
int[] v = new int[19];
int protocol, version, messageLength, messageType,
TransactionId, UnackedCount, LocationReportLength,
LogId, EventId, EventIndex;
Int32 step = 0;
#endregion
//first 2 bytes are Protocol and Version 0xFA 0x08
protocol = data[step++];
version = data[step++];
#region Message Length
string sByteMessLength = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sByteMessLength += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
messageLength = (int)(Convert.ToInt32(sByteMessLength, 2));
#endregion
#region Message Type
messageType = data[step++];
#endregion
#region Transaction ID
string sBytetrid = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sBytetrid += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
TransactionId = (int)(Convert.ToInt32(sBytetrid, 2));
#endregion
#region NOTE: Tallysman Reporting Types
// There are 3 types of reports: Autonomus report 0x03, Immediate response 0x02, Log retrieval report 0x05
//Between Transaction ID and Location report length foreach report type there are the following fields:
//A. Autonomus report 0x03 - Unacked cound
//B. Immediate response 0x02 - none
//C. Log retrieval report 0x05 - Retrieval Flags (1 byte), Returned count, Unreturned count, NextID all uintvar
#region Autonomus report 0x03 - Unacked cound
if (messageType == 3)
{
string sByteUnkCount = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sByteUnkCount += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
UnackedCount = (int)(Convert.ToInt32(sByteUnkCount, 2));
}
#endregion
#region Immediate response 0x02 - none
if (messageType == 2)
{
// do nothing
}
#endregion
#region Log retrieval report 0x05 - Retrieval Flags (1 byte), Returned count, Unreturned count, NextID all uintvar
if (messageType == 5)
{
//Retrieval Flags
step++;
//Returned count
do {/*nothing*/} while (GetBitsAsByte(data[step++], 7, 1) == 1);
//Unreturned count
do {/*nothing*/} while (GetBitsAsByte(data[step++], 7, 1) == 1);
//NextID
do {/*nothing*/} while (GetBitsAsByte(data[step++], 7, 1) == 1);
}
#endregion
#endregion
#region BUNDLE/NORMAL PARSER
do
{
#region Reset Variables
pLat = pLong = "0.0";
itime70 = 0;
iSpeed = 0;
altitude = 0;
//fields for safenet db
bearing = -1;
levelOfConfidence = -1;
accuracy_horizontal = -1;
accuracy_vertical = -1;
odometer = -1;
vio_status = -1;
vio_changed = -1;
rssi = -1;
vital_id = -1;
runtime = idletime = -1;
eventTime = -1;
eventField = -1;
firmware_version = string.Empty;
average_speed = 0;
#endregion
#region Location Report Length
string sByteLocRepLength = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sByteLocRepLength += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
LocationReportLength = (int)(Convert.ToInt32(sByteLocRepLength, 2));
start_parse = step;
#endregion
#region Log ID
string sByteLogId = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sByteLogId += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
LogId = (int)(Convert.ToInt32(sByteLogId, 2));
#endregion
#region Event ID
string sByteEventID = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sByteEventID += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
// The value of the Event ID is (Event Type ID + Event Index * 128) and is encoded as a uintvar
EventId = (int)(Convert.ToInt32(sByteEventID, 2))% 128;
EventIndex = (int)(Convert.ToInt32(sByteEventID, 2)) / 128;
//Check if eventId is periodic event Distance = 12, VIO = 14, Periodic = 15
int[] array = new int[] { 12, 14, 15 };
EventIndex += (Array.Exists(array, element => element == EventId)) ? 1 : 0;
#endregion
#region Report Form parser
int count = 0;
v = new int[19];
do
{
for (int i = 0; i < 7; i++)
{
v[count++] = (int)GetBitsAsByte(data[step], i, 1);
if (count == 19) break;
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
#endregion
#region Parse data based on report form's bits
for ( int i = 0; i <= 18; i++)
{
if (v[i] == 1)
{
switch (i)
{
//Property: Type, Form Flag + Name, Fixed/Variable Length, Length(bytes)
#region Type: Latitude and Longitude, F0 - latitude and longitude, Fixed Length 8 bytes total
case (int)FS.F0:
{
//process LAT
bool sign = false;
string hexLatitude = string.Empty;
if ((data[step] & 0x80) != 0)
{
hexLatitude = (data[step++] - 128).ToString("X2");
sign = true;
}
else hexLatitude += data[step++].ToString("X2");
for (int j = 0; j < 3; j++)
{
hexLatitude += data[step++].ToString("X2");
}
long decAgainLatiude = int.Parse(hexLatitude, System.Globalization.NumberStyles.HexNumber);
double dlat = decAgainLatiude * 90 / (double)Math.Pow(2, 31);
if (sign) dlat *= -1;
pLat = Convert.ToString(dlat);
//process LONG
string hexLongitude = string.Empty;
if ((data[step] & 0x80) != 0)
{
hexLongitude = "FFFFFFFF" + (data[step++]).ToString("X2");
}
else hexLongitude += data[step++].ToString("X2");
for (int j = 0; j < 3; j++)
{
hexLongitude += data[step++].ToString("X2");
}
long decAgainLongitude = Int64.Parse(hexLongitude, System.Globalization.NumberStyles.HexNumber);
double dlong = decAgainLongitude * 360 / (double)Math.Pow(2, 32);
pLong = Convert.ToString(dlong);
}
break;
#endregion
#region Type: Timestamp, F1 - gps fix time, Fixed Length 5 bytes
case (int)FS.F1:
{
string hexGPS = string.Empty;
for (int j = 0; j < 5; j++)
{
hexGPS += data[step++].ToString("X2");
}
decimal decAgainGPS = Int64.Parse(hexGPS, System.Globalization.NumberStyles.HexNumber);
int year = (int)Decimal.Truncate(decAgainGPS / (decimal)Math.Pow(2, 26));
decimal modYear = decAgainGPS % (decimal)Math.Pow(2, 26);
int month = (int)Decimal.Truncate(modYear / (decimal)Math.Pow(2, 22));
decimal modMonth = modYear % (decimal)Math.Pow(2, 22);
int day = (int)Decimal.Truncate(modMonth / (decimal)Math.Pow(2, 17));
decimal modDay = modMonth % (decimal)Math.Pow(2, 17);
int hour = (int)Decimal.Truncate(modDay / (decimal)Math.Pow(2, 12));
decimal modHour = modDay % (decimal)Math.Pow(2, 12);
int minute = (int)Decimal.Truncate(modHour / (decimal)Math.Pow(2, 6));
int second = (int)(modHour % (decimal)Math.Pow(2, 6));
System.DateTime dateTime = new DateTime(year, month, day, hour, minute, second, DateTimeKind.Utc);
TimeSpan diff = new DateTime(year, month, day, hour, minute, second, DateTimeKind.Utc) -
new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
DateTime d = new DateTime();
if (d.IsDaylightSavingTime())
{
SafeMobileLib.Utils.WriteLine("Time: Is daylight");
itime70 = Convert.ToInt32(diff.TotalSeconds) + 3600;
}
else
itime70 = Convert.ToInt32(diff.TotalSeconds);
}
break;
#endregion
#region Type: Speed, F2 - horizontal speed, F8 - vertical speed, F14 - average speed, Variable Length 1->3 bytes
case (int)FS.F2:
case (int)FS.F8:
case (int)FS.F14:
{
double speed = 0;
string sByte = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sByte += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
if (i == (int)FS.F2)
{
//meteres per second
speed = (double)(Convert.ToInt32(sByte, 2)) / 10;
//transform to km per hour
speed *= (double)3.6;
speed = (int)Math.Round(speed);
}
switch (i)
{
case (int)FS.F2:
iSpeed = speed;
break;
case (int)FS.F8:
break;
case (int)FS.F14:
average_speed = speed;
break;
}
}
break;
#endregion
#region Type: Sintvar, F4 - altitude, Variable Length 1->3 bytes
case (int)FS.F4:
{
string sByte = string.Empty;
bool sign = false;
do
{
for (int j = 6; j >= 0; j--)
{
if (GetBitsAsByte(data[step], 7, 1) == 1 && GetBitsAsByte(data[step], j, 1) == 1 && j == 6)
sign = true;
else
sByte += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
altitude = (int)(Convert.ToInt32(sByte, 2));
if (sign) altitude *= -1;
}
break;
#endregion
#region Type: Uint8, F5 - confidence level, Fixed Length 1 byte
case (int)FS.F5:
levelOfConfidence = data[step++];
break;
#endregion
#region Type: Uint16, F10 - odometer, F17 - RSSI radio signal strength in dBm
case (int)FS.F10:
// Odometer on 2 bytes - ignoring
step += 2;
break;
case (int)FS.F17:
// msb - most significant bit, lsb - least significant bit
int msb = data[step++];
int lsb = data[step++];
rssi = (msb + (lsb * 1000 + 128) / 256000);
break;
#endregion
#region Type: Uint16 F12 - VIO current / VIO changed 4 bytes 2 for current, 2 for changed
case (int)FS.F12:
//step += 4;
{
//2 bytes for current VIO
//Hardware VIO 1 Sprite Protocol page 67/68
int currentVIO = data[step++];
vio_status = currentVIO;
//Hardware VIO 2
step++;
//2 bytes for changed VIO
vio_changed = data[step++];
step++;
//send telemetry to message bus only if event type <> 1
string test = "#233#" + imei.ToString() + "#" + Convert.ToInt16(currentVIO) + "#hyt#";
locManager.SendOnMsgBuss("0.0", test);
}
break;
#endregion
#region Type: RTM + ITM
// F11 - Run time/idle time
case (int)FS.F11:
{
string sByte = string.Empty;
#region runtime
//Indicates the run time in minutes. The run time is the current value of a timer that
//continuously increments whenever a specified input is detected (e.g. ignition-on). The
//ignition line from the vehicle must be wired to the Sprite.
do
{
for (int j = 6; j >= 0; j--)
{
sByte += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
runtime = (int)(Convert.ToInt32(sByte, 2));
#endregion
#region idletime
//Indicates the idle time in minutes. The idle time is the current value of a timer that
//continuously increments whenever a specified input is detected (e.g. ignition-on) and
//the unit is not moving (i.e. stopped). The ignition line from the vehicle must be wired to
//the Sprite.
sByte = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sByte += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
idletime = (int)(Convert.ToInt32(sByte, 2));
#endregion
}
break;
#endregion
#region Type: Uintvar, F3, F6, F7, F9, F13, F18
// F3 - bearing, F6 - horizontal accuracy, F7 - vertical accuracy, F9 - odometer,
// F13 - event time, F18 - vital ID
case (int)FS.F3:
case (int)FS.F6:
case (int)FS.F7:
case (int)FS.F9:
case (int)FS.F13:
case (int)FS.F18:
{
string sByte = string.Empty;
do
{
for (int j = 6; j >= 0; j--)
{
sByte += ((int)GetBitsAsByte(data[step], j, 1)).ToString();
}
}
while (GetBitsAsByte(data[step++], 7, 1) == 1);
switch (i)
{
case (int)FS.F3:
bearing = (int)(Convert.ToInt32(sByte, 2));
break;
case (int)FS.F6:
accuracy_horizontal = (int)(Convert.ToInt32(sByte, 2));
break;
case (int)FS.F7:
accuracy_vertical = (int)(Convert.ToInt32(sByte, 2));
break;
case (int)FS.F9:
odometer = (int)(Convert.ToInt32(sByte, 2));
break;
case (int)FS.F13:
// The number of seconds that have passed since the GPS fix time for the report.
// If the GPS fix is old, then the GPS fix time will not be the time that the report was generated.
// This field, in combination with the GPS fix time field, allows the time of the event to be calculated.
// The GPS fix time should always be requested if the Event time is requested.
eventTime = (int)(Convert.ToInt32(sByte, 2));
break;
case (int)FS.F18:
vital_id = (int)(Convert.ToInt32(sByte, 2));
break;
}
}
break;
#endregion
#region 4 bytes FS: F15 - event field
//for (int j = 0; j < 4; j++)
//{
// eventField <<= 8;
// eventField |= (Int64)data[step];
// //Console.Write(" 0x" + data[step].ToString("X2"));
// step++;
//}
#endregion
#region 6 bytes FS: F16-firmware version, configuration version, configuration update count
case (int)FS.F16:
int major_version = data[step++];
int minor_version = data[step++];
int micro_version = data[step++];
firmware_version = string.Format("{0}.{1}.{2}", major_version, minor_version, micro_version);
step += 3;
break;
#endregion
}
}
}
#endregion
//send location to message bus
if (itime70 == 0 && EventId == (int)TallysmanEvents.ImmediateRequest) //immediate response
itime70 = DateTime.UtcNow.GetSecondsFromDT();
locManager.SendLoc2messagebusTallysman(imei, (UInt32)(itime70), iSpeed.ToString(), pLat, pLong, altitude.ToString(), LogId.ToString(), 0);
#region generate fake ars message if event is IgnitionOff, RadioOff, IgnitionOn, RadioOn
//2.4 ARS REPLACEMENT FOR TW200, TW250 ............................................... 10
string sequenceID = "";
if (EventId == (int)TallysmanEvents.IgnitionOff || EventId == (int)TallysmanEvents.RadioOff)
{
byte[] arsStatus = new byte[] { (byte)'O', (byte)'F', (byte)'F' };
Byte[] toSendMulticast = Utils.createMulticastMessage(130, imei, arsStatus, out sequenceID);
udpMulticast.Send(toSendMulticast, toSendMulticast.Length);
}
if (EventId == (int)TallysmanEvents.IgnitionOn || EventId == (int)TallysmanEvents.RadioOn)
{
byte[] arsStatus = new byte[] { (byte)'O', (byte)'N' };
Byte[] toSendMulticast = Utils.createMulticastMessage(130, imei, arsStatus, out sequenceID);
udpMulticast.Send(toSendMulticast, toSendMulticast.Length);
}
#endregion
#region generate fake emergency
if (EventId == (int)TallysmanEvents.Emergency)
{
SafeMobileLib.Utils.WriteLine("Received emergency from radio id: " + imei);
string seqID = "0.0";
string toSend = "#138#" + imei + "#";
String cmdok = "#" + seqID + toSend;
Int32 tmp = cmdok.Length + 1;
tmp += tmp.ToString().Length;
cmdok = "#" + tmp.ToString() + cmdok;
System.Text.Encoding enc = System.Text.Encoding.ASCII;
byte[] buf = enc.GetBytes(cmdok);
//put on multicast bus
SafeMobileLib.Utils.WriteLine("Emergency alarm sent on multicast bus: " + cmdok);
udpMulticast.Send(buf, buf.Length);
}
#endregion
sum = LocationReportLength + start_parse;
delta = sum - step;
step += delta;
}
while (len > step + 1);
#endregion
return numberofMessByte;
}
///
/// Gets a specific group of bits as a byte values. This process is created appling a
/// custom mask and then shifting the remaining bits to right
///
/// The byte from which the bits will be extracted
/// The number of bits which will be skipped from right[LST]. O for no offset
/// Number of bits which are required
/// The byte value of the required bits. This values is generated shifting the bits towards LST
public static byte GetBitsAsByte(byte b, int offset, int count)
{
return (byte)((b >> offset) & ((1 << count) - 1));
}
public enum FS
{
F0 = 0, // LAT + LON "Latitude Longitude"
F1, // FTM "Time Timestamp"
F2, // HSP "Speed*"
F3, // BEA "uintvar"
F4, // ALT "sintvar"
F5, // CNF "uint8"
F6, // HAC "uintvar"
F7, // VAC "uintvar"
F8, // VSP "Speed*"
F9, // ODU "uintvar"
F10, // OD2 "unsigned 16 bit integer, scaled by 10"
F11, // RTM + ITM "uintvar"
F12, // VCU + VCH "Bitmask (2 bytes)"
F13, // ETM "uintvar"
F14, // ASP "Speed*"
F15, // EVF "uint32 A four byte length unsigned integer."
F16, // FWV + CFV + CFU 3 concatenated uintvars + uintvar + uintvar
F17, // RSS "2 bytes"
F18 // VID "uintvar"
// *Speed is represented in meters per second and is encoded with one decimal point precision
// *by scaling the speed by 10 and encoding it as a uintvar.
}
public enum TallysmanEvents
{
ImmediateRequest = 1,
PTT = 2,
Turn = 3,
Speeding = 4,
IgnitionOn = 5,
IgnitionOff = 6,
RadioOn = 7,
RadioOff = 8,
Stopped = 9,
Moving = 10,
GPSFix = 11,
GPSNoFix = 12,
Distance = 13,
VIO = 14,
Periodic = 15,
CircleWaypoint = 16,
Emergency = 17,
ChannelChange = 18,
Tracking = 19,
SafeArea = 20,
RSSICapture = 21,
UserInput = 22,
PolygonWaypointInEvent = 23,
PolygonWaypointOutEvent = 24,
CircleWaypointOutEvent = 25,
BusStopEvent = 26
}
}
}