public void Parse(byte[] received_bytes)
{
var packet = new MappingPacket();
string converted = Encoding.UTF8.GetString(received_bytes, 0, received_bytes.Length);
using (StringReader reader = new StringReader(converted))
{
string line = string.Empty;
do
{
line = reader.ReadLine();
if (line != null)
{
var spl_line = line.Split('=');
var map = spl_line[1].Replace("\"", "");
var sensid = Convert.ToInt32(spl_line[0], 16);
if (!mapping.ContainsKey(sensid))
{
mapping.Add(sensid, map);
}
else
{
mapping[sensid] = map;
}
}
} while (line != null);
}
initialized = true;
}
public void ParseUpdate(BinaryReader br, MappingPacket mapping_packet = null)
{
//BinaryReader br = new BinaryReader(packet_content_stream);
//foreach (Module module in modules){foreach (var sensor in module.sensors){ sensor.active = false; }}
module_serials_in_current_packet.Clear();
module_count = br.ReadByte();
for (int i = 0; i < module_count; i++)
{
Module cur_module = null;
int cur_module_serial = br.ReadUInt16();
module_serials_in_current_packet.Add(cur_module_serial);
foreach (Module module in modules)
{
if (module.serial == cur_module_serial)
{
cur_module = module;
break;
}
}
if (cur_module == null)
{
cur_module = new Module
{
serial = cur_module_serial
};
modules.Add(cur_module);
}
cur_module.sensors_active = br.ReadByte();
cur_module.data_integrity = br.ReadByte();
foreach (var sensor in cur_module.sensors)
{
sensor.active = false;
}
for (int s = 0; s < 6; s++)
{
Sensor cur_sensor = null;
int cur_sensor_id = cur_module.serial * 16 + s;
foreach (var sensor in cur_module.sensors)
{
if (sensor.id == cur_sensor_id)
{
cur_sensor = sensor;
break;
}
}
if (cur_sensor == null)
{
cur_sensor = new Sensor
{
id = cur_sensor_id
};
cur_module.sensors.Add(cur_sensor);
}
if (mapping_packet != null && mapping_packet.mapping.TryGetValue(cur_sensor.id, out temp_map_str))
{
cur_sensor.mapping = temp_map_str;
temp_map_str = "";
}
cur_sensor.active = (cur_module.sensors_active & (1 << s)) > 0;
cur_sensor.bad_coords = (cur_module.data_integrity & (1 << s)) > 0;
if (cur_sensor.active)
{
cur_sensor.q0 = br.ReadSingle();
cur_sensor.q1 = br.ReadSingle();
cur_sensor.q2 = br.ReadSingle();
cur_sensor.q3 = br.ReadSingle();
cur_sensor.x = br.ReadSingle();
cur_sensor.y = br.ReadSingle();
cur_sensor.z = br.ReadSingle();
int bonelength = br.ReadInt32();
if (bonelength > 0)
{
while (bonelength > cur_sensor.bones.Count)
{
cur_sensor.bones.Add(new IKBone());
}
for (int bl = 0; bl < bonelength; bl++)
{
var cur_bone = cur_sensor.bones[bl];
cur_bone.x = br.ReadSingle();
cur_bone.y = br.ReadSingle();
cur_bone.z = br.ReadSingle();
}
}
}
}
}
module_removal_in_process = true;
while (module_removal_in_process)
{
module_removal_in_process = false;
foreach (Module module in modules)
{
if (!module_serials_in_current_packet.Contains(module.serial))
{
modules.Remove(module);
module_removal_in_process = true;
break;
}
}
}
initialized = true;
}
