278 lines
11 KiB
C#
278 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
using System.Text;
|
|
|
|
namespace MultiWheelC;
|
|
|
|
internal static class VehicleSyncBinaryCodec
|
|
{
|
|
private const byte Version = 2;
|
|
private const byte RegisterType = 1;
|
|
private const byte NotificationType = 2;
|
|
private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
|
|
|
|
public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info)
|
|
{
|
|
using var stream = new MemoryStream();
|
|
using var writer = new BinaryWriter(stream, Encoding.UTF8);
|
|
WriteHeader(writer, RegisterType);
|
|
writer.Write(carNum);
|
|
WriteInfo(writer, info);
|
|
writer.Flush();
|
|
return stream.ToArray();
|
|
}
|
|
|
|
public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload)
|
|
{
|
|
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
|
|
using var reader = new BinaryReader(stream, Encoding.UTF8);
|
|
var version = ReadHeader(reader, RegisterType);
|
|
var carNum = reader.ReadInt32();
|
|
var info = ReadInfo(reader, version);
|
|
EnsureFullyRead(stream);
|
|
return (carNum, info);
|
|
}
|
|
|
|
public static byte[] EncodeNotification(VehicleSyncNotification notification)
|
|
{
|
|
using var stream = new MemoryStream();
|
|
using var writer = new BinaryWriter(stream, Encoding.UTF8);
|
|
WriteHeader(writer, NotificationType);
|
|
|
|
writer.Write(notification.Seq);
|
|
writer.Write(BuildNotificationFlags(notification));
|
|
writer.Write(notification.Mode);
|
|
writer.Write(notification.FleetStopSourceCar);
|
|
writer.Write(notification.CenterX);
|
|
writer.Write(notification.CenterY);
|
|
writer.Write(notification.CenterTh);
|
|
writer.Write(notification.FleetVx);
|
|
writer.Write(notification.FleetFrontTh);
|
|
writer.Write(notification.FleetRearTh);
|
|
writer.Write(notification.FleetOmega);
|
|
writer.Write(notification.RequestedFleetOmega);
|
|
writer.Write(notification.SyncTh);
|
|
writer.Write(notification.SyncDistance);
|
|
writer.Write(notification.DeltaDetectCenter);
|
|
writer.Write(notification.RotateActiveOmega);
|
|
writer.Write(notification.RotateCompXyFac);
|
|
writer.Write(notification.RotateCompXyIFac);
|
|
writer.Write(notification.RotateCompXyMax);
|
|
writer.Write(notification.RotateCompThFac);
|
|
writer.Write(notification.RotateCompThIFac);
|
|
writer.Write(notification.RotateCompThMax);
|
|
writer.Write(notification.RotateCompTangentFrac);
|
|
writer.Write(notification.RotateStartWheelAlignDeg);
|
|
writer.Write(notification.RotateActiveWheelAlignDeg);
|
|
writer.Write(notification.IdealX);
|
|
writer.Write(notification.IdealY);
|
|
writer.Write(notification.IdealTh);
|
|
WriteString(writer, notification.FleetStopReason);
|
|
|
|
var fleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
|
|
if (fleet.Count > ushort.MaxValue)
|
|
throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit.");
|
|
writer.Write((ushort)fleet.Count);
|
|
foreach (var kv in fleet)
|
|
{
|
|
writer.Write(kv.Key);
|
|
WriteInfo(writer, kv.Value);
|
|
}
|
|
|
|
writer.Flush();
|
|
return stream.ToArray();
|
|
}
|
|
|
|
public static VehicleSyncNotification DecodeNotification(byte[] payload)
|
|
{
|
|
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
|
|
using var reader = new BinaryReader(stream, Encoding.UTF8);
|
|
var version = ReadHeader(reader, NotificationType);
|
|
|
|
var notification = new VehicleSyncNotification
|
|
{
|
|
Seq = reader.ReadInt64()
|
|
};
|
|
|
|
ApplyNotificationFlags(notification, reader.ReadUInt16());
|
|
notification.Mode = reader.ReadInt32();
|
|
notification.FleetStopSourceCar = reader.ReadInt32();
|
|
notification.CenterX = reader.ReadSingle();
|
|
notification.CenterY = reader.ReadSingle();
|
|
notification.CenterTh = reader.ReadSingle();
|
|
notification.FleetVx = reader.ReadSingle();
|
|
notification.FleetFrontTh = reader.ReadSingle();
|
|
notification.FleetRearTh = reader.ReadSingle();
|
|
notification.FleetOmega = reader.ReadSingle();
|
|
notification.RequestedFleetOmega = reader.ReadSingle();
|
|
notification.SyncTh = reader.ReadSingle();
|
|
notification.SyncDistance = reader.ReadSingle();
|
|
notification.DeltaDetectCenter = reader.ReadSingle();
|
|
notification.RotateActiveOmega = reader.ReadSingle();
|
|
notification.RotateCompXyFac = reader.ReadSingle();
|
|
notification.RotateCompXyIFac = reader.ReadSingle();
|
|
notification.RotateCompXyMax = reader.ReadSingle();
|
|
notification.RotateCompThFac = reader.ReadSingle();
|
|
notification.RotateCompThIFac = reader.ReadSingle();
|
|
notification.RotateCompThMax = reader.ReadSingle();
|
|
notification.RotateCompTangentFrac = reader.ReadSingle();
|
|
notification.RotateStartWheelAlignDeg = reader.ReadSingle();
|
|
notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
|
|
notification.IdealX = reader.ReadSingle();
|
|
notification.IdealY = reader.ReadSingle();
|
|
notification.IdealTh = reader.ReadSingle();
|
|
notification.FleetStopReason = ReadString(reader);
|
|
|
|
var fleetCount = reader.ReadUInt16();
|
|
notification.Fleet = new Dictionary<int, VehicleSyncInfo>(fleetCount);
|
|
for (var i = 0; i < fleetCount; ++i)
|
|
{
|
|
var carNum = reader.ReadInt32();
|
|
notification.Fleet[carNum] = ReadInfo(reader, version);
|
|
}
|
|
|
|
EnsureFullyRead(stream);
|
|
return notification;
|
|
}
|
|
|
|
private static void WriteHeader(BinaryWriter writer, byte type)
|
|
{
|
|
writer.Write(Magic);
|
|
writer.Write(Version);
|
|
writer.Write(type);
|
|
writer.Write((ushort)0);
|
|
}
|
|
|
|
private static byte ReadHeader(BinaryReader reader, byte expectedType)
|
|
{
|
|
for (var i = 0; i < Magic.Length; ++i)
|
|
{
|
|
if (reader.ReadByte() != Magic[i])
|
|
throw new InvalidDataException("Invalid multi-vehicle sync binary magic.");
|
|
}
|
|
|
|
var version = reader.ReadByte();
|
|
if (version < 1 || version > Version)
|
|
throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
|
|
|
|
var type = reader.ReadByte();
|
|
if (type != expectedType)
|
|
throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}.");
|
|
|
|
var reserved = reader.ReadUInt16();
|
|
if (reserved != 0)
|
|
throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
|
|
|
|
return version;
|
|
}
|
|
|
|
private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
|
|
{
|
|
writer.Write(BuildInfoFlags(info));
|
|
WriteString(writer, info.Ip);
|
|
writer.Write(info.Port);
|
|
writer.Write(info.X);
|
|
writer.Write(info.Y);
|
|
writer.Write(info.Th);
|
|
writer.Write(info.LayoutX);
|
|
writer.Write(info.LayoutY);
|
|
writer.Write(info.LayoutTh);
|
|
WriteString(writer, info.MotionInfeasibleReason);
|
|
WriteString(writer, info.RotateWheelAlignDetail);
|
|
writer.Write(info.AppliedNotificationSeq);
|
|
}
|
|
|
|
private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
|
|
{
|
|
var info = new VehicleSyncInfo();
|
|
ApplyInfoFlags(info, reader.ReadUInt16());
|
|
info.Ip = ReadString(reader);
|
|
info.Port = reader.ReadInt32();
|
|
info.X = reader.ReadSingle();
|
|
info.Y = reader.ReadSingle();
|
|
info.Th = reader.ReadSingle();
|
|
info.LayoutX = reader.ReadSingle();
|
|
info.LayoutY = reader.ReadSingle();
|
|
info.LayoutTh = reader.ReadSingle();
|
|
info.MotionInfeasibleReason = ReadString(reader);
|
|
info.RotateWheelAlignDetail = ReadString(reader);
|
|
info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
|
|
return info;
|
|
}
|
|
|
|
private static ushort BuildInfoFlags(VehicleSyncInfo info)
|
|
{
|
|
ushort flags = 0;
|
|
if (info.Master) flags |= 1 << 0;
|
|
if (info.PosAvailable) flags |= 1 << 1;
|
|
if (info.Aligned) flags |= 1 << 2;
|
|
if (info.DetectOk) flags |= 1 << 3;
|
|
if (info.MotionFeasible) flags |= 1 << 4;
|
|
if (info.RotateWheelsAligned) flags |= 1 << 5;
|
|
return flags;
|
|
}
|
|
|
|
private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags)
|
|
{
|
|
info.Master = (flags & (1 << 0)) != 0;
|
|
info.PosAvailable = (flags & (1 << 1)) != 0;
|
|
info.Aligned = (flags & (1 << 2)) != 0;
|
|
info.DetectOk = (flags & (1 << 3)) != 0;
|
|
info.MotionFeasible = (flags & (1 << 4)) != 0;
|
|
info.RotateWheelsAligned = (flags & (1 << 5)) != 0;
|
|
}
|
|
|
|
private static ushort BuildNotificationFlags(VehicleSyncNotification notification)
|
|
{
|
|
ushort flags = 0;
|
|
if (notification.PosAvailable) flags |= 1 << 0;
|
|
if (notification.Aligned) flags |= 1 << 1;
|
|
if (notification.FleetMotionReleased) flags |= 1 << 2;
|
|
if (notification.FleetStopActive) flags |= 1 << 3;
|
|
if (notification.AutoEnabled) flags |= 1 << 4;
|
|
if (notification.ManualEnabled) flags |= 1 << 5;
|
|
if (notification.HasIdeal) flags |= 1 << 6;
|
|
if (notification.RotateParamsValid) flags |= 1 << 7;
|
|
if (notification.UseDetourCorrection) flags |= 1 << 8;
|
|
return flags;
|
|
}
|
|
|
|
private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags)
|
|
{
|
|
notification.PosAvailable = (flags & (1 << 0)) != 0;
|
|
notification.Aligned = (flags & (1 << 1)) != 0;
|
|
notification.FleetMotionReleased = (flags & (1 << 2)) != 0;
|
|
notification.FleetStopActive = (flags & (1 << 3)) != 0;
|
|
notification.AutoEnabled = (flags & (1 << 4)) != 0;
|
|
notification.ManualEnabled = (flags & (1 << 5)) != 0;
|
|
notification.HasIdeal = (flags & (1 << 6)) != 0;
|
|
notification.RotateParamsValid = (flags & (1 << 7)) != 0;
|
|
notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
|
|
}
|
|
|
|
private static void WriteString(BinaryWriter writer, string value)
|
|
{
|
|
var bytes = Encoding.UTF8.GetBytes(value ?? "");
|
|
if (bytes.Length > ushort.MaxValue)
|
|
throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit.");
|
|
writer.Write((ushort)bytes.Length);
|
|
writer.Write(bytes);
|
|
}
|
|
|
|
private static string ReadString(BinaryReader reader)
|
|
{
|
|
var length = reader.ReadUInt16();
|
|
var bytes = reader.ReadBytes(length);
|
|
if (bytes.Length != length)
|
|
throw new EndOfStreamException("Truncated multi-vehicle sync string payload.");
|
|
return Encoding.UTF8.GetString(bytes);
|
|
}
|
|
|
|
private static void EnsureFullyRead(MemoryStream stream)
|
|
{
|
|
if (stream.Position != stream.Length)
|
|
throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet.");
|
|
}
|
|
}
|