public Packet(header_c C, header_m M0, header_m M1, state State, header_m M2)
{
this.C = C;
this._M[0] = M0;
this._M[1] = M1;
this._M[2] = M2;
this.State = State;
}
public Packet(header_c C, header_m M0, header_m M1, state State, header_m M2, wheel_o[] wheel_O, header_m M3)
{
this.C = C;
this._M[0] = M0;
this._M[1] = M1;
this._M[2] = M2;
this.State = State;
this.Wheel_O = wheel_O;
this._M[3] = M3;
}
//public void DisplayPacket(header_c C, state state)
//{
// Debug.Log("packet "+C.frameNo+ ": x position is"+ state.state_base.pos.x);
//}
public void DisplayPacket(header_c C)
{
Console.WriteLine(C.magicNo);
Console.WriteLine(C.version);
Console.WriteLine(C.headerSize);
Console.WriteLine(C.dataSize);
Console.WriteLine(C.frameNo);
Console.WriteLine(DateTime.FromOADate(C.simTime));
Console.WriteLine(Environment.NewLine);
}
public byte[] FormPacketArray(header_c header_C, header_m header_M0, header_m header_M1, state state, header_m header_M2)
{
byte[] a1 = serialization.Serialize(header_C);
byte[] a2 = serialization.Serialize(header_M0);
byte[] a3 = serialization.Serialize(header_M1);
byte[] a4 = serialization.Serialize(state);
byte[] a5 = serialization.Serialize(header_M2);
myArray = Combine(a1, a2, a3, a4, a5);
return(myArray);
}
public byte[] Serialize(header_c header_C)
{
List <byte> DataStream = new List <byte>();
DataStream.AddRange(BitConverter.GetBytes(header_C.magicNo));
DataStream.AddRange(BitConverter.GetBytes(header_C.version));
DataStream.AddRange(BitConverter.GetBytes(header_C.headerSize));
DataStream.AddRange(BitConverter.GetBytes(header_C.dataSize));
DataStream.AddRange(BitConverter.GetBytes(header_C.frameNo));
DataStream.AddRange(BitConverter.GetBytes(header_C.simTime));
return(DataStream.ToArray());
}
public Packet Parse(byte[] DataStream, int[] lens)
{
b = Split(DataStream, lens);
header_c _header_C = new header_c(b[0]);
header_m _header_M0 = new header_m(b[1]);
header_m _header_M1 = new header_m(b[2]);
state _state = new state(b[3]);
header_m _header_M2 = new header_m(b[4]);
Packet pkt = new Packet(_header_C, _header_M0, _header_M1, _state, _header_M2);
return(pkt);
}
public byte[] FormPacketArray(header_c header_C, header_m header_M0, header_m header_M1, state state, header_m header_M2, wheel_o[] wheel_O, header_m header_M3)
{
byte[] a1 = serialization.Serialize(header_C);
byte[] a2 = serialization.Serialize(header_M0);
byte[] a3 = serialization.Serialize(header_M1);
byte[] a4 = serialization.Serialize(state);
byte[] a5 = serialization.Serialize(header_M2);
byte[] a6 = serialization.Serialize(wheel_O[0]);
byte[] a7 = serialization.Serialize(wheel_O[1]);
byte[] a8 = serialization.Serialize(wheel_O[2]);
byte[] a9 = serialization.Serialize(wheel_O[3]);
byte[] a10 = serialization.Serialize(header_M3);
myArray = Combine(a1, a2, a3, a4, a5, a6, a7, a8, a9, a10);
return(myArray);
}
public static byte[] CatchPacket(double simTime, UInt32 id, geo geo, coord pos, coord speed, coord accel, UInt32 counter, wheel_o[] wheel_O)
{
Packet pkt = new Packet();
header_m[] header_M = new header_m[4];
header_c header_C;
state_o state_O = new state_o(id, 1, 1, 0x6, hp.myname, geo, pos, 0, 0, 0);
state_e state_E = new state_e(speed, accel, 498.55f, hp.spare);
state state = new state(state_O, state_E);
header_M[3] = new header_m(0, 0, 2, 0x0000);
header_M[2] = new header_m(176, 44, 14, 0x0000);
header_M[1] = new header_m(208, 208, 9, 0x1);
header_M[0] = new header_m(0, 0, 1, 0x0000);
header_C = new header_c(448, counter, simTime);
byte[] stream = pkt.FormPacketArray(header_C, header_M[0], header_M[1], state, header_M[2], wheel_O, header_M[3]);
return(stream);
}
public static byte[] CatchPacket(geo geo, coord pos, coord speed, coord accel, UInt32 counter)
{
Packet pkt = new Packet();
header_m[] header_M = new header_m[3];
state_o state_O = new state_o(3, 255, 255, 3, hp.myname, geo, pos, 3, 3, 8);
state_e state_E = new state_e(speed, accel, 498.55f, hp.spare);
state state = new state(state_O, state_E);
header_M[2] = new header_m(0, 0, 2, 99);
header_M[1] = new header_m(208 + header_M[2].headerSize, 208 + header_M[2].headerSize, 9, 99);
header_M[0] = new header_m(header_M[1].headerSize + header_M[1].dataSize, header_M[1].headerSize + header_M[1].dataSize, 1, 99);
header_c header_C = new header_c(header_M[0].headerSize + header_M[0].dataSize, counter);
int[] lens = { (int)header_C.headerSize, (int)header_M[0].headerSize, (int)header_M[1].headerSize, (int)header_M[1].dataSize - (int)header_M[2].headerSize, (int)header_M[2].headerSize };
byte[] stream = pkt.FormPacketArray(header_C, header_M[0], header_M[1], state, header_M[2]);
return(stream);
}
public Packet Parse(byte[] DataStream, int[] lens)
{
Packet pkt;
wheel_o[] myWheel_O = new wheel_o[4];
b = Split(DataStream, lens);
header_c _header_C = new header_c(b[0]);
header_m _header_M0 = new header_m(b[1]);
header_m _header_M1 = new header_m(b[2]);
state _state = new state(b[3]);
header_m _header_M2 = new header_m(b[4]);
myWheel_O[0] = new wheel_o(b[5]);
myWheel_O[1] = new wheel_o(b[6]);
myWheel_O[2] = new wheel_o(b[7]);
myWheel_O[3] = new wheel_o(b[8]);
header_m _header_M3 = new header_m(b[9]);
pkt = new Packet(_header_C, _header_M0, _header_M1, _state, _header_M2, myWheel_O, _header_M3);
return(pkt);
}
public Packet Parse(byte[] DataStream)
{
Packet pkt;
int offset = 0;
int entry_hdr_size = 16;
byte[] b = new byte[4096];
byte[] filtered_packet = new byte[4096];
byte[] wheel_pkg = new byte[512];
byte[] state_pkg = new byte[512];
wheel_o[] myWheel = new wheel_o[4];
state _state = new state(new byte[1024]);
header_m _header_M0 = new header_m(0, 0, 1, 0); // RDB_PKG_ID_START_OF_FRAME
header_m _header_M1 = new header_m(0, 0, 0, 0); // Object state
header_m _header_M2 = new header_m(0, 0, 0, 0); // Wheel frame
header_m _header_M3 = new header_m(0, 0, 2, 0); // RDB_PKG_ID_END_OF_FRAME
// Read main header
header_c _header_C = new header_c(DataStream);
// Move past header. Data segment is the first entry header.
offset += (int)_header_C.headerSize;
Console.WriteLine("Frame no: " + _header_C.frameNo);
// Then check reamining sub packages
while (offset < (int)_header_C.headerSize + (int)_header_C.dataSize)
{
// Read entry header
Buffer.BlockCopy(DataStream, offset, b, 0, entry_hdr_size);
header_m entry_header = new header_m(b);
// Move ahead passed entry header, read data
offset += (int)entry_header.headerSize;
if (entry_header.dataSize > 0)
{
Buffer.BlockCopy(DataStream, offset, b, 0, (int)entry_header.dataSize);
if (entry_header.pkgId == 9) // RDB_PKG_ID_OBJECT_STATE
{
_header_M1 = entry_header;
for (int i = 0; i < entry_header.dataSize / entry_header.elementSize; i++)
{
Buffer.BlockCopy(b, i * (int)entry_header.elementSize, state_pkg, 0, (int)entry_header.elementSize);
state state_tmp = new state(state_pkg);
Console.WriteLine("state :" + state_tmp.state_base.id);
if (state_tmp.state_base.id == 1)
{
_state = state_tmp;
Console.WriteLine("------Storing state pos (" +
_state.state_base.pos.x + ", " +
_state.state_base.pos.y + ", " +
_state.state_base.pos.z + ")");
}
}
}
else if (entry_header.pkgId == 14) // RDB_PKG_ID_WHEEL
{
_header_M2 = entry_header;
for (int i = 0; i < entry_header.dataSize / entry_header.elementSize; i++)
{
Buffer.BlockCopy(b, i * (int)entry_header.elementSize, wheel_pkg, 0, (int)entry_header.elementSize);
wheel_o tmp_wheel = new wheel_o(wheel_pkg);
Console.WriteLine("wheel: " + tmp_wheel.playerId);
if (tmp_wheel.playerId == 1)
{
Console.WriteLine("------Storing wheel no " + i + " steering: " + tmp_wheel.steeringAngle);
myWheel[tmp_wheel.id] = new wheel_o(wheel_pkg);
}
}
}
else
{
Console.WriteLine("Discarded package id: " + entry_header.pkgId);
}
offset += (int)entry_header.dataSize;
}
}
if (_header_M1.pkgId == 0 | _header_M2.pkgId == 0)
{
Console.WriteLine("Did not get start and/or end frame!\n");
}
pkt = new Packet(_header_C, _header_M0, _header_M1, _state, _header_M2, myWheel, _header_M3);
return(pkt);
}
