using System;
using System.Linq;
using System.Text;

namespace StandardScene.Fass2Simulator
{
    /// <summary>
    /// FASS 2.0 PCB/UDP 报文编解码，字段布局对齐
    /// <c>StandardScene.Magnetic.Protocol.Fass2Protocol.ParseState</c> 与 backend PCB 仿真器。
    /// </summary>
    public static class Fass2SimProtocol
    {
        public const byte Begin = 0xBB;
        public const byte End = 0xEE;

        public const byte CmdQuery = 0x00;
        public const byte CmdStart = 0x01;
        public const byte CmdStop = 0x02;
        public const byte CmdEmergencyStop = 0x03;
        public const byte CmdReset = 0x04;
        public const byte CmdRest = 0x05;
        public const byte CmdShutdown = 0x06;
        public const byte CmdAction = 0xA1;
        public const byte CmdNodes = 0xB1;
        public const byte CmdStateResponse = 0x10;

        public const int ControlFrameLength = 50;
        public const int StateFrameLength = 100;
        public const int ActionFrameLength = 100;
        public const int NodesFrameLength = 300;

        public static byte[] BuildState(Fass2SimVehicleSnapshot snapshot)
        {
            var frame = new byte[StateFrameLength];
            frame[0] = Begin;
            frame[1] = snapshot.LastCommand;
            WriteUInt16(frame, 2, snapshot.Car);
            WriteUInt16(frame, 4, snapshot.Length);
            WriteUInt16(frame, 6, snapshot.Width);

            var carType = Encoding.ASCII.GetBytes("Fass2Sim".PadRight(16, '\0'));
            Buffer.BlockCopy(carType, 0, frame, 12, Math.Min(16, carType.Length));

            frame[28] = snapshot.BatteryCharge;
            frame[29] = snapshot.BatteryHealth;
            WriteUInt16(frame, 30, snapshot.BatteryCurrent);
            WriteUInt16(frame, 32, snapshot.BatteryVoltage);
            WriteUInt16(frame, 34, snapshot.HeadingAngle);
            frame[36] = snapshot.State;
            WriteUInt64(frame, 37, snapshot.Alarm);
            WriteUInt64(frame, 45, snapshot.Task);

            var nodeBytes = BuildNodeBytes(snapshot);
            Buffer.BlockCopy(nodeBytes, 0, frame, 53, 25);

            frame[98] = Xor(frame, 1, 97);
            frame[99] = End;
            return frame;
        }

        public static byte[] BuildNodeBytes(Fass2SimVehicleSnapshot snapshot)
        {
            var bytes = new byte[25];
            WriteUInt16(bytes, 0, snapshot.Node);
            WriteUInt16(bytes, 2, snapshot.Distance);
            bytes[4] = snapshot.StartStop;
            bytes[5] = snapshot.Direction;
            bytes[6] = snapshot.Orientation;
            bytes[7] = snapshot.Byroad;
            WriteUInt16(bytes, 8, snapshot.Speed);
            bytes[10] = snapshot.Obstacle;
            bytes[11] = snapshot.Audio;
            bytes[12] = snapshot.Light;
            bytes[13] = snapshot.Charge;
            bytes[14] = snapshot.Rest;
            bytes[15] = snapshot.Lift;
            bytes[16] = snapshot.Clamp;
            bytes[17] = snapshot.Tray;
            bytes[18] = snapshot.Roll;
            bytes[19] = snapshot.Shutdown;
            return bytes;
        }

        public static bool TryGetCommand(byte[] packet, out byte command, out int expectedLength)
        {
            command = 0;
            expectedLength = 0;
            if (packet == null || packet.Length < 2 || packet[0] != Begin)
            {
                return false;
            }

            command = packet[1];
            expectedLength = ResolvePacketLength(command);
            return expectedLength > 0;
        }

        public static int ResolvePacketLength(byte command)
        {
            switch (command)
            {
                case CmdQuery:
                case CmdStart:
                case CmdStop:
                case CmdEmergencyStop:
                case CmdReset:
                case CmdRest:
                case CmdShutdown:
                case CmdStateResponse:
                    return ControlFrameLength;
                case CmdAction:
                    return ActionFrameLength;
                case CmdNodes:
                    return NodesFrameLength;
                default:
                    return 0;
            }
        }

        public static string CommandName(byte command)
        {
            switch (command)
            {
                case CmdQuery: return "Query(0x00)";
                case CmdStart: return "Start(0x01)";
                case CmdStop: return "Stop(0x02)";
                case CmdEmergencyStop: return "EmergencyStop(0x03)";
                case CmdReset: return "Reset(0x04)";
                case CmdRest: return "Rest(0x05)";
                case CmdShutdown: return "Shutdown(0x06)";
                case CmdAction: return "Action(0xA1)";
                case CmdNodes: return "Nodes(0xB1)";
                case CmdStateResponse: return "StateResponse(0x10)";
                default: return $"Unknown(0x{command:X2})";
            }
        }

        public static string StateText(byte state)
        {
            switch (state)
            {
                case 0: return "未准备";
                case 1: return "运行中";
                case 2: return "停止中";
                case 3: return "急停中";
                case 4: return "故障中";
                case 5: return "任务中";
                case 6: return "休眠中";
                case 7: return "关机中";
                case 8: return "充电中";
                default: return $"未知({state})";
            }
        }

        /// <summary>协议速度(0.1 m/min) → mm/s。</summary>
        public static double ProtocolSpeedToMmPerSec(ushort protocolSpeed)
        {
            if (protocolSpeed == 0)
            {
                return 0;
            }

            return protocolSpeed * 0.1 * 1000.0 / 60.0;
        }

        public static string ToHex(byte[] bytes)
        {
            return bytes == null ? string.Empty : string.Join(" ", bytes.Select(b => b.ToString("X2")));
        }

        public static byte Xor(byte[] data, int start, int length)
        {
            byte xor = 0;
            for (var i = 0; i < length; i++)
            {
                xor ^= data[start + i];
            }
            return xor;
        }

        public static bool TryParseNodes(byte[] packet, out ulong taskId, out Fass2SimNodeMessage[] nodes)
        {
            taskId = 0;
            nodes = Array.Empty<Fass2SimNodeMessage>();
            if (packet == null || packet.Length < NodesFrameLength || packet[1] != CmdNodes)
            {
                return false;
            }

            taskId = BitConverter.ToUInt64(packet, 4);
            var count = BitConverter.ToUInt16(packet, 12);
            if (count == 0 || count > 10)
            {
                return false;
            }

            var requiredLength = 14 + count * 25 + 2;
            if (packet.Length < requiredLength)
            {
                return false;
            }

            nodes = new Fass2SimNodeMessage[count];
            for (var i = 0; i < count; i++)
            {
                nodes[i] = Fass2SimNodeMessage.FromBytes(packet, 14 + i * 25);
            }

            return true;
        }

        public static bool TryParseAction(byte[] packet, out ulong actionId, out Fass2SimNodeMessage node)
        {
            actionId = 0;
            node = null;
            if (packet == null || packet.Length < ActionFrameLength || packet[1] != CmdAction)
            {
                return false;
            }

            actionId = BitConverter.ToUInt64(packet, 4);
            node = Fass2SimNodeMessage.FromBytes(packet, 14);
            return true;
        }

        private static void WriteUInt16(byte[] buffer, int offset, ushort value)
        {
            var bytes = BitConverter.GetBytes(value);
            buffer[offset] = bytes[0];
            buffer[offset + 1] = bytes[1];
        }

        private static void WriteUInt64(byte[] buffer, int offset, ulong value)
        {
            var bytes = BitConverter.GetBytes(value);
            Buffer.BlockCopy(bytes, 0, buffer, offset, 8);
        }
    }
}