using System;
using System.Linq;

namespace StandardScene.Magnetic.Protocol
{
    /// <summary>
    /// FASS 2.0 通用控制接口（PLC）报文编解码，对齐
    /// <c>FASS.Extend.Car.Fairyland.Plc</c> 与调度器 <c>CarRequestService</c> 逻辑。
    /// </summary>
    internal static class Fass2Protocol
    {
        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 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 byte[] BuildControl(byte command, ushort car, ushort param)
        {
            var frame = new byte[ControlFrameLength];
            frame[0] = Begin;
            frame[1] = command;
            WriteUInt16(frame, 2, car);
            WriteUInt16(frame, 4, param);
            frame[48] = Xor(frame, 1, 47);
            frame[49] = End;
            return frame;
        }

        public static byte[] BuildNodes(ushort car, ulong taskId, Fass2NodeMessage[] nodes)
        {
            if (nodes == null || nodes.Length == 0)
            {
                throw new ArgumentException("nodes required");
            }
            if (nodes.Length > 10)
            {
                throw new ArgumentException("FASS2 nodes count must be <= 10");
            }

            var frame = new byte[NodesFrameLength];
            frame[0] = Begin;
            frame[1] = CmdNodes;
            WriteUInt16(frame, 2, car);
            WriteUInt64(frame, 4, taskId);
            WriteUInt16(frame, 12, (ushort)nodes.Length);
            for (var i = 0; i < nodes.Length; i++)
            {
                Buffer.BlockCopy(nodes[i].ToBytes(), 0, frame, 14 + i * 25, 25);
            }
            frame[298] = Xor(frame, 1, 297);
            frame[299] = End;
            return frame;
        }

        public static byte[] BuildAction(ushort car, ulong actionId, Fass2NodeMessage node)
        {
            if (node == null)
            {
                throw new ArgumentException("node required");
            }

            var frame = new byte[ActionFrameLength];
            frame[0] = Begin;
            frame[1] = CmdAction;
            WriteUInt16(frame, 2, car);
            WriteUInt64(frame, 4, actionId);
            Buffer.BlockCopy(node.ToBytes(), 0, frame, 14, 25);
            frame[98] = Xor(frame, 1, 97);
            frame[99] = End;
            return frame;
        }

        /// <summary>
        /// PCB/UDP 模式：收到 AGV 主动上报的 100B 状态后，回 0x10（50B）时间戳应答。
        /// </summary>
        public static byte[] BuildStateResponse(ushort car, ulong timestampMs)
        {
            var frame = new byte[ControlFrameLength];
            frame[0] = Begin;
            frame[1] = CmdStateResponse;
            WriteUInt16(frame, 2, car);
            WriteUInt64(frame, 4, timestampMs);

            var epoch = new DateTime(1970, 1, 1, 8, 0, 0, DateTimeKind.Unspecified);
            var date = epoch.AddMilliseconds(timestampMs > long.MaxValue ? long.MaxValue : (long)timestampMs);
            frame[12] = (byte)(date.Year - 1970);
            frame[13] = (byte)date.Month;
            frame[14] = (byte)date.Day;
            frame[15] = (byte)date.Hour;
            frame[16] = (byte)date.Minute;
            frame[17] = (byte)date.Second;
            WriteUInt16(frame, 18, (ushort)date.Millisecond);

            frame[48] = Xor(frame, 1, 47);
            frame[49] = End;
            return frame;
        }

        public static ulong BeijingUnixTimeMs()
        {
            var epoch = new DateTime(1970, 1, 1, 8, 0, 0, DateTimeKind.Unspecified);
            return (ulong)(DateTime.Now - epoch).TotalMilliseconds;
        }

        public static bool TryExtractStateFrame(byte[] buffer, int length, out byte[] frame)
        {
            frame = null;
            if (buffer == null || length < StateFrameLength)
            {
                return false;
            }

            for (var i = 0; i <= length - StateFrameLength; i++)
            {
                if (buffer[i] != Begin || buffer[i + StateFrameLength - 1] != End)
                {
                    continue;
                }

                frame = new byte[StateFrameLength];
                Buffer.BlockCopy(buffer, i, frame, 0, StateFrameLength);
                return true;
            }

            if (length == StateFrameLength && buffer[0] == Begin && buffer[StateFrameLength - 1] == End)
            {
                frame = new byte[StateFrameLength];
                Buffer.BlockCopy(buffer, 0, frame, 0, StateFrameLength);
                return true;
            }

            return false;
        }

        public static Fass2StateReport ParseState(byte[] bytes)
        {
            if (bytes == null || bytes.Length < StateFrameLength)
            {
                throw new ArgumentException("FASS2 state frame must be 100 bytes");
            }
            if (bytes[0] != Begin || bytes[99] != End)
            {
                throw new ArgumentException($"invalid FASS2 state frame: begin=0x{bytes[0]:X2}, end=0x{bytes[99]:X2}");
            }

            var nodeBytes = new byte[25];
            Buffer.BlockCopy(bytes, 53, nodeBytes, 0, 25);
            return new Fass2StateReport
            {
                Command = bytes[1],
                Car = ReadUInt16(bytes, 2),
                Length = ReadUInt16(bytes, 4),
                Width = ReadUInt16(bytes, 6),
                BatteryCharge = bytes[28],
                BatteryHealth = bytes[29],
                BatteryCurrent = ReadUInt16(bytes, 30),
                BatteryVoltage = ReadUInt16(bytes, 32),
                HeadingAngle = ReadUInt16(bytes, 34),
                State = bytes[36],
                Alarm = ReadUInt64(bytes, 37),
                Task = ReadUInt64(bytes, 45),
                Node = Fass2NodeMessage.FromBytes(nodeBytes)
            };
        }

        public static string ToHex(byte[] bytes)
        {
            return bytes == null ? string.Empty : string.Join(" ", bytes.Select(b => b.ToString("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})";
            }
        }

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

        private static ushort ReadUInt16(byte[] bytes, int offset)
        {
            return BitConverter.ToUInt16(bytes, offset);
        }

        private static ulong ReadUInt64(byte[] bytes, int offset)
        {
            return BitConverter.ToUInt64(bytes, offset);
        }
    }

    public sealed class Fass2NodeMessage
    {
        public ushort Node { get; set; }
        public ushort Distance { get; set; }
        public byte StartStop { get; set; }
        public byte Direction { get; set; }
        public byte Orientation { get; set; }
        public byte Byroad { get; set; }
        public ushort Speed { get; set; }
        public byte Obstacle { get; set; }
        public byte Audio { get; set; }
        public byte Light { get; set; }
        public byte Charge { get; set; }
        public byte Rest { get; set; }
        public byte Lift { get; set; }
        public byte Clamp { get; set; }
        public byte Tray { get; set; }
        public byte Roll { get; set; }
        public byte Shutdown { get; set; }

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

        public static Fass2NodeMessage FromBytes(byte[] bytes)
        {
            if (bytes == null || bytes.Length < 25)
            {
                throw new ArgumentException("node frame must be 25 bytes");
            }
            return new Fass2NodeMessage
            {
                Node = ReadUInt16(bytes, 0),
                Distance = ReadUInt16(bytes, 2),
                StartStop = bytes[4],
                Direction = bytes[5],
                Orientation = bytes[6],
                Byroad = bytes[7],
                Speed = ReadUInt16(bytes, 8),
                Obstacle = bytes[10],
                Audio = bytes[11],
                Light = bytes[12],
                Charge = bytes[13],
                Rest = bytes[14],
                Lift = bytes[15],
                Clamp = bytes[16],
                Tray = bytes[17],
                Roll = bytes[18],
                Shutdown = bytes[19]
            };
        }

        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 ushort ReadUInt16(byte[] bytes, int offset)
        {
            return BitConverter.ToUInt16(bytes, offset);
        }
    }

    public sealed class Fass2StateReport
    {
        public byte Command { get; set; }
        public ushort Car { get; set; }
        public ushort Length { get; set; }
        public ushort Width { get; set; }
        public byte BatteryCharge { get; set; }
        public byte BatteryHealth { get; set; }
        public ushort BatteryCurrent { get; set; }
        public ushort BatteryVoltage { get; set; }
        public ushort HeadingAngle { get; set; }
        public byte State { get; set; }
        public ulong Alarm { get; set; }
        public ulong Task { get; set; }
        public Fass2NodeMessage Node { get; set; } = new Fass2NodeMessage();
    }
}