namespace UbloxSC20.UBXDecoder;
public class UbxDecoder
{
    const byte SYNC_CHAR_1 = 0xB5;
    const byte SYNC_CHAR_2 = 0x62;
    const byte CLASS_NAV = 0x02;
    const byte MSG_SFRBX = 0x13; // ID for UBX-RXM-SFRBX

    [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential, Pack = 1)]
    struct UbxHeader
    {
        public byte Sync1;
        public byte Sync2;
        public byte ClassId;
        public byte MsgId;
        public ushort Length;
    }

    [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential, Pack = 1)]
    struct UbxSfrbxMessage
    {
        public UbxHeader Header;
        public byte GnssId;
        public byte SvId;
        public byte SigId;
        public byte FreqId;
        public uint[] LastPayload;
        public byte CkA;
        public byte CkB;
    }

    public class UbxSfrbxResult
    {
        public byte GnssId { get; set; }
        public byte SvId { get; set; }
        public byte SigId { get; set; }
        public byte FreqId { get; set; }
        public uint[] LastPayload { get; set; }

    }

    private bool VerifyChecksum(byte[] data, ushort length, byte ckA, byte ckB)
    {
        byte a = 0, b = 0;
        for (ushort i = 0; i < length; i++)
        {
            a += data[i];
            b += a;
        }
        return (a == ckA) && (b == ckB);
    }

    public UbxSfrbxResult DecodeUbxSfrbx(byte[] buffer)
    {
        if (buffer.Length < System.Runtime.InteropServices.Marshal.SizeOf(typeof(UbxHeader)) + 4)
        {
            Console.WriteLine("Buffer too small!");
            return null;
        }

        UbxHeader header;
        IntPtr headerPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(System.Runtime.InteropServices.Marshal.SizeOf(typeof(UbxHeader)));
        try
        {
            System.Runtime.InteropServices.Marshal.Copy(buffer, 0, headerPtr, System.Runtime.InteropServices.Marshal.SizeOf(typeof(UbxHeader)));
            header = (UbxHeader)System.Runtime.InteropServices.Marshal.PtrToStructure(headerPtr, typeof(UbxHeader));
        }
        finally
        {
            System.Runtime.InteropServices.Marshal.FreeHGlobal(headerPtr);
        }

        // Kiểm tra độ dài thực tế của buffer
        if (buffer.Length < header.Length + System.Runtime.InteropServices.Marshal.SizeOf(typeof(UbxHeader)) + 2)
        {
            Console.WriteLine("Buffer length mismatch!");
            return null;
        }

        // Kiểm tra checksum
        if (!VerifyChecksum(buffer[2..], (ushort)(header.Length + 4), buffer[^2], buffer[^1]))
        {
            Console.WriteLine("Checksum verification failed!");
            return null;
        }

        // Giải mã các trường chính
        UbxSfrbxMessage msg = new UbxSfrbxMessage
        {
            Header = header,
            GnssId = buffer[6],
            SvId = buffer[7],
            SigId = buffer[8],
            FreqId = buffer[9],
            CkA = buffer[^2],
            CkB = buffer[^1]
        };

        // Lấy payload (các byte còn lại sau các trường chính)
        int payloadStartIndex = 14;
        int payloadLength = header.Length - (payloadStartIndex - 6); // Trừ đi các byte đã ánh xạ
        uint[] lastPayload = new uint[payloadLength / 4];

        for (int i = 0; i < lastPayload.Length; i++)
        {
            lastPayload[i] = BitConverter.ToUInt32(buffer, payloadStartIndex + i * 4);
        }

        // Trả về kết quả
        return new UbxSfrbxResult
        {
            GnssId = msg.GnssId,
            SvId = msg.SvId,
            SigId = msg.SigId,
            FreqId = msg.FreqId,
            LastPayload = lastPayload
        };
    }

    public static byte[] HexStringToByteArray(string hex)
    {
        int numberChars = hex.Length;
        byte[] bytes = new byte[numberChars / 2];
        for (int i = 0; i < numberChars; i += 2)
        {
            bytes[i / 2] = Convert.ToByte(hex.Substring(i, 2), 16);
        }
        return bytes;
    }

}