using System;
using System.Collections.Generic;
using System.IO;
using System.Text;

namespace MultiWheelC;

internal static class VehicleSyncBinaryCodec
{
    private const byte Version = 1;
    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);
        ReadHeader(reader, RegisterType);
        var carNum = reader.ReadInt32();
        var info = ReadInfo(reader);
        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.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);
        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.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);
        }

        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 void 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 != 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.");
    }

    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);
    }

    private static VehicleSyncInfo ReadInfo(BinaryReader reader)
    {
        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);
        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;
        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;
    }

    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.");
    }
}