V2GDecoderC/Port/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 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
        /// </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));

            // VC2022 exact logic: check if all bits fit in current buffer
            if (numBits <= _stream.Capacity)
            {
                // Simple case: all bits fit into current buffer
                uint mask = (uint)(0xFF >> (EXIConstantsExact.BITS_IN_BYTE - numBits));
                _stream.Buffer = (byte)((_stream.Buffer << numBits) | (val & mask));
                _stream.Capacity = (byte)(_stream.Capacity - numBits);
                
                // If buffer is full, write byte
                if (_stream.Capacity == 0)
                {
                    if (_stream.Position >= _stream.Size)
                        throw new InvalidOperationException("Output buffer overflow");
                        
                    _stream.Data[_stream.Position++] = _stream.Buffer;
                    _stream.Capacity = EXIConstantsExact.BITS_IN_BYTE;
                    _stream.Buffer = 0;
                }
            }
            else
            {
                // Complex case: buffer is not enough - EXACT VC2022 implementation
                
                // 1) Fill current buffer
                uint fillMask = (uint)(0xFF >> (EXIConstantsExact.BITS_IN_BYTE - _stream.Capacity));
                _stream.Buffer = (byte)((_stream.Buffer << _stream.Capacity) |
                    ((val >> (numBits - _stream.Capacity)) & fillMask));
                
                numBits -= _stream.Capacity;
                
                // Write filled buffer
                if (_stream.Position >= _stream.Size)
                    throw new InvalidOperationException("Output buffer overflow");
                _stream.Data[_stream.Position++] = _stream.Buffer;
                _stream.Buffer = 0;
                
                // 2) Write whole bytes - EXACT VC2022 algorithm
                while (numBits >= EXIConstantsExact.BITS_IN_BYTE)
                {
                    numBits -= EXIConstantsExact.BITS_IN_BYTE;
                    
                    if (_stream.Position >= _stream.Size)
                        throw new InvalidOperationException("Output buffer overflow");
                    _stream.Data[_stream.Position++] = (byte)(val >> numBits);
                }
                
                // 3) Store remaining bits in buffer - VC2022 critical logic
                _stream.Buffer = (byte)val; // Note: 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)
        {
            WriteBits(1, bit);
        }

        /// <summary>
        /// Write N-bit unsigned integer - exact implementation
        /// </summary>
        public void WriteNBitUnsignedInteger(int numBits, int val)
        {
            if (numBits > 0)
                WriteBits(numBits, val);
        }

        /// <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)
        {
            const int MASK_7_BITS = 0x7F;
            const int CONTINUATION_BIT = 0x80;
            
            if (val < 0)
                throw new ArgumentException("Value must be non-negative", nameof(val));
            
            // 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)
        {
            int posBefore = _stream.Position;
            Console.Error.WriteLine($"🔬 [WriteInteger16] val={val}, pos_before={posBefore}");
            
            // Write sign bit (1 bit)
            bool isNegative = val < 0;
            WriteBit(isNegative ? 1 : 0);
            
            // Calculate unsigned magnitude
            uint magnitude;
            if (isNegative)
            {
                // For negative: magnitude = (-val) - 1
                magnitude = (uint)((-val) - 1);
            }
            else
            {
                // For positive: magnitude = val
                magnitude = (uint)val;
            }
            
            // Write unsigned magnitude using variable length encoding
            WriteUnsignedInteger(magnitude);
            
            int posAfter = _stream.Position;
            Console.Error.WriteLine($"🔬 [WriteInteger16] val={val}, pos_after={posAfter}, used_bytes={posAfter - posBefore}");
        }

        /// <summary>
        /// Flush remaining bits - exact implementation of flush()
        /// </summary>
        public void Flush()
        {
            // If there are remaining bits in buffer, flush with zero padding
            if (_stream.Capacity < EXIConstantsExact.BITS_IN_BYTE)
            {
                // Shift remaining bits to MSB and write
                byte paddedBuffer = (byte)(_stream.Buffer << _stream.Capacity);
                
                if (_stream.Position >= _stream.Size)
                    throw new InvalidOperationException("Output buffer overflow");
                    
                _stream.Data[_stream.Position++] = paddedBuffer;
                _stream.Buffer = 0;
                _stream.Capacity = EXIConstantsExact.BITS_IN_BYTE;
            }
        }

        public byte[] ToArray()
        {
            return _stream.ToArray();
        }
        
        public int Position => _stream.Position;
        public int BitPosition => EXIConstantsExact.BITS_IN_BYTE - _stream.Capacity;
    }
}