V2GDecoderC/DotNet/EXI/BitStreamExact.cs

/*
 * Copyright (C) 2007-2024 C# Port
 * Original Copyright (C) 2007-2018 Siemens AG
 *
 * Exact BitStream implementation - byte-compatible with OpenV2G C implementation
 * Matches BitInputStream.c and BitOutputStream.c exactly
 */

using System;

namespace V2GDecoderNet.EXI
{
    /// <summary>
    /// Exact bit input stream implementation matching OpenV2G BitInputStream.c
    /// </summary>
    public class BitInputStreamExact
    {
        private readonly BitstreamExact _stream;

        public BitInputStreamExact(byte[] buffer)
        {
            _stream = new BitstreamExact(buffer);
        }

        public BitInputStreamExact(BitstreamExact stream)
        {
            _stream = stream ?? throw new ArgumentNullException(nameof(stream));
        }

        /// <summary>
        /// Read specified number of bits - exact implementation of readBits()
        /// </summary>
        public int ReadBits(int numBits)
        {
            if (numBits < 1 || numBits > 32)
                throw new ArgumentException("Number of bits must be between 1 and 32", nameof(numBits));

            int val = 0;

            while (numBits > 0)
            {
                // If buffer is empty, read next byte
                if (_stream.Capacity == 0)
                {
                    if (_stream.Position >= _stream.Size)
                        return -1; // End of stream

                    _stream.Buffer = _stream.Data[_stream.Position++];
                    _stream.Capacity = EXIConstantsExact.BITS_IN_BYTE;
                }

                // Calculate how many bits to read from current buffer
                int bitsToRead = Math.Min(numBits, _stream.Capacity);
                
                // Extract bits from buffer (from MSB side)
                int mask = (0xFF >> (EXIConstantsExact.BITS_IN_BYTE - bitsToRead));
                int bits = (_stream.Buffer >> (_stream.Capacity - bitsToRead)) & mask;
                
                // Add to result value
                val = (val << bitsToRead) | bits;
                
                // Update state
                _stream.Capacity -= (byte)bitsToRead;
                numBits -= bitsToRead;
            }

            return val;
        }

        /// <summary>
        /// Read single bit - exact implementation
        /// </summary>
        public int ReadBit()
        {
            return ReadBits(1);
        }

        /// <summary>
        /// Read N-bit unsigned integer - exact implementation of decodeNBitUnsignedInteger()
        /// </summary>
        public int ReadNBitUnsignedInteger(int numBits)
        {
            if (numBits == 0) return 0;
            return ReadBits(numBits);
        }

        /// <summary>
        /// Read variable length unsigned integer - exact implementation of decodeUnsignedInteger()
        /// Uses 7-bit continuation encoding exactly like C implementation
        /// </summary>
        public long ReadUnsignedInteger()
        {
            const int MASK_7_BITS = 0x7F;
            const int CONTINUATION_BIT = 0x80;
            
            byte[] maskedOctets = new byte[8]; // Max 8 bytes for 64-bit value
            int i = 0;
            byte b;
            
            // Read continuation bytes exactly like C implementation
            do
            {
                int byteVal = ReadBits(8);
                if (byteVal < 0) throw new InvalidOperationException("Unexpected end of stream");
                
                b = (byte)byteVal;
                maskedOctets[i++] = (byte)(b & MASK_7_BITS);
                
                if (i >= maskedOctets.Length)
                    throw new InvalidOperationException("Variable length integer too long");
                    
            } while ((b & CONTINUATION_BIT) != 0);
            
            // Assemble value from bytes (reverse order) - exact C algorithm
            long value = 0;
            for (int j = i - 1; j >= 0; j--)
            {
                value = (value << 7) | maskedOctets[j];
            }
            
            return value;
        }

        /// <summary>
        /// Read variable length signed integer - exact implementation
        /// </summary>
        public long ReadInteger()
        {
            long magnitude = ReadUnsignedInteger();
            
            // Check sign bit (LSB of magnitude)
            bool isNegative = (magnitude & 1) != 0;
            
            // Remove sign bit and adjust value
            long value = magnitude >> 1;
            
            return isNegative ? -(value + 1) : value;
        }

        /// <summary>
        /// Read 16-bit unsigned integer - exact implementation of decodeUnsignedInteger16()
        /// Uses VC2022 DecoderChannel.c algorithm exactly
        /// VC2022 function name: decodeUnsignedInteger16
        /// </summary>
        public ushort ReadUnsignedInteger16()
        {
            // Console.Error.WriteLine($"🔬 [ReadUnsignedInteger16] Starting at pos={Position}, bit={BitPosition}");
            uint mShift = 0;
            ushort result = 0;
            byte b;
            int iterCount = 0;
            
            do
            {
                // 1. Read the next octet (8 bits)
                b = (byte)ReadBits(8);
                // Console.Error.WriteLine($"🔬 [ReadUnsignedInteger16] Iter {iterCount}: read byte=0x{b:X2}, pos={Position}, bit={BitPosition}");
                
                // 2. Multiply the value of the unsigned number represented by the 7
                // least significant bits of the octet by the current multiplier and add the result to
                // the current value
                ushort addition = (ushort)((b & 127) << (int)mShift);
                result = (ushort)(result + addition);
                // Console.Error.WriteLine($"🔬 [ReadUnsignedInteger16] Iter {iterCount}: (b & 127)={b & 127}, mShift={mShift}, addition={addition}, result={result}");
                
                // 3. Multiply the multiplier by 128
                mShift += 7;
                
                // 4. If the most significant bit of the octet was 1, go back to step 1
                bool continues = (b >> 7) == 1;
                // Console.Error.WriteLine($"🔬 [ReadUnsignedInteger16] Iter {iterCount}: MSB={(b >> 7)}, continues={continues}");
                iterCount++;
                
            } while ((b >> 7) == 1);
            
            // Console.Error.WriteLine($"🔬 [ReadUnsignedInteger16] Final result={result}");
            return result;
        }

        /// <summary>
        /// Read 16-bit signed integer using C decodeInteger16 algorithm
        /// First bit is sign bit: 0=positive, 1=negative
        /// For negative: -(magnitude + 1)
        /// </summary>
        public short ReadInteger16()
        {
            // Read sign bit (1 bit)
            bool isNegative = ReadBit() != 0;
            
            // Read unsigned magnitude
            uint magnitude = (uint)ReadUnsignedInteger();
            
            if (isNegative)
            {
                return (short)(-(magnitude + 1));
            }
            else
            {
                return (short)magnitude;
            }
        }

        public bool IsEndOfStream => _stream.Position >= _stream.Size && _stream.Capacity == 0;
        
        public int Position => _stream.Position;
        public int BitPosition => EXIConstantsExact.BITS_IN_BYTE - _stream.Capacity;

        /// <summary>
        /// Get remaining bytes from current position
        /// </summary>
        public byte[] GetRemainingBytes()
        {
            int remainingBits = _stream.Capacity;
            int currentBytePos = Position;
            
            if (remainingBits > 0)
            {
                // If there are remaining bits in current byte, we need to include it
                currentBytePos--;
            }
            
            int remainingByteCount = _stream.Size - currentBytePos;
            if (remainingByteCount <= 0) return new byte[0];
            
            byte[] remaining = new byte[remainingByteCount];
            Array.Copy(_stream.Data, currentBytePos, remaining, 0, remainingByteCount);
            return remaining;
        }
    }

    /// <summary>
    /// Exact bit output stream implementation matching OpenV2G BitOutputStream.c
    /// </summary>
    public class BitOutputStreamExact
    {
        private readonly BitstreamExact _stream;

        public BitOutputStreamExact(int capacity = EXIConstantsExact.BUFFER_SIZE)
        {
            _stream = new BitstreamExact(capacity);
        }

        public BitOutputStreamExact(BitstreamExact stream)
        {
            _stream = stream ?? throw new ArgumentNullException(nameof(stream));
        }

        /// <summary>
        /// Write specified number of bits - EXACT implementation matching VC2022 writeBits()
        /// Based on BitOutputStream.c lines 40-108 - BYTE FOR BYTE IDENTICAL
        /// VC2022 function name: writeBits
        /// </summary>
        public void writeBits(int numBits, int val)
        {
            if (numBits < 1 || numBits > 32)
                throw new ArgumentException("Number of bits must be between 1 and 32", nameof(numBits));

            // Console.Error.WriteLine($"🔬 [writeBits] ENTRY: pos={_stream.Position}, nbits={numBits}, val={val:X}, capacity={_stream.Capacity}, buffer=0x{_stream.Buffer:X2}");

            // VC2022 line 43: if (nbits <= stream->capacity)
            if (numBits <= _stream.Capacity)
            {
                // Console.Error.WriteLine($"🔬 [writeBits] Using single-byte path (nbits <= capacity)");
                // VC2022 line 45: stream->buffer = (uint8_t)(stream->buffer << (nbits)) | (uint8_t)(val & (uint32_t)(0xff >> (uint32_t)(BITS_IN_BYTE - nbits)));
                uint mask = (uint)(0xFF >> (EXIConstantsExact.BITS_IN_BYTE - numBits));
                // Console.Error.WriteLine($"🔬 [writeBits] mask=0x{mask:X2}");
                if (_stream.Position >= 28 && _stream.Position <= 35 && _stream.Capacity == 1 && numBits == 1)
                {
                    Console.Error.WriteLine($"🔍 [writeBits] LAST BIT: pos={_stream.Position}, cap={_stream.Capacity}, buf=0x{_stream.Buffer:X2}, val={val}, writing to LSB");
                }
                _stream.Buffer = (byte)((_stream.Buffer << numBits) | (val & mask));
                // Console.Error.WriteLine($"🔬 [writeBits] new buffer=0x{_stream.Buffer:X2}");
                
                // VC2022 line 46: stream->capacity = (uint8_t)(stream->capacity - nbits);
                _stream.Capacity = (byte)(_stream.Capacity - numBits);
                // Console.Error.WriteLine($"🔬 [writeBits] new capacity={_stream.Capacity}");
                
                // VC2022 line 48: if (stream->capacity == 0)
                if (_stream.Capacity == 0)
                {
                    // Console.Error.WriteLine($"🔬 [writeBits] Flushing buffer 0x{_stream.Buffer:X2} to position {_stream.Position}");
                    // VC2022 line 53: stream->data[(*stream->pos)++] = stream->buffer;
                    if (_stream.Position >= _stream.Size)
                        throw new InvalidOperationException("Output buffer overflow");
                    _stream.Data[_stream.Position++] = _stream.Buffer;
                    
                    // VC2022 line 61-62: stream->capacity = BITS_IN_BYTE; stream->buffer = 0;
                    _stream.Capacity = EXIConstantsExact.BITS_IN_BYTE;
                    _stream.Buffer = 0;
                }
            }
            else
            {
                // VC2022 line 67-68: stream->buffer = (uint8_t)(stream->buffer << stream->capacity) | ( (uint8_t)(val >> (nbits - stream->capacity)) & (uint8_t)(0xff >> (BITS_IN_BYTE - stream->capacity)) );
                if (_stream.Position >= 28 && _stream.Position <= 35)
                {
                    Console.Error.WriteLine($"🔍 [writeBits] BOUNDARY: pos={_stream.Position}, cap={_stream.Capacity}, buf=0x{_stream.Buffer:X2}, val={val}, nbits={numBits}");
                    Console.Error.WriteLine($"🔍 [writeBits] shift_amount={numBits - _stream.Capacity}, val_shifted={(byte)(val >> (numBits - _stream.Capacity))}");
                }
                _stream.Buffer = (byte)((_stream.Buffer << _stream.Capacity) |
                    (((byte)(val >> (numBits - _stream.Capacity))) & (byte)(0xFF >> (EXIConstantsExact.BITS_IN_BYTE - _stream.Capacity))));
                
                // VC2022 line 70: nbits = (nbits - stream->capacity);
                numBits = numBits - _stream.Capacity;
                
                // VC2022 line 75: stream->data[(*stream->pos)++] = stream->buffer;
                if (_stream.Position >= _stream.Size)
                    throw new InvalidOperationException("Output buffer overflow");
                if (_stream.Position >= 28 && _stream.Position <= 35)
                    Console.Error.WriteLine($"🔍 [writeBits] Writing byte 0x{_stream.Buffer:X2} to position {_stream.Position}");
                _stream.Data[_stream.Position++] = _stream.Buffer;
                
                // VC2022 line 83: stream->buffer = 0;
                _stream.Buffer = 0;
                
                // VC2022 line 86-92: while (errn == 0 && nbits >= BITS_IN_BYTE)
                while (numBits >= EXIConstantsExact.BITS_IN_BYTE)
                {
                    // VC2022 line 87: nbits = (nbits - BITS_IN_BYTE);
                    numBits = numBits - EXIConstantsExact.BITS_IN_BYTE;
                    
                    // VC2022 line 92: stream->data[(*stream->pos)++] = (uint8_t)(val >> (nbits));
                    if (_stream.Position >= _stream.Size)
                        throw new InvalidOperationException("Output buffer overflow");
                    _stream.Data[_stream.Position++] = (byte)(val >> numBits);
                }
                
                // VC2022 line 103-104: stream->buffer = (uint8_t)val; stream->capacity = (uint8_t)(BITS_IN_BYTE - (nbits));
                _stream.Buffer = (byte)val; // Note: the high bits will be shifted out during further filling
                _stream.Capacity = (byte)(EXIConstantsExact.BITS_IN_BYTE - numBits);
            }
        }

        /// <summary>
        /// Write single bit - exact implementation
        /// </summary>
        public void WriteBit(int bit)
        {
            if (Position >= 28 && Position <= 35)
                Console.Error.WriteLine($"🔍 [WriteBit] pos={Position}:{BitPosition}, bit={bit}");
            writeBits(1, bit);
        }
        
        /// <summary>
        /// Compatibility wrapper - keep C# naming for internal use
        /// </summary>
        public void WriteBits(int numBits, int val)
        {
            if (Position >= 28 && Position <= 45)
                Console.Error.WriteLine($"🔍 [WriteBits] pos={Position}, writing {numBits} bits, val={val:X}");
            writeBits(numBits, val);
            if (Position >= 28 && Position <= 45)
                Console.Error.WriteLine($"🔍 [WriteBits] pos after={Position}");
        }

        /// <summary>
        /// Write N-bit unsigned integer - exact implementation of encodeNBitUnsignedInteger()
        /// VC2022 function name: encodeNBitUnsignedInteger
        /// </summary>
        public void encodeNBitUnsignedInteger(int numBits, int val)
        {
            if (numBits > 0)
            {
                if (Position >= 28 && Position <= 35)
                    Console.Error.WriteLine($"🔍 [encodeNBit] pos={Position}:{BitPosition}, writing {numBits} bits, val={val}");
                writeBits(numBits, val);
                // Console.Error.WriteLine($"🔬 [encodeNBit] After write pos_after={Position}, buf=0x{BufferState:X2}, cap={CapacityState}");
            }
        }
        
        /// <summary>
        /// Compatibility wrapper - keep C# naming for internal use
        /// </summary>
        /// <summary>
        /// Legacy C# style alias for backward compatibility
        /// </summary>
        public void WriteNBitUnsignedInteger(int numBits, int val) => encodeNBitUnsignedInteger(numBits, val);

        /// <summary>
        /// Compatibility wrapper - keep C# naming for internal use
        /// </summary>
        public void WriteUnsignedInteger16(ushort val) => encodeUnsignedInteger16(val);
        
        /// <summary>
        /// Helper method - exact implementation of numberOf7BitBlocksToRepresent()
        /// </summary>
        private byte NumberOf7BitBlocksToRepresent(ushort n)
        {
            if (n < 128) return 1;
            if (n < 16384) return 2; // 128 * 128 = 16384
            return 3;
        }

        /// <summary>
        /// Number of 7-bit blocks needed to represent a value - exact VC2022 algorithm
        /// </summary>
        private static byte NumberOf7BitBlocksToRepresent(uint n)
        {
            /* 7 bits */
            if (n < 128) {
                return 1;
            }
            /* 14 bits */
            else if (n < 16384) {
                return 2;
            }
            /* 21 bits */
            else if (n < 2097152) {
                return 3;
            }
            /* 28 bits */
            else if (n < 268435456) {
                return 4;
            }
            /* 35 bits */
            else {
                /* int, 32 bits */
                return 5;
            }
        }

        /// <summary>
        /// Encode unsigned integer using VC2022 encodeUnsignedInteger32 exact algorithm
        /// </summary>
        public void encodeUnsignedInteger32(uint n)
        {
            if (n < 128)
            {
                // Write byte as is
                WriteBits(8, (byte)n);
            }
            else
            {
                byte n7BitBlocks = NumberOf7BitBlocksToRepresent(n);

                switch (n7BitBlocks)
                {
                    case 5:
                        WriteBits(8, (byte)(128 | n));
                        n = n >> 7;
                        goto case 4;
                    case 4:
                        WriteBits(8, (byte)(128 | n));
                        n = n >> 7;
                        goto case 3;
                    case 3:
                        WriteBits(8, (byte)(128 | n));
                        n = n >> 7;
                        goto case 2;
                    case 2:
                        WriteBits(8, (byte)(128 | n));
                        n = n >> 7;
                        goto case 1;
                    case 1:
                        // 0 .. 7 (last byte)
                        WriteBits(8, (byte)(0 | n));
                        break;
                }
            }
        }

        /// <summary>
        /// Encode unsigned integer using VC2022 encodeUnsignedInteger16 exact algorithm
        /// </summary>
        public void encodeUnsignedInteger16(ushort n)
        {
            // if (n == 471) Console.Error.WriteLine($"🔍 [encodeUnsignedInteger16] Encoding 471, pos={Position}");
            
            if (n < 128)
            {
                // Write byte as is
                // if (n == 471) Console.Error.WriteLine($"🔍 [encodeUnsignedInteger16] 471 < 128, writing {n}");
                WriteBits(8, (byte)n);
            }
            else
            {
                byte n7BitBlocks = NumberOf7BitBlocksToRepresent(n);
                // if (n == 471) Console.Error.WriteLine($"🔍 [encodeUnsignedInteger16] 471 >= 128, n7BitBlocks={n7BitBlocks}");

                switch (n7BitBlocks)
                {
                    case 3:
                        // if (n == 471) Console.Error.WriteLine($"🔍 [encodeUnsignedInteger16] case 3: writing {(byte)(128 | n)} = {128 | n}");
                        WriteBits(8, (byte)(128 | n));
                        n = (ushort)(n >> 7);
                        goto case 2;
                    case 2:
                        // if (n == 471) Console.Error.WriteLine($"🔍 [encodeUnsignedInteger16] case 2: writing {(byte)(128 | n)} = {128 | n}");
                        WriteBits(8, (byte)(128 | n));
                        n = (ushort)(n >> 7);
                        // if (n == 3) Console.Error.WriteLine($"🔍 [encodeUnsignedInteger16] after >>7, n=3, going to case 1");
                        goto case 1;
                    case 1:
                        // 0 .. 7 (last byte)
                        // if (n == 3) Console.Error.WriteLine($"🔍 [encodeUnsignedInteger16] case 1: writing final {(byte)(0 | n)} = {0 | n}");
                        WriteBits(8, (byte)(0 | n));
                        break;
                }
            }
        }

        /// <summary>
        /// Write variable length unsigned integer - exact implementation of encodeUnsignedInteger()
        /// Uses 7-bit continuation encoding exactly like C implementation
        /// </summary>
        public void WriteUnsignedInteger(long val)
        {
            if (val < 0)
                throw new ArgumentException("Value must be non-negative", nameof(val));
            
            // Use VC2022 exact algorithm for 32-bit values
            if (val <= uint.MaxValue)
            {
                encodeUnsignedInteger32((uint)val);
                return;
            }

            const int MASK_7_BITS = 0x7F;
            const int CONTINUATION_BIT = 0x80;
            
            // Handle zero as special case
            if (val == 0)
            {
                WriteBits(8, 0);
                return;
            }
            
            // Split into 7-bit chunks with continuation bits - exact C algorithm
            byte[] bytes = new byte[10]; // Max bytes needed for 64-bit value
            int numBytes = 0;
            
            while (val > 0)
            {
                byte chunk = (byte)(val & MASK_7_BITS);
                val >>= 7;
                
                // Set continuation bit if more bytes follow
                if (val > 0)
                    chunk |= CONTINUATION_BIT;
                    
                bytes[numBytes++] = chunk;
            }
            
            // Write bytes in forward order
            for (int i = 0; i < numBytes; i++)
            {
                WriteBits(8, bytes[i]);
            }
        }

        /// <summary>
        /// Write variable length signed integer - exact implementation
        /// </summary>
        public void WriteInteger(long val)
        {
            // Encode sign in LSB and magnitude in remaining bits
            bool isNegative = val < 0;
            long magnitude = isNegative ? (-val - 1) : val;
            
            // Shift magnitude left and set sign bit
            long encodedValue = (magnitude << 1) | (isNegative ? 1 : 0);
            
            WriteUnsignedInteger(encodedValue);
        }

        /// <summary>
        /// Write 16-bit signed integer using VC2022 encodeInteger16 algorithm
        /// First bit is sign bit: 0=positive, 1=negative
        /// For negative: -(magnitude + 1)
        /// Exactly matches VC2022's encodeInteger16() implementation
        /// </summary>
        public void WriteInteger16(short val)
        {
            Console.Error.WriteLine($"🔢 [WriteInteger16] Input: {val}");
            
            // Write sign bit (1 bit)  
            bool isNegative = val < 0;
            WriteBit(isNegative ? 1 : 0);
            Console.Error.WriteLine($"🔢 [WriteInteger16] Sign bit: {(isNegative ? 1 : 0)} (negative: {isNegative})");
            
            // Calculate unsigned magnitude
            uint magnitude;
            if (isNegative)
            {
                // For negative: magnitude = (-val) - 1
                magnitude = (uint)((-val) - 1);
            }
            else
            {
                // For positive: magnitude = val
                magnitude = (uint)val;
            }
            
            Console.Error.WriteLine($"🔢 [WriteInteger16] Magnitude: {magnitude}");
            
            // Write unsigned magnitude using VC2022's encodeUnsignedInteger16 
            encodeUnsignedInteger16((ushort)magnitude);
        }

        /// <summary>
        /// Flush remaining bits - exact implementation of VC2022 flush()
        /// VC2022: if (stream->capacity == BITS_IN_BYTE) { /* nothing */ } else { writeBits(stream, stream->capacity, 0); }
        /// </summary>
        public void Flush()
        {
            // Console.Error.WriteLine($"🔍 [Flush] capacity={_stream.Capacity}, BITS_IN_BYTE={EXIConstantsExact.BITS_IN_BYTE}");
            // VC2022 exact implementation
            if (_stream.Capacity == EXIConstantsExact.BITS_IN_BYTE)
            {
                // nothing to do, no bits in buffer
                // Console.Error.WriteLine($"🔍 [Flush] nothing to do");
            }
            else
            {
                // errn = writeBits(stream, stream->capacity, 0);
                // Console.Error.WriteLine($"🔍 [Flush] calling writeBits({_stream.Capacity}, 0)");
                writeBits(_stream.Capacity, 0);
            }
        }

        /// <summary>
        /// Reset buffer state - exact match to VC2022 writeEXIHeader initialization
        /// stream->buffer = 0; stream->capacity = 8;
        /// </summary>
        public void ResetBuffer()
        {
            _stream.Buffer = 0;
            _stream.Capacity = 8;
        }

        public byte[] ToArray()
        {
            // VC2022 equivalent: encodeFinish() calls flush()
            Flush();
            return _stream.ToArray();
        }
        
        public int Position => _stream.Position;
        public int BitPosition => EXIConstantsExact.BITS_IN_BYTE - _stream.Capacity;
        public byte BufferState => _stream.Buffer;
        public byte CapacityState => _stream.Capacity;
    }
}