#nullable enable
#pragma warning disable CS0219 // Retained upstream decoder locals used by debug assertions.
using System;
using System.Collections.Generic;
using System.Linq;
using static HumanoidRetargeterZstd.UnsafeHelper;
using System;
using System.Runtime.InteropServices;
using System.Runtime.CompilerServices;
using System.Runtime.Intrinsics.X86;

namespace HumanoidRetargeterZstd.Unsafe
{
    public static unsafe partial class Methods
    {


        /*-********************************************************
         *  bitStream decoding
         **********************************************************/
        /*! BIT_initDStream() :
         *  Initialize a BIT_DStream_t.
         * `bitD` : a pointer to an already allocated BIT_DStream_t structure.
         * `srcSize` must be the *exact* size of the bitStream, in bytes.
         * @return : size of stream (== srcSize), or an errorCode if a problem is detected
         */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_initDStream(BIT_DStream_t* bitD, void* srcBuffer, nuint srcSize)
        {
            if (srcSize < 1)
            {
                *bitD = new BIT_DStream_t();
                return unchecked((nuint)(-(int)ZSTD_ErrorCode.ZSTD_error_srcSize_wrong));
            }

            bitD->start = (sbyte*)srcBuffer;
            bitD->limitPtr = bitD->start + sizeof(nuint);
            if (srcSize >= (nuint)sizeof(nuint))
            {
                bitD->ptr = (sbyte*)srcBuffer + srcSize - sizeof(nuint);
                bitD->bitContainer = MEM_readLEST(bitD->ptr);
                {
                    byte lastByte = ((byte*)srcBuffer)[srcSize - 1];
                    bitD->bitsConsumed = lastByte != 0 ? 8 - ZSTD_highbit32(lastByte) : 0;
                    if (lastByte == 0)
                        return unchecked((nuint)(-(int)ZSTD_ErrorCode.ZSTD_error_GENERIC));
                }
            }
            else
            {
                bitD->ptr = bitD->start;
                bitD->bitContainer = *(byte*)bitD->start;
                switch (srcSize)
                {
                    case 7:
                        bitD->bitContainer += (nuint)((byte*)srcBuffer)[6] << sizeof(nuint) * 8 - 16;
                        goto case 6;
                    case 6:
                        bitD->bitContainer += (nuint)((byte*)srcBuffer)[5] << sizeof(nuint) * 8 - 24;
                        goto case 5;
                    case 5:
                        bitD->bitContainer += (nuint)((byte*)srcBuffer)[4] << sizeof(nuint) * 8 - 32;
                        goto case 4;
                    case 4:
                        bitD->bitContainer += (nuint)((byte*)srcBuffer)[3] << 24;
                        goto case 3;
                    case 3:
                        bitD->bitContainer += (nuint)((byte*)srcBuffer)[2] << 16;
                        goto case 2;
                    case 2:
                        bitD->bitContainer += (nuint)((byte*)srcBuffer)[1] << 8;
                        goto default;
                    default:
                        break;
                }

                {
                    byte lastByte = ((byte*)srcBuffer)[srcSize - 1];
                    bitD->bitsConsumed = lastByte != 0 ? 8 - ZSTD_highbit32(lastByte) : 0;
                    if (lastByte == 0)
                        return unchecked((nuint)(-(int)ZSTD_ErrorCode.ZSTD_error_corruption_detected));
                }

                bitD->bitsConsumed += (uint)((nuint)sizeof(nuint) - srcSize) * 8;
            }

            return srcSize;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_getMiddleBits(nuint bitContainer, uint start, uint nbBits)
        {
            uint regMask = (uint)(sizeof(nuint) * 8 - 1);
            assert(nbBits < sizeof(uint) * 32 / sizeof(uint));
            if (Bmi2.X64.IsSupported)
            {
                return (nuint)Bmi2.X64.ZeroHighBits(bitContainer >> (int)(start & regMask), nbBits);
            }

            if (Bmi2.IsSupported)
            {
                return Bmi2.ZeroHighBits((uint)(bitContainer >> (int)(start & regMask)), nbBits);
            }

            return (nuint)(bitContainer >> (int)(start & regMask) & ((ulong)1 << (int)nbBits) - 1);
        }

        /*! BIT_lookBits() :
         *  Provides next n bits from local register.
         *  local register is not modified.
         *  On 32-bits, maxNbBits==24.
         *  On 64-bits, maxNbBits==56.
         * @return : value extracted */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_lookBits(BIT_DStream_t* bitD, uint nbBits)
        {
            return BIT_getMiddleBits(bitD->bitContainer, (uint)(sizeof(nuint) * 8) - bitD->bitsConsumed - nbBits, nbBits);
        }

        /*! BIT_lookBitsFast() :
         *  unsafe version; only works if nbBits >= 1 */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
private static nuint BIT_lookBitsFast(BIT_DStream_t* bitD, uint nbBits)
        {
            uint regMask = (uint)(sizeof(nuint) * 8 - 1);
            assert(nbBits >= 1);
            return bitD->bitContainer << (int)(bitD->bitsConsumed & regMask) >> (int)(regMask + 1 - nbBits & regMask);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
private static void BIT_skipBits(BIT_DStream_t* bitD, uint nbBits)
        {
            bitD->bitsConsumed += nbBits;
        }

        /*! BIT_readBits() :
         *  Read (consume) next n bits from local register and update.
         *  Pay attention to not read more than nbBits contained into local register.
         * @return : extracted value. */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_readBits(BIT_DStream_t* bitD, uint nbBits)
        {
            nuint value = BIT_lookBits(bitD, nbBits);
            BIT_skipBits(bitD, nbBits);
            return value;
        }

        /*! BIT_readBitsFast() :
         *  unsafe version; only works if nbBits >= 1 */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_readBitsFast(BIT_DStream_t* bitD, uint nbBits)
        {
            nuint value = BIT_lookBitsFast(bitD, nbBits);
            assert(nbBits >= 1);
            BIT_skipBits(bitD, nbBits);
            return value;
        }

        /*! BIT_reloadDStream_internal() :
         *  Simple variant of BIT_reloadDStream(), with two conditions:
         *  1. bitstream is valid : bitsConsumed <= sizeof(bitD->bitContainer)*8
         *  2. look window is valid after shifted down : bitD->ptr >= bitD->start
         */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static BIT_DStream_status BIT_reloadDStream_internal(BIT_DStream_t* bitD)
        {
            assert(bitD->bitsConsumed <= (uint)(sizeof(nuint) * 8));
            bitD->ptr -= bitD->bitsConsumed >> 3;
            assert(bitD->ptr >= bitD->start);
            bitD->bitsConsumed &= 7;
            bitD->bitContainer = MEM_readLEST(bitD->ptr);
            return BIT_DStream_status.BIT_DStream_unfinished;
        }

        /*! BIT_reloadDStreamFast() :
         *  Similar to BIT_reloadDStream(), but with two differences:
         *  1. bitsConsumed <= sizeof(bitD->bitContainer)*8 must hold!
         *  2. Returns BIT_DStream_overflow when bitD->ptr < bitD->limitPtr, at this
         *     point you must use BIT_reloadDStream() to reload.
         */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
private static BIT_DStream_status BIT_reloadDStreamFast(BIT_DStream_t* bitD)
        {
            if (bitD->ptr < bitD->limitPtr)
                return BIT_DStream_status.BIT_DStream_overflow;
            return BIT_reloadDStream_internal(bitD);
        }

        private static ReadOnlySpan<byte> Span_static_zeroFilled => new byte[]
        {
            0,
            0,
            0,
            0,
            0,
            0,
            0,
            0
        };
        private static nuint* static_zeroFilled => (nuint*)System.Runtime.CompilerServices.Unsafe.AsPointer(ref MemoryMarshal.GetReference(Span_static_zeroFilled));
        /*! BIT_reloadDStream() :
         *  Refill `bitD` from buffer previously set in BIT_initDStream() .
         *  This function is safe, it guarantees it will not never beyond src buffer.
         * @return : status of `BIT_DStream_t` internal register.
         *           when status == BIT_DStream_unfinished, internal register is filled with at least 25 or 57 bits */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static BIT_DStream_status BIT_reloadDStream(BIT_DStream_t* bitD)
        {
            if (bitD->bitsConsumed > (uint)(sizeof(nuint) * 8))
            {
                const nuint zeroFilled = 0;
                bitD->ptr = (sbyte*)&static_zeroFilled[0];
                return BIT_DStream_status.BIT_DStream_overflow;
            }

            assert(bitD->ptr >= bitD->start);
            if (bitD->ptr >= bitD->limitPtr)
            {
                return BIT_reloadDStream_internal(bitD);
            }

            if (bitD->ptr == bitD->start)
            {
                if (bitD->bitsConsumed < (uint)(sizeof(nuint) * 8))
                    return BIT_DStream_status.BIT_DStream_endOfBuffer;
                return BIT_DStream_status.BIT_DStream_completed;
            }

            {
                uint nbBytes = bitD->bitsConsumed >> 3;
                BIT_DStream_status result = BIT_DStream_status.BIT_DStream_unfinished;
                if (bitD->ptr - nbBytes < bitD->start)
                {
                    nbBytes = (uint)(bitD->ptr - bitD->start);
                    result = BIT_DStream_status.BIT_DStream_endOfBuffer;
                }

                bitD->ptr -= nbBytes;
                bitD->bitsConsumed -= nbBytes * 8;
                bitD->bitContainer = MEM_readLEST(bitD->ptr);
                return result;
            }
        }

        /*! BIT_endOfDStream() :
         * @return : 1 if DStream has _exactly_ reached its end (all bits consumed).
         */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static uint BIT_endOfDStream(BIT_DStream_t* DStream)
        {
            return DStream->ptr == DStream->start && DStream->bitsConsumed == (uint)(sizeof(nuint) * 8) ? 1U : 0U;
        }

        /*-********************************************************
         *  bitStream decoding
         **********************************************************/
        /*! BIT_initDStream() :
         *  Initialize a BIT_DStream_t.
         * `bitD` : a pointer to an already allocated BIT_DStream_t structure.
         * `srcSize` must be the *exact* size of the bitStream, in bytes.
         * @return : size of stream (== srcSize), or an errorCode if a problem is detected
         */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_initDStream(ref BIT_DStream_t bitD, void* srcBuffer, nuint srcSize)
        {
            if (srcSize < 1)
            {
                bitD = new BIT_DStream_t();
                return unchecked((nuint)(-(int)ZSTD_ErrorCode.ZSTD_error_srcSize_wrong));
            }

            bitD.start = (sbyte*)srcBuffer;
            bitD.limitPtr = bitD.start + sizeof(nuint);
            if (srcSize >= (nuint)sizeof(nuint))
            {
                bitD.ptr = (sbyte*)srcBuffer + srcSize - sizeof(nuint);
                bitD.bitContainer = MEM_readLEST(bitD.ptr);
                {
                    byte lastByte = ((byte*)srcBuffer)[srcSize - 1];
                    bitD.bitsConsumed = lastByte != 0 ? 8 - ZSTD_highbit32(lastByte) : 0;
                    if (lastByte == 0)
                        return unchecked((nuint)(-(int)ZSTD_ErrorCode.ZSTD_error_GENERIC));
                }
            }
            else
            {
                bitD.ptr = bitD.start;
                bitD.bitContainer = *(byte*)bitD.start;
                switch (srcSize)
                {
                    case 7:
                        bitD.bitContainer += (nuint)((byte*)srcBuffer)[6] << sizeof(nuint) * 8 - 16;
                        goto case 6;
                    case 6:
                        bitD.bitContainer += (nuint)((byte*)srcBuffer)[5] << sizeof(nuint) * 8 - 24;
                        goto case 5;
                    case 5:
                        bitD.bitContainer += (nuint)((byte*)srcBuffer)[4] << sizeof(nuint) * 8 - 32;
                        goto case 4;
                    case 4:
                        bitD.bitContainer += (nuint)((byte*)srcBuffer)[3] << 24;
                        goto case 3;
                    case 3:
                        bitD.bitContainer += (nuint)((byte*)srcBuffer)[2] << 16;
                        goto case 2;
                    case 2:
                        bitD.bitContainer += (nuint)((byte*)srcBuffer)[1] << 8;
                        goto default;
                    default:
                        break;
                }

                {
                    byte lastByte = ((byte*)srcBuffer)[srcSize - 1];
                    bitD.bitsConsumed = lastByte != 0 ? 8 - ZSTD_highbit32(lastByte) : 0;
                    if (lastByte == 0)
                        return unchecked((nuint)(-(int)ZSTD_ErrorCode.ZSTD_error_corruption_detected));
                }

                bitD.bitsConsumed += (uint)((nuint)sizeof(nuint) - srcSize) * 8;
            }

            return srcSize;
        }

        /*! BIT_lookBits() :
         *  Provides next n bits from local register.
         *  local register is not modified.
         *  On 32-bits, maxNbBits==24.
         *  On 64-bits, maxNbBits==56.
         * @return : value extracted */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_lookBits(nuint bitD_bitContainer, uint bitD_bitsConsumed, uint nbBits)
        {
            return BIT_getMiddleBits(bitD_bitContainer, (uint)(sizeof(nuint) * 8) - bitD_bitsConsumed - nbBits, nbBits);
        }

        /*! BIT_lookBitsFast() :
         *  unsafe version; only works if nbBits >= 1 */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
private static nuint BIT_lookBitsFast(nuint bitD_bitContainer, uint bitD_bitsConsumed, uint nbBits)
        {
            uint regMask = (uint)(sizeof(nuint) * 8 - 1);
            assert(nbBits >= 1);
            return bitD_bitContainer << (int)(bitD_bitsConsumed & regMask) >> (int)(regMask + 1 - nbBits & regMask);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
private static void BIT_skipBits(ref uint bitD_bitsConsumed, uint nbBits)
        {
            bitD_bitsConsumed += nbBits;
        }

        /*! BIT_readBits() :
         *  Read (consume) next n bits from local register and update.
         *  Pay attention to not read more than nbBits contained into local register.
         * @return : extracted value. */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_readBits(nuint bitD_bitContainer, ref uint bitD_bitsConsumed, uint nbBits)
        {
            nuint value = BIT_lookBits(bitD_bitContainer, bitD_bitsConsumed, nbBits);
            BIT_skipBits(ref bitD_bitsConsumed, nbBits);
            return value;
        }

        /*! BIT_readBitsFast() :
         *  unsafe version; only works if nbBits >= 1 */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static nuint BIT_readBitsFast(nuint bitD_bitContainer, ref uint bitD_bitsConsumed, uint nbBits)
        {
            nuint value = BIT_lookBitsFast(bitD_bitContainer, bitD_bitsConsumed, nbBits);
            assert(nbBits >= 1);
            BIT_skipBits(ref bitD_bitsConsumed, nbBits);
            return value;
        }

        /*! BIT_reloadDStream() :
         *  Refill `bitD` from buffer previously set in BIT_initDStream() .
         *  This function is safe, it guarantees it will not never beyond src buffer.
         * @return : status of `BIT_DStream_t` internal register.
         *           when status == BIT_DStream_unfinished, internal register is filled with at least 25 or 57 bits */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static BIT_DStream_status BIT_reloadDStream(ref nuint bitD_bitContainer, ref uint bitD_bitsConsumed, ref sbyte* bitD_ptr, sbyte* bitD_start, sbyte* bitD_limitPtr)
        {
            if (bitD_bitsConsumed > (uint)(sizeof(nuint) * 8))
            {
                const nuint zeroFilled = 0;
                bitD_ptr = (sbyte*)&static_zeroFilled[0];
                return BIT_DStream_status.BIT_DStream_overflow;
            }

            assert(bitD_ptr >= bitD_start);
            if (bitD_ptr >= bitD_limitPtr)
            {
                return BIT_reloadDStream_internal(ref bitD_bitContainer, ref bitD_bitsConsumed, ref bitD_ptr, bitD_start);
            }

            if (bitD_ptr == bitD_start)
            {
                if (bitD_bitsConsumed < (uint)(sizeof(nuint) * 8))
                    return BIT_DStream_status.BIT_DStream_endOfBuffer;
                return BIT_DStream_status.BIT_DStream_completed;
            }

            {
                uint nbBytes = bitD_bitsConsumed >> 3;
                BIT_DStream_status result = BIT_DStream_status.BIT_DStream_unfinished;
                if (bitD_ptr - nbBytes < bitD_start)
                {
                    nbBytes = (uint)(bitD_ptr - bitD_start);
                    result = BIT_DStream_status.BIT_DStream_endOfBuffer;
                }

                bitD_ptr -= nbBytes;
                bitD_bitsConsumed -= nbBytes * 8;
                bitD_bitContainer = MEM_readLEST(bitD_ptr);
                return result;
            }
        }

        /*! BIT_reloadDStream_internal() :
         *  Simple variant of BIT_reloadDStream(), with two conditions:
         *  1. bitstream is valid : bitsConsumed <= sizeof(bitD->bitContainer)*8
         *  2. look window is valid after shifted down : bitD->ptr >= bitD->start
         */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static BIT_DStream_status BIT_reloadDStream_internal(ref nuint bitD_bitContainer, ref uint bitD_bitsConsumed, ref sbyte* bitD_ptr, sbyte* bitD_start)
        {
            assert(bitD_bitsConsumed <= (uint)(sizeof(nuint) * 8));
            bitD_ptr -= bitD_bitsConsumed >> 3;
            assert(bitD_ptr >= bitD_start);
            bitD_bitsConsumed &= 7;
            bitD_bitContainer = MEM_readLEST(bitD_ptr);
            return BIT_DStream_status.BIT_DStream_unfinished;
        }

        /*! BIT_endOfDStream() :
         * @return : 1 if DStream has _exactly_ reached its end (all bits consumed).
         */
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static uint BIT_endOfDStream(uint DStream_bitsConsumed, sbyte* DStream_ptr, sbyte* DStream_start)
        {
            return DStream_ptr == DStream_start && DStream_bitsConsumed == (uint)(sizeof(nuint) * 8) ? 1U : 0U;
        }
    }
}