SafeNet/LMdirect_SOC/LMDirectParser.cs
2021-02-24 13:50:23 +02:00

453 lines
17 KiB
C#

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections;
using SafeNetLib;
namespace LMdirect_SOC
{
class LMDirectParser
{
private string time;
private string lat;
private string lng;
private string speed;
private string IMEI;
private int DImask = 0;
private Int32 IDLength = 0;
public Int64 ID = 0;
private Int32 IDTypeLength = 0;
private Int32 IDType = 0;
private Int32 ServiceType = 0;
private Int32 UpdateTime = 0;
public Int32 MessageType = 0;
public Int32 SequenceNr = 0;
public Int32 TimeofFix = 0;
public Double Lat = 0;
public Double Long = 0;
public Int32 Speed = 0;
public Byte ForACKMobidIDLength = 0x00;
public Byte[] ForACKMobidID;
public Byte ForACKMobidIDTypeLength = 0x00;
public Byte[] ForACKMobidType;
public Byte ForACKServiceType = 0x00;
public Byte ForACKMessageType = 0x00;
public Byte[] ForACKSequnceNumber;
private Double Alitude = 0;
private Int32 Heading = 0;
private Int32 Satellites = 0;
private Int32 FixStatus = 0;
private Int32 Carrier = 0;
private Int32 RSSI = 0;
private Int32 CommState = 0;
private Int32 HDOP = 0;
private Int32 Inputs = 0;
private Int32 UnitStatus = 0;
private Int32 EventIndex = 0;
private Int32 EventCode = 0;
private Int32 Accums = 0;
private Int32 Spare = 0;
private Int32 UserMessageRoute = 0;
private Int32 UserMessageID = 0;
private Int32 UserMessageLength = 0;
private String UserMessage = "";
private ArrayList AccumList = new ArrayList();
private Boolean parseSucceded = false;
public Boolean MsgWith_MobileID = false;
public Boolean MsgWith_MobileIDType = false;
public Boolean MsgWith_AuthWord = false;
public Boolean MsgWith_Routing = false;
public Boolean MsgWith_Forward = false;
public Boolean MsgWith_ResponseRedir = false;
public LMDirectParser(byte[] data, int len)
{
int offset = 0;
if (data == null || data.Length == 0)
{
Console.WriteLine("Message is null");
return;
}
//parse optional header
/*if (!((data[offset] & 0x83) != 0))
{
Console.WriteLine("Option Byte not found");
return;
}
else Console.WriteLine("Option Byte found");*/
if ((data[offset] & 0x01) == 1) MsgWith_MobileID = true;
if ((data[offset] & 0x02) == 2) MsgWith_MobileIDType = true;
if ((data[offset] & 0x04) == 4) MsgWith_AuthWord = true;
if ((data[offset] & 0x08) == 8) MsgWith_Routing = true;
if ((data[offset] & 0x10) == 16) MsgWith_Forward = true;
if ((data[offset] & 0x20) == 32) MsgWith_ResponseRedir = true;
offset++;
IDLength = data[offset];
ForACKMobidIDLength = data[offset];
//Console.WriteLine("Id lenght is :"+IDLength);
offset++;
ID=0;
Int32 step = 1;
if (((data[offset + IDLength - 1] % 16) == 0xF))
step = 0;
ForACKMobidID = new Byte[IDLength];
for (int i = offset; i < (offset+IDLength); i++)
{
ID += ((Int32)(data[i] / 16)) * (long)Math.Pow(10, 2 * (offset + IDLength - i - 1)+step);
ID += ((Int32)(data[i] % 16)) * (long)Math.Pow(10, 2 * (offset + IDLength - i - 1)+step-1);
ForACKMobidID[i-offset] = data[i];
}
//Console.WriteLine("ID is :"+ID);
IMEI = ID.ToString();
offset += IDLength;
IDTypeLength = data[offset];
ForACKMobidIDTypeLength = data[offset];
//Console.WriteLine("Id TYPE lenght is :" + IDTypeLength);
offset++;
IDType = 0;
ForACKMobidType = new Byte[IDTypeLength];
for (int i = offset; i < (offset + IDTypeLength); i++)
{
IDType += data[i] * (Int32)Math.Pow(256, offset + IDTypeLength - i - 1);
ForACKMobidType[i - offset] = data[i];
}
//Console.WriteLine("Mobid type is :" + IDType);
offset += IDTypeLength;
if (MsgWith_AuthWord) offset += 5;
if (MsgWith_Routing) offset += 9;
if (MsgWith_Forward) offset += 9;
if (MsgWith_ResponseRedir) offset += 7;
ServiceType = data[offset];
ForACKServiceType = data[offset];
//Console.WriteLine("Service type is :" + ServiceType);
offset++;
MessageType = data[offset];
ForACKMessageType = data[offset];
//Console.WriteLine("Message type is :" + MessageType);
offset++;
ForACKSequnceNumber = new Byte[2];
ForACKSequnceNumber[0] = data[offset];
ForACKSequnceNumber[1] = data[offset+1];
SequenceNr = data[offset] * 256 + data[offset + 1];
//Console.WriteLine("Sequence number is :" + SequenceNr);
offset += 2;
UpdateTime = data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3];
//Console.WriteLine("UpdateTime is :" + UpdateTime);
offset += 4;
if (MessageType != 10)
{
TimeofFix = data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3];
//Console.WriteLine("TimeTofix is :" + TimeofFix);
offset += 4;
Lat = data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3];
//Console.WriteLine("Lat is :" + Lat);
byte[] vector8 = { data[offset], data[offset + 1], data[offset + 2], data[offset + 3] };
int integerLat = ToInt(vector8);
float floatLat = ToDegres(integerLat);
Lat = (double)floatLat;
//Console.WriteLine("Lat: " + floatLat);
offset += 4;
Long = data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3];
//Console.WriteLine("Long is :" + Long);
byte[] vector3 = { data[offset], data[offset + 1], data[offset + 2], data[offset + 3] };
int integerLong = ToInt(vector3);
float floatLong = ToDegres(integerLong);
Long = (double)floatLong;
//Console.WriteLine("Long: " + floatLong);
offset += 4;
Alitude = data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3];
//Console.WriteLine("Altitude is :" + Alitude);
Alitude = Alitude / 100;
offset += 4;
Speed = data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3];
//Console.WriteLine("Speed is :" + Speed);
offset += 4;
Heading = data[offset] * 256 + data[offset + 1];
//Console.WriteLine("Heading is :" + Heading);
offset += 2;
Satellites = data[offset];
//Console.WriteLine("Satellites is :" + Satellites);
offset++;
FixStatus = data[offset];
//Console.WriteLine("Fixstatus is :" + Convert.ToString(FixStatus, 2).PadLeft(8, '0'));
offset++;
Carrier = data[offset] * 256 + data[offset + 1];
//Console.WriteLine("Carrier is :" + Carrier);
offset += 2;
RSSI = data[offset] * 256 + data[offset + 1];
//Console.WriteLine("RSSI is :" + RSSI);
offset += 2;
CommState = data[offset];
//Console.WriteLine("CommState is :" + Convert.ToString(CommState, 2).PadLeft(8, '0'));
offset++;
HDOP = data[offset];
//Console.WriteLine("HDOP is :" + HDOP);
offset++;
Inputs = data[offset];
//Console.WriteLine("Inputs is :" + Convert.ToString(Inputs, 2).PadLeft(8, '0'));
DImask = Inputs;
offset++;
UnitStatus = data[offset];
//Console.WriteLine("UnitStatus is :" + Convert.ToString(UnitStatus, 2).PadLeft(8, '0'));
offset++;
/*EventIndex = data[offset];
Console.WriteLine("EventIndex is :" + EventIndex);
offset++;
EventCode = data[offset];
Console.WriteLine("EventCode is :" + EventCode);
offset++;
Accums = data[offset];
Console.WriteLine("Accums is :" + Accums);
offset++;
Spare = data[offset];
Console.WriteLine("Spare is :" + Spare);
offset++;*/
UserMessageRoute = data[offset];
//Console.WriteLine("UserMessageRoute is :" + UserMessageRoute);
offset++;
UserMessageID = data[offset];
//Console.WriteLine("UserMessageID is :" + UserMessageID);
offset++;
UserMessageLength = data[offset] * 256 + data[offset + 1];
//Console.WriteLine("UserMessageLength is :" + UserMessageLength);
offset += 2;
//UserMessage = "";
//for (int i = offset; i < (offset + IDLength); i++)
//{
// UserMessage += data[i].ToString("X2") + " ";
//}
//Console.WriteLine("UserMessage is :" + UserMessage);
//offset += IDLength;
UserMessage = "";
if (UserMessageLength < len - offset)
{
for (int i = 0; i < UserMessageLength; i++)
{
UserMessage += data[offset].ToString("X2") + " ";
offset++;
}
}
//Console.WriteLine("UserMessage is :" + UserMessage);
offset += IDLength;
}
else
{
Lat = data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3];
//Console.WriteLine("Lat is :" + Lat);
byte[] vector8 = { data[offset], data[offset + 1], data[offset + 2], data[offset + 3] };
int integerLat = ToInt(vector8);
float floatLat = ToDegres(integerLat);
Lat = (double)floatLat;
//Console.WriteLine("Lat: " + floatLat);
offset += 4;
Long = data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3];
//Console.WriteLine("Long is :" + Long);
byte[] vector3 = { data[offset], data[offset + 1], data[offset + 2], data[offset + 3] };
int integerLong = ToInt(vector3);
float floatLong = ToDegres(integerLong);
Long = (double)floatLong;
//Console.WriteLine("Long: " + floatLong);
offset += 4;
Heading = data[offset] * 256 + data[offset + 1];
//Console.WriteLine("Heading is :" + Heading);
offset += 2;
Speed = data[offset];
//Console.WriteLine("Speed is :" + Speed);
offset++;
FixStatus = data[offset];
//Console.WriteLine("Fixstatus is :" + Convert.ToString(FixStatus, 2).PadLeft(8, '0'));
offset++;
CommState = data[offset];
//Console.WriteLine("CommState is :" + Convert.ToString(CommState, 2).PadLeft(8, '0'));
offset++;
Inputs = data[offset];
//Console.WriteLine("Inputs is :" + Convert.ToString(Inputs, 2).PadLeft(8, '0'));
DImask = Inputs;
offset++;
EventCode = data[offset];
//Console.WriteLine("EventCode is :" + EventCode);
offset++;
Accums = data[offset];
//Console.WriteLine("Accums is :" + Accums);
offset++;
//AccumList
if (Accums < len - offset)
{
for (int i = 0; i < Accums; i++)
{
AccumList.Add(data[offset] * 256 * 256 * 256 + data[offset + 1] * 256 * 256 + data[offset + 2] * 256 + data[offset + 3]);
offset += 4;
}
}
}
//add data to location QUEUE
try
{
//locManager.SendLoc2messagebus(imei, (Int32)(itime70/1000), iSpeed.ToString(), pLat, pLong);
htCell_t cell = new htCell_t();
cell.suid = IMEI;
Speed = (int)(Speed * 0.036); //convert from cm/s to km/h
cell.spd = Speed.ToString();
cell.lat = Lat.ToString();
cell.lng = Long.ToString();
cell.d_lat = Lat;
cell.d_lng = Long;
cell.location_time = Utils.UnixTimeStampToDateTime(UpdateTime);
//cell.telemetryList = new List<Telemetry>(sourceTelemetry);
Utils.ConsWrite(DebugMSG_Type.GPS, "GPS for SUID: " + IMEI + " with time: " + cell.location_time + " lat: " + Lat + " lng: " + Long);
LOGS.LOG(DateTime.Now.ToString() + " : GPS for SUID: " + IMEI + " with time: " + cell.location_time + " lat: " + Lat + " lng: " + Long);
SN_Queues.DBQueueLocation.PostItem(cell);
Utils.ConsWrite(DebugMSG_Type.GPS, "Message added to queue!!!");
LOGS.LOG(DateTime.Now.ToString() + " : Message added to queue!!!");
//process ALERTS
//if (SN_Queues.ht_SUInfo.ContainsKey(IMEI))
//{
// Utils.ConsWrite(DebugMSG_Type.ALERTS, "Processing alerts for :" + IMEI);
// Utils.ConsWrite(DebugMSG_Type.ALERTS, "Message mask:" + DImask);
// SUinfo sui = (SUinfo)SN_Queues.ht_SUInfo[IMEI];
// foreach (Alert alert in sui.alertList)
// {
// if (alert.Type == Alert_TYPE.DI)
// if (alert.DImask1 == DImask)
// {
// alert.Alert_latitude = Lat.ToString();
// alert.Alert_longitude = Long.ToString();
// alert.Speed = Speed;
// alert.Digital_in = DImask;
// alert.Position_time = Utils.UnixTimeStampToDateTime(UpdateTime).ToString("yyyy:MM:dd HH:mm:ss");
// SN_Queues.alertQueue.PostItem(alert);
// Utils.ConsWrite(DebugMSG_Type.ALERTS, "Alert: " + alert.Alert_name + " added to queue");
// LOGS.LOG(DateTime.Now.ToString() + " : Alert: " + alert.Alert_name + " added to queue");
// }
// }
//}
}
catch (Exception ex)
{
Utils.ConsWrite(DebugMSG_Type.always, "ConnectionThread.cs =>> Error adding location to queue!!!");
Utils.ConsWrite(DebugMSG_Type.always, ex.ToString());
}
}
static byte Mirror(int input)
{
int returnval = 0;
int bit;
for (int i = 0; i < 7; ++i)
{
bit = input & 0x01;
returnval |= bit;
input >>= 1;
returnval <<= 1;
}
bit = input & 0x01;
returnval |= bit;
return (byte)returnval;
}
int ToInt(byte[] bytes)
{
if (BitConverter.IsLittleEndian)
Array.Reverse(bytes);
int i = BitConverter.ToInt32(bytes, 0);
return i;
}
uint ToUInt(byte[] bytes)
{
if (BitConverter.IsLittleEndian)
Array.Reverse(bytes);
uint i = BitConverter.ToUInt32(bytes, 0);
return i;
}
short ToShort(byte[] bytes)
{
if (BitConverter.IsLittleEndian)
Array.Reverse(bytes);
short i = BitConverter.ToInt16(bytes, 0);
return i;
}
ushort ToUShort(byte[] bytes)
{
if (BitConverter.IsLittleEndian)
Array.Reverse(bytes);
ushort i = BitConverter.ToUInt16(bytes, 0);
return i;
}
float ToDegres(int degree)
{
return (float)degree / 10000000;
}
}
}