using RobotNet.Script;
using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;

namespace RobotNet.ScriptManager.Connections;

public class CcLinkIeBasicClient(IeBasicClientOptions options) : ICcLinkIeBasicClient
{
    private readonly SemaphoreSlim _sendLock = new(1, 1);
    private UdpClient? _udp;
    private TcpClient? _tcp;
    private NetworkStream? _tcpStream;
    private IPEndPoint? _remoteEp;
    private CancellationTokenSource? _pollCts;

    private IeBasicStatus _status = new()
    {
        LinkUp = false,
        ConsecutiveErrors = 0,
        LastEndCode = null,
        LastErrorText = null,
        AvgRtt = null,
        MaxRtt = null
    };

    private volatile bool _connected;
    private long _rttCount;
    private long _rttSumTicks;

    public bool IsConnected => _connected;
    public int RetryCount { get => options.RetryCount; set => options = options with { RetryCount = value }; }

    public event EventHandler<PollUpdatedEventArgs>? Polled;

    // ====== Public API ======

    public async Task ConnectAsync(CancellationToken ct = default)
    {
        if (options.FrameFormat != SlmpFrameFormat.Format3E)
            throw new NotSupportedException("Triển khai tham chiếu này hiện hỗ trợ 3E binary frame.");

        if (options.Transport == IeBasicTransport.Udp)
        {
            _udp?.Dispose();
            _udp = new UdpClient();
            _udp.Client.ReceiveTimeout = (int)options.Timeout.TotalMilliseconds;
            _udp.Client.SendTimeout = (int)options.Timeout.TotalMilliseconds;

            var ip = await ResolveHostAsync(options.Host, ct);
            _remoteEp = new IPEndPoint(ip, options.Port);
            _udp.Connect(_remoteEp);
        }
        else
        {
            _tcp?.Close();
            _tcp = new TcpClient
            {
                NoDelay = true,
                ReceiveTimeout = (int)options.Timeout.TotalMilliseconds,
                SendTimeout = (int)options.Timeout.TotalMilliseconds
            };

            var ip = await ResolveHostAsync(options.Host, ct);
            await _tcp.ConnectAsync(ip, options.Port, ct);
            _tcpStream = _tcp.GetStream();
        }

        _connected = true;
        _status = _status with { LinkUp = true, ConsecutiveErrors = 0, LastErrorText = null, LastEndCode = null };
    }

    public Task DisconnectAsync(CancellationToken ct = default)
    {
        StopPollingInternal();

        _connected = false;

        try { _tcpStream?.Dispose(); } catch { }
        try { _tcp?.Close(); } catch { }
        try { _udp?.Dispose(); } catch { }

        _tcpStream = null;
        _tcp = null;
        _udp = null;

        _status = _status with { LinkUp = false };
        return Task.CompletedTask;
    }

    public async Task<bool[]> ReadBitsAsync(DeviceCode device, int startAddress, int count, CancellationToken ct = default)
    {
        // 3E binary: Command 0x0401, Subcommand 0x0001 (bit units)
        var payload = BuildBatchRead(device, startAddress, count, isBit: true);
        var resp = await SendSlmpAsync(SlmpCommand.ReadDevice, payload, ct);
        EnsureOk(resp);

        // Bit data: packed per bit (LSB-first) theo manual. Ở đây đơn giản dùng từng byte -> từng bit.
        return UnpackBits(resp.Payload.Span, count);
    }

    public async Task WriteBitsAsync(DeviceCode device, int startAddress, ReadOnlyMemory<bool> values, CancellationToken ct = default)
    {
        // 3E binary: Command 0x1401, Subcommand 0x0001 (bit units)
        var payload = BuildBatchWriteBits(device, startAddress, values.Span);
        var resp = await SendSlmpAsync(SlmpCommand.WriteDevice, payload, ct);
        EnsureOk(resp);
    }

    public async Task<ushort[]> ReadWordsAsync(DeviceCode device, int startAddress, int wordCount, CancellationToken ct = default)
    {
        // 3E binary: Command 0x0401, Subcommand 0x0000 (word units)
        var payload = BuildBatchRead(device, startAddress, wordCount, isBit: false);
        var resp = await SendSlmpAsync(SlmpCommand.ReadDevice, payload, ct);
        EnsureOk(resp);

        // Word data: little-endian UInt16
        if (resp.Payload.Length < wordCount * 2)
            throw new InvalidOperationException("Payload ngắn hơn mong đợi.");
        var span = resp.Payload.Span;
        var result = new ushort[wordCount];
        for (int i = 0; i < wordCount; i++)
            result[i] = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(i * 2, 2));
        return result;
    }

    public async Task WriteWordsAsync(DeviceCode device, int startAddress, ReadOnlyMemory<ushort> words, CancellationToken ct = default)
    {
        // 3E binary: Command 0x1401, Subcommand 0x0000 (word units)
        var payload = BuildBatchWriteWords(device, startAddress, words.Span);
        var resp = await SendSlmpAsync(SlmpCommand.WriteDevice, payload, ct);
        EnsureOk(resp);
    }

    public async Task<uint[]> ReadDWordsAsync(DeviceCode device, int startAddress, int dwordCount, CancellationToken ct = default)
    {
        if (SupportsDirectDWordRead(device))
        {
            // 3E binary: Command 0x0401, Subcommand 0x0002 (dword units)
            var payload = BuildBatchRead(device, startAddress, dwordCount, isBit: false, isDWord: true);
            var resp = await SendSlmpAsync(SlmpCommand.ReadDevice, payload, ct);
            EnsureOk(resp);

            if (resp.Payload.Length < dwordCount * 4)
                throw new InvalidOperationException("Payload ngắn hơn mong đợi.");
            var span = resp.Payload.Span;
            var result = new uint[dwordCount];
            for (int i = 0; i < dwordCount; i++)
                result[i] = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(i * 4, 4));
            return result;
        }
        else
        {
            // Đọc dword như đọc 2 word/liên tiếp
            var words = await ReadWordsAsync(device, startAddress, dwordCount * 2, ct);
            var result = new uint[dwordCount];

            if (options.LittleEndianWordOrder)
            {
                for (int i = 0, j = 0; i < dwordCount; i++, j += 2)
                    result[i] = (uint)(words[j] | ((uint)words[j + 1] << 16));
            }
            else
            {
                for (int i = 0, j = 0; i < dwordCount; i++, j += 2)
                    result[i] = (uint)(words[j + 1] | ((uint)words[j] << 16));
            }
            return result;
        }
    }

    public async Task WriteDWordsAsync(DeviceCode device, int startAddress, ReadOnlyMemory<uint> dwords, CancellationToken ct = default)
    {
        var span = dwords.Span;
        var buf = new ushort[span.Length * 2];

        if (options.LittleEndianWordOrder)
        {
            for (int i = 0, j = 0; i < span.Length; i++, j += 2)
            {
                buf[j] = (ushort)(span[i] & 0xFFFF);
                buf[j + 1] = (ushort)((span[i] >> 16) & 0xFFFF);
            }
        }
        else
        {
            for (int i = 0, j = 0; i < span.Length; i++, j += 2)
            {
                buf[j] = (ushort)((span[i] >> 16) & 0xFFFF);
                buf[j + 1] = (ushort)(span[i] & 0xFFFF);
            }
        }
        await WriteWordsAsync(device, startAddress, buf, ct);
    }

    public async Task<ushort[]> ReadRandomWordsAsync((DeviceCode Device, int Address)[] points, CancellationToken ct = default)
    {
        // 3E binary: Read Random (0x0403). Payload: điểm (addr+devcode)
        var payload = BuildRandomRead(points);
        var resp = await SendSlmpAsync(SlmpCommand.ReadRandom, payload, ct);
        EnsureOk(resp);

        // Trả về word liên tiếp
        var span = resp.Payload.Span;
        if (span.Length < points.Length * 2) throw new InvalidOperationException("Payload ngắn hơn mong đợi.");
        var result = new ushort[points.Length];
        for (int i = 0; i < points.Length; i++)
            result[i] = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(i * 2, 2));
        return result;
    }

    public async Task WriteRandomWordsAsync((DeviceCode Device, int Address)[] points, ReadOnlyMemory<ushort> values, CancellationToken ct = default)
    {
        if (points.Length != values.Length)
            throw new ArgumentException("Số điểm và số giá trị không khớp.");

        var payload = BuildRandomWrite(points, values.Span);
        var resp = await SendSlmpAsync(SlmpCommand.WriteRandom, payload, ct);
        EnsureOk(resp);
    }

    public async Task<IeBasicStatus> GetStatusAsync(CancellationToken ct = default)
    {
        // Có thể gửi NodeMonitor để “làm nóng” trạng thái; ở đây trả về snapshot hiện có
        await Task.Yield();
        return _status;
    }

    public async Task<bool> PingAsync(CancellationToken ct = default)
    {
        try
        {
            // Node monitor: dùng CMD DeviceInfo/NodeMonitor rỗng để kiểm phản hồi
            var resp = await SendSlmpAsync(SlmpCommand.NodeMonitor, ReadOnlyMemory<byte>.Empty, ct);
            return resp.EndCode == SlmpEndCode.Completed;
        }
        catch
        {
            return false;
        }
    }

    public async Task<ModuleIdentity> IdentifyAsync(CancellationToken ct = default)
    {
        // Tối giản: gửi DeviceInfo và parse sơ bộ (tuỳ PLC mà payload khác nhau)
        var resp = await SendSlmpAsync(SlmpCommand.DeviceInfo, ReadOnlyMemory<byte>.Empty, ct);
        // Ở đây không chuẩn hoá do payload phụ thuộc model; trả về khung rỗng để người dùng tự giải.
        return new ModuleIdentity
        {
            Vendor = "Mitsubishi Electric",
            AdditionalInfo = $"Raw bytes: {resp.Payload.Length} (xem manual model để parse)"
        };
    }

    public async Task<SlmpResponse> SendSlmpAsync(SlmpCommand command, ReadOnlyMemory<byte> payload, CancellationToken ct = default)
    {
        if (!_connected) throw new InvalidOperationException("Chưa kết nối.");

        // Dựng 3E frame
        var req = Build3EFrame(options, (ushort)command, subcommand: CurrentSubcommandHint, payload);

        // Gửi/nhận với retry
        var sw = System.Diagnostics.Stopwatch.StartNew();
        for (int attempt = 0; attempt <= RetryCount; attempt++)
        {
            try
            {
                var rawResp = await SendAndReceiveAsync(req, ct);
                var resp = Parse3EResponse(rawResp);

                sw.Stop();
                TrackRtt(sw.Elapsed);

                _status = _status with
                {
                    LinkUp = true,
                    ConsecutiveErrors = 0,
                    LastEndCode = resp.EndCode,
                    LastErrorText = null,
                    AvgRtt = TimeSpan.FromTicks((_rttCount > 0) ? _rttSumTicks / _rttCount : 0),
                    MaxRtt = (_status.MaxRtt == null || sw.Elapsed > _status.MaxRtt) ? sw.Elapsed : _status.MaxRtt
                };
                return resp;
            }
            catch (Exception ex) when (attempt < RetryCount)
            {
                _status = _status with
                {
                    LinkUp = false,
                    ConsecutiveErrors = _status.ConsecutiveErrors + 1,
                    LastErrorText = ex.Message,
                    LastEndCode = SlmpEndCode.TransportError
                };
                // thử lại
                await Task.Delay(10, ct);
            }
        }

        sw.Stop();
        throw new TimeoutException("SLMP: Hết retry mà vẫn lỗi.");
    }

    public async ValueTask DisposeAsync()
    {
        await DisconnectAsync();
        _sendLock.Dispose();
        GC.SuppressFinalize(this);
    }

    public void Dispose()
    {
        DisposeAsync().AsTask().GetAwaiter().GetResult();
        GC.SuppressFinalize(this);
    }

    public void StartPolling(PollOptions options, CancellationToken ct = default)
    {
        StopPollingInternal();
        _pollCts = CancellationTokenSource.CreateLinkedTokenSource(ct);
        var token = _pollCts.Token;

        _ = Task.Run(async () =>
        {
            // Strategy: gom vùng bit và word/dword riêng để tối ưu
            while (!token.IsCancellationRequested && _connected)
            {
                try
                {
                    // Đơn giản: chỉ đọc word; nếu có bit thì đọc riêng
                    var wordsAreas = options.Areas;
                    ushort[]? words = null;
                    bool[]? bits = null;

                    // (Minh hoạ) Đọc tất cả vùng word liên tiếp bằng cách gộp, bạn có thể tối ưu thêm theo thiết bị.
                    if (wordsAreas.Length > 0)
                    {
                        // Đây đọc lần lượt từng vùng cho đơn giản (dễ hiểu; bạn có thể hợp nhất để giảm gói)
                        var aggWords = new System.Collections.Generic.List<ushort>();
                        foreach (var (Device, Start, Count) in wordsAreas)
                        {
                            if (Device == DeviceCode.X || Device == DeviceCode.Y || Device == DeviceCode.M ||
                                Device == DeviceCode.L || Device == DeviceCode.S || Device == DeviceCode.B)
                            {
                                // Bit area
                                var b = await ReadBitsAsync(Device, Start, Count, token);
                                // Nối bit (minh hoạ): không ghép; đẩy riêng
                                bits = b;
                            }
                            else
                            {
                                var w = await ReadWordsAsync(Device, Start, Count, token);
                                aggWords.AddRange(w);
                            }
                        }
                        if (aggWords.Count > 0) words = [.. aggWords];
                    }

                    Polled?.Invoke(this, new PollUpdatedEventArgs
                    {
                        Timestamp = DateTimeOffset.UtcNow,
                        Bits = bits,
                        Words = words,
                        DWords = null
                    });

                    await Task.Delay(options.Interval, token);
                }
                catch (OperationCanceledException) { }
                catch
                {
                    // bỏ qua vòng lỗi và tiếp tục (tuỳ nhu cầu: bạn có thể tăng backoff)
                    await Task.Delay(TimeSpan.FromMilliseconds(50), token);
                }
            }
        }, token);
    }

    public Task StopPollingAsync(CancellationToken ct = default)
    {
        StopPollingInternal();
        return Task.CompletedTask;
    }

    // ====== Internal helpers ======

    private void StopPollingInternal()
    {
        try { _pollCts?.Cancel(); } catch { }
        try { _pollCts?.Dispose(); } catch { }
        _pollCts = null;
    }

    private static async Task<IPAddress> ResolveHostAsync(string host, CancellationToken ct)
    {
        if (IPAddress.TryParse(host, out var ip)) return ip;
        var entry = await Dns.GetHostEntryAsync(host, ct);
        foreach (var addr in entry.AddressList)
            if (addr.AddressFamily == AddressFamily.InterNetwork) return addr;
        throw new InvalidOperationException("Không tìm thấy IPv4 cho host.");
    }

    private void TrackRtt(TimeSpan elapsed)
    {
        var ticks = elapsed.Ticks;
        System.Threading.Interlocked.Add(ref _rttSumTicks, ticks);
        System.Threading.Interlocked.Increment(ref _rttCount);
    }

    private static void EnsureOk(SlmpResponse resp)
    {
        if (resp.EndCode != SlmpEndCode.Completed)
            throw new InvalidOperationException($"SLMP EndCode: 0x{(ushort)resp.EndCode:X4}");
    }

    // ====== SLMP 3E Binary Build/Parse ======

    // Gợi ý subcommand hiện hành (đặt trước khi Build3EFrame): 0x0000=word, 0x0001=bit
    private ushort CurrentSubcommandHint = 0x0000;

    private static byte[] Build3EFrame(IeBasicClientOptions opt, ushort command, ushort subcommand, ReadOnlyMemory<byte> userData)
    {
        // Tài liệu 3E binary (TCP): Subheader 0x5000; (UDP): 0x5400
        // Header:
        // [0-1]  Subheader (LE)
        // [2]    Network No
        // [3]    PC No (để 0x00)
        // [4-5]  Module I/O No (LE)
        // [6]    Multidrop No
        // [7-8]  Data Length (LE)  = 2(timer) + 2(cmd) + 2(subcmd) + payload.Length
        // [9-10] Timer (LE) đơn vị 250 ms hoặc 1? (tuỳ manual; thường 0x0010 ~ 4s). Ở đây lấy theo Timeout ~ ms/10.
        // [11-12] Command (LE)
        // [13-14] Subcommand (LE)
        // [15..] Payload

        var subheader = (opt.Transport == IeBasicTransport.Udp) ? (ushort)0x5400 : (ushort)0x5000;
        var dataLen = (ushort)(2 + 2 + 2 + userData.Length); // timer + cmd + subcmd + payload

        // Timer: convert từ Timeout ~ ms -> giá trị phù hợp. Ở đây minh hoạ: ms/10 (tuỳ manual).
        var timer = (ushort)Math.Clamp((int)(opt.Timeout.TotalMilliseconds / 10.0), 1, 0xFFFE);

        var buf = new byte[15 + userData.Length];

        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(0, 2), subheader);
        buf[2] = opt.NetworkNo;
        buf[3] = 0x00; // PC No
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(4, 2), opt.ModuleIoNo);
        buf[6] = opt.MultidropNo;
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(7, 2), dataLen);
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(9, 2), timer);
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(11, 2), command);
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(13, 2), subcommand);
        userData.Span.CopyTo(buf.AsSpan(15));

        return buf;
    }

    private async Task<byte[]> SendAndReceiveAsync(byte[] request, CancellationToken ct)
    {
        await _sendLock.WaitAsync(ct).ConfigureAwait(false);

        try
        {
            if (options.Transport == IeBasicTransport.Udp)
            {
                if (_udp is null) throw new InvalidOperationException("UDP client null.");
                await _udp.SendAsync(request, request.Length);
                var recv = await _udp.ReceiveAsync(ct);
                return recv.Buffer;
            }
            else
            {
                if (_tcpStream is null) throw new InvalidOperationException("TCP stream null.");

                // Gửi request
                await _tcpStream.WriteAsync(request, ct);
                await _tcpStream.FlushAsync(ct);

                // Đọc header tối thiểu 15 bytes để biết data length
                var header = new byte[15];
                await ReadExactAsync(_tcpStream, header, ct);

                // Lấy data length
                var dataLen = BinaryPrimitives.ReadUInt16LittleEndian(header.AsSpan(7, 2));
                var rest = new byte[dataLen]; // bao gồm timer/cmd/subcmd/endcode/payload (ở response thay timer bằng EndCode)
                await ReadExactAsync(_tcpStream, rest, ct);

                // Gộp lại thành frame đầy đủ cho parser
                var combined = new byte[header.Length + rest.Length];
                Buffer.BlockCopy(header, 0, combined, 0, header.Length);
                Buffer.BlockCopy(rest, 0, combined, header.Length, rest.Length);
                return combined;
            }
        }
        finally
        {
            _sendLock.Release();
        }
    }

    private static async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken ct)
    {
        int read = 0;
        while (read < buffer.Length)
        {
            var n = await stream.ReadAsync(buffer.AsMemory(read, buffer.Length - read), ct);
            if (n == 0) throw new SocketException((int)SocketError.ConnectionReset);
            read += n;
        }
    }

    private static SlmpResponse Parse3EResponse(byte[] frame)
    {
        // Response 3E:
        // [0-1] Subheader
        // [2]   NetworkNo
        // [3]   PCNo
        // [4-5] ModuleIoNo
        // [6]   MultidropNo
        // [7-8] Data Length (LE)
        // [9-10] End Code (LE)   <-- khác request (timer)
        // [11..] Payload
        if (frame.Length < 11)
            throw new InvalidOperationException("Frame quá ngắn.");

        var dataLen = BinaryPrimitives.ReadUInt16LittleEndian(frame.AsSpan(7, 2));
        if (frame.Length < 11 + dataLen)
            throw new InvalidOperationException("Frame không đủ chiều dài.");

        var endCode = (SlmpEndCode)BinaryPrimitives.ReadUInt16LittleEndian(frame.AsSpan(9, 2));
        var payload = new ReadOnlyMemory<byte>(frame, 11, dataLen - 2); // trừ endcode (2B)
        return new SlmpResponse { EndCode = endCode, Payload = payload, Timestamp = DateTimeOffset.UtcNow };
    }

    // ====== Payload builders (3E binary) ======

    private static byte DevCodeBinary(DeviceCode device) => device switch
    {
        // Mã phổ biến trong MC 3E binary (tham chiếu; kiểm tra manual cho chắc theo CPU):
        DeviceCode.X => 0x9C,
        DeviceCode.Y => 0x9D,
        DeviceCode.M => 0x90,
        DeviceCode.L => 0x92,
        DeviceCode.B => 0xA0,
        DeviceCode.S => 0x98,
        DeviceCode.D => 0xA8,
        DeviceCode.W => 0xB4,
        DeviceCode.R => 0xAF,
        DeviceCode.ZR => 0xB0,
        _ => 0x00
    };

    private ReadOnlyMemory<byte> BuildBatchWriteWords(DeviceCode device, int start, ReadOnlySpan<ushort> words)
    {
        CurrentSubcommandHint = 0x0000; // word units

        var buf = new byte[3 + 1 + 2 + words.Length * 2];
        buf[0] = (byte)(start & 0xFF);
        buf[1] = (byte)((start >> 8) & 0xFF);
        buf[2] = (byte)((start >> 16) & 0xFF);
        buf[3] = DevCodeBinary(device);
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(4, 2), (ushort)words.Length);

        var off = 6;
        for (int i = 0; i < words.Length; i++, off += 2)
            BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(off, 2), words[i]);

        return buf;
    }

    private ReadOnlyMemory<byte> BuildBatchWriteBits(DeviceCode device, int start, ReadOnlySpan<bool> bits)
    {
        CurrentSubcommandHint = 0x0001; // bit units

        // Bit packing theo MC (1 bit/byte hoặc 2 bit/byte tuỳ subcmd). Ở đây dùng 1 bit/bit (1 byte/bit) để đơn giản;
        // nhiều PLC chấp nhận. Nếu cần tối ưu, gói bit theo nibble.
        var buf = new byte[3 + 1 + 2 + bits.Length];
        buf[0] = (byte)(start & 0xFF);
        buf[1] = (byte)((start >> 8) & 0xFF);
        buf[2] = (byte)((start >> 16) & 0xFF);
        buf[3] = DevCodeBinary(device);
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(4, 2), (ushort)bits.Length);

        for (int i = 0; i < bits.Length; i++)
            buf[6 + i] = (byte)(bits[i] ? 0x01 : 0x00);

        return buf;
    }

    private static ReadOnlyMemory<byte> BuildRandomRead((DeviceCode Device, int Address)[] points)
    {
        // Format tối giản: [count(2)] + N * (addr(3) + dev(1))
        var buf = new byte[2 + points.Length * 4];
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(0, 2), (ushort)points.Length);
        var off = 2;
        foreach (var (Device, Address) in points)
        {
            buf[off + 0] = (byte)(Address & 0xFF);
            buf[off + 1] = (byte)((Address >> 8) & 0xFF);
            buf[off + 2] = (byte)((Address >> 16) & 0xFF);
            buf[off + 3] = DevCodeBinary(Device);
            off += 4;
        }
        return buf;
    }

    private static ReadOnlyMemory<byte> BuildRandomWrite((DeviceCode Device, int Address)[] points, ReadOnlySpan<ushort> values)
    {
        // [count(2)] + N*(addr(3)+dev(1)+value(2))
        var buf = new byte[2 + points.Length * (4 + 2)];
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(0, 2), (ushort)points.Length);
        var off = 2;
        for (int i = 0; i < points.Length; i++)
        {
            var (Device, Address) = points[i];
            buf[off + 0] = (byte)(Address & 0xFF);
            buf[off + 1] = (byte)((Address >> 8) & 0xFF);
            buf[off + 2] = (byte)((Address >> 16) & 0xFF);
            buf[off + 3] = DevCodeBinary(Device);
            BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(off + 4, 2), values[i]);
            off += 6;
        }
        return buf;
    }

    private static bool[] UnpackBits(ReadOnlySpan<byte> data, int expectedCount)
    {
        // Đơn giản hoá: nếu payload trả về mỗi bit = 1 byte (0x00/0x01)
        // Nếu PLC trả về packed bits, bạn cần giải theo bit-level. Ở đây xử lý cả hai:
        if (data.Length == expectedCount)
        {
            var res = new bool[expectedCount];
            for (int i = 0; i < expectedCount; i++) res[i] = data[i] != 0;
            return res;
        }
        else
        {
            // Giải packed: LSB-first trong từng byte
            var res = new bool[expectedCount];
            int idx = 0;
            for (int b = 0; b < data.Length && idx < expectedCount; b++)
            {
                byte val = data[b];
                for (int bit = 0; bit < 8 && idx < expectedCount; bit++, idx++)
                    res[idx] = ((val >> bit) & 0x01) != 0;
            }
            return res;
        }
    }

    private ReadOnlyMemory<byte> BuildBatchRead(DeviceCode device, int start, int count, bool isBit, bool isDWord = false)
    {
        // Request data:
        // [0-2]  Address (3 bytes, little-endian 24-bit)
        // [3]    Device Code (1 byte)
        // [4-5]  Points/Count (LE, word/bit/dword tuỳ subcmd)
        // [6-7]  Reserved? (tuỳ lệnh) - KHÔNG cần cho 0x0401
        // Ở đây dùng định dạng tối giản: addr(3) + dev(1) + count(2) + subcmd đã set ở header

        if (isDWord)
            CurrentSubcommandHint = 0x0002; // dword units
        else
            CurrentSubcommandHint = isBit ? (ushort)0x0001 : (ushort)0x0000;

        var buf = new byte[3 + 1 + 2];
        // 24-bit little-endian
        buf[0] = (byte)(start & 0xFF);
        buf[1] = (byte)((start >> 8) & 0xFF);
        buf[2] = (byte)((start >> 16) & 0xFF);
        buf[3] = DevCodeBinary(device);
        BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(4, 2), (ushort)count);
        return buf;
    }

    // 5. Hàm kiểm tra device có hỗ trợ đọc dword trực tiếp không (tuỳ CPU, ví dụ D, W, R, ZR thường hỗ trợ)
    private static bool SupportsDirectDWordRead(DeviceCode device)
    {
        return device == DeviceCode.D || device == DeviceCode.W || device == DeviceCode.R || device == DeviceCode.ZR;
    }
}