package ccsds124

// BitBuffer is a variable-length bit buffer for building compressed output.
//
// Bits are appended sequentially using MSB-first ordering as required by
// CCSDS 124.0-B-1.
//
// Bit Ordering:
//   - First bit appended goes to bit position 7
//   - Second bit goes to position 6, etc.
type BitBuffer struct {
        data    []byte
        numBits int
        // Accumulator for batch bit operations
        acc    uint64 // accumulate up to 64 bits
        accLen int    // number of bits in accumulator
}

// NewBitBuffer creates a new empty bit buffer.
func NewBitBuffer() *BitBuffer {
        return &BitBuffer{
                data:    make([]byte, 0, 256), // Pre-allocate reasonable capacity
                numBits: 0,
        }
}

// Clear resets the buffer to empty.
func (bb *BitBuffer) Clear() {
        bb.data = bb.data[:0]
        bb.numBits = 0
        bb.acc = 0
        bb.accLen = 0
}

// NumBits returns the number of bits in the buffer.
func (bb *BitBuffer) NumBits() int {
        return bb.numBits
}

// flushAcc writes accumulated bits to the data slice.
func (bb *BitBuffer) flushAcc() {
        for bb.accLen >= 8 {
                // Extract top 8 bits
                bb.accLen -= 8
                b := byte(bb.acc >> bb.accLen)
                bb.data = append(bb.data, b)
                bb.acc &= (1 << bb.accLen) - 1 // clear extracted bits
        }
}

// AppendBit appends a single bit to the buffer.
func (bb *BitBuffer) AppendBit(bit int) {
        bb.acc = (bb.acc << 1) | uint64(bit&1)
        bb.accLen++
        bb.numBits++

        // Flush when accumulator has 8+ bits
        if bb.accLen >= 8 {
                bb.flushAcc()
        }
}

// AppendBitsFromWord appends n bits from a 32-bit word (MSB-first).
// The bits are taken from the top n bits of the word.
func (bb *BitBuffer) AppendBitsFromWord(word uint32, n int) {
        if n <= 0 {
                return
        }
        // Shift word so the n bits are at the top
        bits := uint64(word >> (32 - n))
        bb.acc = (bb.acc << n) | bits
        bb.accLen += n
        bb.numBits += n

        // Flush complete bytes
        if bb.accLen >= 8 {
                bb.flushAcc()
        }
}

// AppendBits appends multiple bits from bytes.
// Bits are read MSB-first from the source data.
func (bb *BitBuffer) AppendBits(data []byte, numBits int) {
        for i := 0; i < numBits; i++ {
                byteIndex := i / 8
                bitIndex := i % 8

                // Extract bits MSB-first (bit 7, 6, 5, ..., 0)
                bit := (data[byteIndex] >> (7 - bitIndex)) & 1
                bb.AppendBit(int(bit))
        }
}

// AppendBitVector appends all bits from a BitVector.
func (bb *BitBuffer) AppendBitVector(bv *BitVector) {
        // Calculate number of bytes from bit length
        numBytes := (bv.length + 7) / 8

        // CCSDS MSB-first: bytes in order, bits within each byte from MSB to LSB
        for byteIdx := 0; byteIdx < numBytes; byteIdx++ {
                bitsInThisByte := 8

                // Last byte may have fewer than 8 bits
                if byteIdx == numBytes-1 {
                        remainder := bv.length % 8
                        if remainder != 0 {
                                bitsInThisByte = remainder
                        }
                }

                // With MSB-first bitvector indexing: bit 0 is MSB, bit 7 is LSB
                // We want to append bits in order: MSB first, LSB last
                startBit := byteIdx * 8
                for bitOffset := 0; bitOffset < bitsInThisByte; bitOffset++ {
                        pos := startBit + bitOffset
                        bit := bv.GetBit(pos)
                        bb.AppendBit(bit)
                }
        }
}

// AppendBitVectorN appends the first n bits from a BitVector.
func (bb *BitBuffer) AppendBitVectorN(bv *BitVector, n int) {
        if n > bv.length {
                n = bv.length
        }
        for i := 0; i < n; i++ {
                bb.AppendBit(bv.GetBit(i))
        }
}

// AppendValue appends a value as count bits (MSB-first).
func (bb *BitBuffer) AppendValue(value uint64, count int) {
        if count <= 0 {
                return
        }
        // Mask to get only the bottom 'count' bits
        mask := uint64((1 << count) - 1)
        bb.acc = (bb.acc << count) | (value & mask)
        bb.accLen += count
        bb.numBits += count

        if bb.accLen >= 8 {
                bb.flushAcc()
        }
}

// ToBytes converts buffer contents to bytes.
func (bb *BitBuffer) ToBytes() []byte {
        if bb.numBits == 0 {
                return []byte{}
        }

        // Flush any remaining bits in accumulator
        numBytes := (bb.numBits + 7) / 8

        // Make sure we have enough space
        for len(bb.data) < numBytes {
                // Pad accumulator and flush
                if bb.accLen > 0 {
                        // Pad to byte boundary
                        padBits := 8 - (bb.accLen % 8)
                        if padBits < 8 {
                                bb.acc <<= padBits
                                bb.accLen += padBits
                        }
                        bb.flushAcc()
                } else {
                        bb.data = append(bb.data, 0)
                }
        }

        // Handle any remaining bits in accumulator (< 8 bits)
        if bb.accLen > 0 {
                // Pad to byte boundary and add
                padBits := 8 - bb.accLen
                lastByte := byte(bb.acc << padBits)
                if len(bb.data) < numBytes {
                        bb.data = append(bb.data, lastByte)
                } else {
                        bb.data[numBytes-1] = lastByte
                }
        }

        result := make([]byte, numBytes)
        copy(result, bb.data[:numBytes])
        return result
}

package ccsds124

import (
        "errors"
        "fmt"
)

// ErrEOF is returned when reading past the end of available data.
var ErrEOF = errors.New("no more bits to read")

// BitReader provides sequential bit-level reading from bytes.
//
// Bits are read MSB-first within each byte (matching BitBuffer output):
//   - First bit read is bit position 7 (MSB)
//   - Last bit read is bit position 0 (LSB)
type BitReader struct {
        data      []byte
        totalBits int
        position  int
}

// NewBitReader creates a new bit reader from bytes.
func NewBitReader(data []byte) *BitReader {
        return &BitReader{
                data:      data,
                totalBits: len(data) * 8,
                position:  0,
        }
}

// NewBitReaderWithBits creates a new bit reader with a specific bit count.
// This is useful when the data doesn't fill the last byte completely.
func NewBitReaderWithBits(data []byte, numBits int) *BitReader {
        maxBits := len(data) * 8
        if numBits > maxBits {
                numBits = maxBits
        }
        return &BitReader{
                data:      data,
                totalBits: numBits,
                position:  0,
        }
}

// Remaining returns the number of bits remaining to read.
func (br *BitReader) Remaining() int {
        return br.totalBits - br.position
}

// Position returns the current bit position.
func (br *BitReader) Position() int {
        return br.position
}

// PeekBit peeks at the next bit without consuming it.
func (br *BitReader) PeekBit() (int, error) {
        if br.position >= br.totalBits {
                return 0, ErrEOF
        }

        byteIndex := br.position / 8
        bitIndex := br.position % 8

        // MSB-first: bit 0 in stream is bit 7 of first byte
        bit := (br.data[byteIndex] >> (7 - bitIndex)) & 1
        return int(bit), nil
}

// ReadBit reads and consumes a single bit.
func (br *BitReader) ReadBit() (int, error) {
        bit, err := br.PeekBit()
        if err != nil {
                return 0, err
        }
        br.position++
        return bit, nil
}

// ReadBits reads and consumes multiple bits as an integer.
// Bits are returned MSB-first as an integer value.
func (br *BitReader) ReadBits(numBits int) (uint64, error) {
        if numBits == 0 {
                return 0, nil
        }

        if numBits > 64 {
                return 0, fmt.Errorf("cannot read more than 64 bits at once")
        }

        if br.position+numBits > br.totalBits {
                return 0, fmt.Errorf("not enough bits: need %d, have %d", numBits, br.Remaining())
        }

        var result uint64
        for i := 0; i < numBits; i++ {
                bit, err := br.ReadBit()
                if err != nil {
                        return 0, err
                }
                result = (result << 1) | uint64(bit)
        }

        return result, nil
}

// AlignByte advances to the next byte boundary.
// If already at a byte boundary, does nothing.
func (br *BitReader) AlignByte() {
        bitOffset := br.position % 8
        if bitOffset != 0 {
                br.position += 8 - bitOffset
        }
}

// Skip advances the position by the given number of bits.
func (br *BitReader) Skip(numBits int) error {
        if br.position+numBits > br.totalBits {
                return fmt.Errorf("cannot skip %d bits: only %d remaining", numBits, br.Remaining())
        }
        br.position += numBits
        return nil
}

package ccsds124

import (
        "errors"
        "math/bits"
)

// BitVector is a fixed-length bit vector using 32-bit word storage.
//
// Bit Numbering Convention (CCSDS 124.0-B-1 Section 1.6.1):
//   - Bit 0 = MSB (Most Significant Bit, transmitted first)
//   - Bit N-1 = LSB (Least Significant Bit)
//
// Word Packing (Big-Endian):
//   - Word[i] contains Bytes [4i, 4i+1, 4i+2, 4i+3]
//   - Byte 4i at bits 24-31 (most significant)
//   - Byte 4i+3 at bits 0-7 (least significant)
type BitVector struct {
        data     []uint32
        length   int // Length in bits
        numWords int
}

// NewBitVector creates a new bit vector with the specified length in bits.
func NewBitVector(numBits int) (*BitVector, error) {
        if numBits <= 0 {
                return nil, errors.New("numBits must be positive")
        }

        // Calculate number of 32-bit words needed
        numBytes := (numBits + 7) / 8
        numWords := (numBytes + 3) / 4

        return &BitVector{
                data:     make([]uint32, numWords),
                length:   numBits,
                numWords: numWords,
        }, nil
}

// Length returns the length of the bit vector in bits.
func (bv *BitVector) Length() int {
        return bv.length
}

// Zero sets all bits to zero.
func (bv *BitVector) Zero() {
        for i := range bv.data {
                bv.data[i] = 0
        }
}

// Copy creates a copy of this bit vector.
func (bv *BitVector) Copy() *BitVector {
        result := &BitVector{
                data:     make([]uint32, bv.numWords),
                length:   bv.length,
                numWords: bv.numWords,
        }
        copy(result.data, bv.data)
        return result
}

// CopyFrom copies contents from another bit vector into this one.
func (bv *BitVector) CopyFrom(other *BitVector) {
        numWords := bv.numWords
        if other.numWords < numWords {
                numWords = other.numWords
        }
        for i := 0; i < numWords; i++ {
                bv.data[i] = other.data[i]
        }
}

// GetBit returns the bit value at the specified position.
// Bit 0 is the MSB, bit length-1 is the LSB.
func (bv *BitVector) GetBit(pos int) int {
        if pos < 0 || pos >= bv.length {
                return 0
        }

        // Optimized: single division, bit operations for the rest
        // pos/8 = byte index, pos&7 = bit within byte
        // byte/4 = word index, byte&3 = byte within word
        wordIndex := pos >> 5 // pos / 32
        bitInWord := 31 - (pos & 31)

        return int((bv.data[wordIndex] >> bitInWord) & 1)
}

// SetBit sets the bit value at the specified position.
// Bit 0 is the MSB, bit length-1 is the LSB.
func (bv *BitVector) SetBit(pos int, value int) {
        if pos < 0 || pos >= bv.length {
                return
        }

        // Optimized: use bit operations instead of multiple divisions
        wordIndex := pos >> 5 // pos / 32
        bitInWord := 31 - (pos & 31)

        if value != 0 {
                // Set bit to 1
                bv.data[wordIndex] |= 1 << bitInWord
        } else {
                // Clear bit to 0
                bv.data[wordIndex] &= ^(1 << bitInWord)
        }
}

// FromBytes loads the bit vector from bytes (big-endian).
func (bv *BitVector) FromBytes(data []byte) {
        // Zero the array first
        bv.Zero()

        // Pack bytes into 32-bit words (big-endian)
        j := 4 // Counter for bytes within word (4, 3, 2, 1)
        var bytesToInt uint32
        currentWord := 0

        for _, b := range data {
                j--
                bytesToInt |= uint32(b) << (j * 8)

                if j == 0 {
                        // Word complete - store it
                        bv.data[currentWord] = bytesToInt
                        currentWord++
                        bytesToInt = 0
                        j = 4
                }
        }

        // Handle incomplete final word
        if j < 4 {
                bv.data[currentWord] = bytesToInt
        }
}

// ToBytes converts the bit vector to bytes (big-endian).
func (bv *BitVector) ToBytes() []byte {
        expectedBytes := (bv.length + 7) / 8
        result := make([]byte, expectedBytes)

        byteIndex := 0
        for wordIndex := 0; wordIndex < bv.numWords && byteIndex < expectedBytes; wordIndex++ {
                word := bv.data[wordIndex]

                // Extract up to 4 bytes from this word
                for j := 3; j >= 0 && byteIndex < expectedBytes; j-- {
                        result[byteIndex] = byte((word >> (j * 8)) & 0xFF)
                        byteIndex++
                }
        }

        return result
}

// XOR computes the bitwise XOR of this vector with another.
func (bv *BitVector) XOR(other *BitVector) *BitVector {
        result, _ := NewBitVector(bv.length)

        numWords := bv.numWords
        if other.numWords < numWords {
                numWords = other.numWords
        }

        for i := 0; i < numWords; i++ {
                result.data[i] = bv.data[i] ^ other.data[i]
        }

        return result
}

// XORInto computes a XOR b and stores the result in this vector.
func (bv *BitVector) XORInto(a, b *BitVector) {
        numWords := bv.numWords
        if a.numWords < numWords {
                numWords = a.numWords
        }
        if b.numWords < numWords {
                numWords = b.numWords
        }

        for i := 0; i < numWords; i++ {
                bv.data[i] = a.data[i] ^ b.data[i]
        }
}

// OR computes the bitwise OR of this vector with another.
func (bv *BitVector) OR(other *BitVector) *BitVector {
        result, _ := NewBitVector(bv.length)

        numWords := bv.numWords
        if other.numWords < numWords {
                numWords = other.numWords
        }

        for i := 0; i < numWords; i++ {
                result.data[i] = bv.data[i] | other.data[i]
        }

        return result
}

// ORInto computes a OR b and stores the result in this vector.
func (bv *BitVector) ORInto(a, b *BitVector) {
        numWords := bv.numWords
        if a.numWords < numWords {
                numWords = a.numWords
        }
        if b.numWords < numWords {
                numWords = b.numWords
        }

        for i := 0; i < numWords; i++ {
                bv.data[i] = a.data[i] | b.data[i]
        }
}

// AND computes the bitwise AND of this vector with another.
func (bv *BitVector) AND(other *BitVector) *BitVector {
        result, _ := NewBitVector(bv.length)

        numWords := bv.numWords
        if other.numWords < numWords {
                numWords = other.numWords
        }

        for i := 0; i < numWords; i++ {
                result.data[i] = bv.data[i] & other.data[i]
        }

        return result
}

// NOT computes the bitwise NOT (inversion) of this vector.
func (bv *BitVector) NOT() *BitVector {
        result, _ := NewBitVector(bv.length)

        for i := 0; i < bv.numWords; i++ {
                result.data[i] = ^bv.data[i]
        }

        // Mask off unused bits in last word
        if bv.numWords > 0 {
                numBytes := (bv.length + 7) / 8
                bytesInLastWord := ((numBytes - 1) % 4) + 1
                bitsInLastByte := bv.length - ((numBytes - 1) * 8)

                // Create mask for valid bits in big-endian word
                // For MSB-first indexing, valid bits in partial byte are at the high end
                var mask uint32
                for b := 0; b < bytesInLastWord; b++ {
                        var byteMask uint32
                        if b == bytesInLastWord-1 {
                                // Partial byte: create mask for high bits (MSB-first)
                                byteMask = uint32(0xFF) << (8 - bitsInLastByte) & 0xFF
                        } else {
                                byteMask = 0xFF
                        }
                        shiftAmt := (3 - b) * 8
                        mask |= byteMask << shiftAmt
                }
                result.data[bv.numWords-1] &= mask
        }

        return result
}

// LeftShift computes a left shift by 1 position.
// Left shift moves bits toward MSB (lower indices).
// MSB is lost, LSB becomes 0.
func (bv *BitVector) LeftShift() *BitVector {
        result, _ := NewBitVector(bv.length)

        // Word-level left shift (big-endian: MSB in high bits of first word)
        // Shift each word left by 1, carry MSB from next word
        var carry uint32
        for i := bv.numWords - 1; i >= 0; i-- {
                word := bv.data[i]
                result.data[i] = ((word << 1) | carry) & 0xFFFFFFFF
                carry = (word >> 31) & 1
        }

        return result
}

// Reverse returns a new bit vector with reversed bit order.
func (bv *BitVector) Reverse() *BitVector {
        result, _ := NewBitVector(bv.length)

        for i := 0; i < bv.length; i++ {
                bit := bv.GetBit(i)
                result.SetBit((bv.length-1)-i, bit)
        }

        return result
}

// HammingWeight returns the number of 1 bits.
func (bv *BitVector) HammingWeight() int {
        count := 0

        // Count '1' bits in each word using hardware POPCNT
        for i := 0; i < bv.numWords; i++ {
                count += bits.OnesCount32(bv.data[i])
        }

        // Adjust for any extra bits in last word
        numBytes := (bv.length + 7) / 8
        extraBits := (numBytes * 8) - bv.length
        if extraBits > 0 && bv.numWords > 0 {
                // Count bits in the unused portion of the last word and subtract
                lastWord := bv.data[bv.numWords-1]
                mask := uint32((1 << extraBits) - 1)
                count -= bits.OnesCount32(lastWord & mask)
        }

        return count
}

// Equals checks if this bit vector equals another.
func (bv *BitVector) Equals(other *BitVector) bool {
        if bv.length != other.length {
                return false
        }

        for i := 0; i < bv.numWords; i++ {
                if bv.data[i] != other.data[i] {
                        return false
                }
        }

        return true
}

package ccsds124

import (
        "bytes"
        "errors"
)

// Version is the library version.
const Version = "1.0.0"

// ErrNotImplemented is returned when a function is not yet implemented.
var ErrNotImplemented = errors.New("not implemented")

// Compress compresses the input data using CCSDS 124.0-B-1 algorithm.
//
// Parameters:
//   - data: Input bytes to compress (must be multiple of packetSize)
//   - packetSize: Size of each packet in bytes
//   - robustness: Robustness parameter R (1-7)
//   - ptLimit: Period limit for new_mask_flag (pt)
//   - ftLimit: Period limit for send_mask_flag (ft)
//   - rtLimit: Period limit for uncompressed_flag (rt)
//
// Returns compressed data or an error.
func Compress(data []byte, packetSize, robustness, ptLimit, ftLimit, rtLimit int) ([]byte, error) {
        if len(data) == 0 {
                return []byte{}, nil
        }
        if packetSize <= 0 {
                return nil, errors.New("packet size must be positive")
        }
        if len(data)%packetSize != 0 {
                return nil, errors.New("data length must be multiple of packet size")
        }
        if robustness < 1 || robustness > 7 {
                return nil, errors.New("robustness must be between 1 and 7")
        }

        // Convert packet size from bytes to bits
        F := packetSize * 8

        // Create compressor
        comp, err := NewCompressor(F, nil, robustness, ptLimit, ftLimit, rtLimit)
        if err != nil {
                return nil, err
        }

        // Output buffer
        var output bytes.Buffer

        // Number of packets
        numPackets := len(data) / packetSize

        // Compress each packet
        for i := 0; i < numPackets; i++ {
                // Extract packet data
                packetData := data[i*packetSize : (i+1)*packetSize]

                // Create bit vector from packet
                input, err := NewBitVector(F)
                if err != nil {
                        return nil, err
                }
                input.FromBytes(packetData)

                // Determine compression parameters (matching C implementation)
                params := &CompressParams{
                        MinRobustness: robustness,
                }

                if i == 0 {
                        // First packet: fixed init values, counters not checked
                        params.SendMaskFlag = true
                        params.UncompressedFlag = true
                        params.NewMaskFlag = false
                } else {
                        // Packets 1+: check and update countdown counters

                        // ft counter
                        if comp.ftCounter == 1 {
                                params.SendMaskFlag = true
                                comp.ftCounter = ftLimit
                        } else {
                                comp.ftCounter--
                                params.SendMaskFlag = false
                        }

                        // pt counter
                        if comp.ptCounter == 1 {
                                params.NewMaskFlag = true
                                comp.ptCounter = ptLimit
                        } else {
                                comp.ptCounter--
                                params.NewMaskFlag = false
                        }

                        // rt counter
                        if comp.rtCounter == 1 {
                                params.UncompressedFlag = true
                                comp.rtCounter = rtLimit
                        } else {
                                comp.rtCounter--
                                params.UncompressedFlag = false
                        }

                        // Override for remaining init packets: CCSDS requires first Rt+1 packets
                        // to have ft=1, rt=1, pt=0. In 0-indexed: if (i <= Rt)
                        if i <= robustness {
                                params.SendMaskFlag = true
                                params.UncompressedFlag = true
                                params.NewMaskFlag = false
                        }
                }

                // Compress packet
                compressed, err := comp.CompressPacket(input, params)
                if err != nil {
                        return nil, err
                }

                // Append to output
                output.Write(compressed)
        }

        return output.Bytes(), nil
}

package ccsds124

import (
        "errors"
        "fmt"
)

// Constants
const (
        MaxHistory      = 16    // History depth for change vectors
        MaxVtHistory    = 16    // History size for Vt calculation
        MaxRobustness   = 7     // Maximum robustness level
        MaxPacketLength = 65535 // CCSDS 124.0-B-1 section 3.2: 1 <= F <= 2^16 - 1
)

// CompressParams holds compression parameters for a single packet.
type CompressParams struct {
        MinRobustness    int  // Rt: Minimum robustness level (0-7)
        NewMaskFlag      bool // pt: Update mask from build vector
        SendMaskFlag     bool // ft: Include mask in output
        UncompressedFlag bool // rt: Send uncompressed
}

// Compressor maintains state for CCSDS 124.0-B-1 compression.
type Compressor struct {
        // Configuration (immutable after init)
        F          int // Input vector length in bits
        robustness int // Rt: Base robustness level (0-7)

        // Period limits for automatic parameter management
        ptLimit int
        ftLimit int
        rtLimit int

        // State (updated each cycle)
        mask        *BitVector
        prevMask    *BitVector
        build       *BitVector
        prevInput   *BitVector
        initialMask *BitVector

        // Change history (circular buffer)
        changeHistory [MaxHistory]*BitVector
        historyIndex  int

        // Flag history for ct calculation
        newMaskFlagHistory [MaxVtHistory]int
        flagHistoryIndex   int

        // Cycle counter
        t int

        // Countdown counters for automatic mode
        ptCounter int
        ftCounter int
        rtCounter int

        // Pre-allocated working buffers (avoid per-packet allocations)
        workChange      *BitVector // For change computation
        workXt          *BitVector // For robustness window
        workCombined    *BitVector // For combining changes
        workInvMask     *BitVector // For inverted mask
        workExtractMask *BitVector // For extraction mask
        workMaskShifted *BitVector // For mask shift
        workMaskDiff    *BitVector // For mask XOR
        workChanges     *BitVector // For input XOR prevInput
        workOutput      *BitBuffer // For output buffer
}

// NewCompressor creates a new compressor.
func NewCompressor(F int, initialMask *BitVector, robustness, ptLimit, ftLimit, rtLimit int) (*Compressor, error) {
        if F <= 0 || F > MaxPacketLength {
                return nil, fmt.Errorf("F must be between 1 and %d bits", MaxPacketLength)
        }
        if robustness < 0 || robustness > MaxRobustness {
                return nil, fmt.Errorf("robustness must be between 0 and %d", MaxRobustness)
        }

        comp := &Compressor{
                F:          F,
                robustness: robustness,
                ptLimit:    ptLimit,
                ftLimit:    ftLimit,
                rtLimit:    rtLimit,
        }

        // Initialize bit vectors
        var err error
        comp.mask, err = NewBitVector(F)
        if err != nil {
                return nil, err
        }
        comp.prevMask, _ = NewBitVector(F)
        comp.build, _ = NewBitVector(F)
        comp.prevInput, _ = NewBitVector(F)
        comp.initialMask, _ = NewBitVector(F)

        // Initialize change history
        for i := 0; i < MaxHistory; i++ {
                comp.changeHistory[i], _ = NewBitVector(F)
        }

        // Initialize working buffers
        comp.workChange, _ = NewBitVector(F)
        comp.workXt, _ = NewBitVector(F)
        comp.workCombined, _ = NewBitVector(F)
        comp.workInvMask, _ = NewBitVector(F)
        comp.workExtractMask, _ = NewBitVector(F)
        comp.workMaskShifted, _ = NewBitVector(F)
        comp.workMaskDiff, _ = NewBitVector(F)
        comp.workChanges, _ = NewBitVector(F)
        comp.workOutput = NewBitBuffer()

        // Set initial mask if provided
        if initialMask != nil {
                comp.initialMask.CopyFrom(initialMask)
                comp.mask.CopyFrom(initialMask)
        }

        // Reset state
        comp.Reset()

        return comp, nil
}

// Reset resets the compressor to initial state.
func (comp *Compressor) Reset() {
        comp.t = 0
        comp.historyIndex = 0

        // Reset mask to initial
        comp.mask.CopyFrom(comp.initialMask)
        comp.prevMask.Zero()

        // Clear build and prevInput
        comp.build.Zero()
        comp.prevInput.Zero()

        // Clear change history
        for i := 0; i < MaxHistory; i++ {
                comp.changeHistory[i].Zero()
        }

        // Clear flag history
        for i := 0; i < MaxVtHistory; i++ {
                comp.newMaskFlagHistory[i] = 0
        }
        comp.flagHistoryIndex = 0

        // Reset countdown counters
        comp.ptCounter = comp.ptLimit
        comp.ftCounter = comp.ftLimit
        comp.rtCounter = comp.rtLimit
}

// CompressPacket compresses a single input packet.
func (comp *Compressor) CompressPacket(input *BitVector, params *CompressParams) ([]byte, error) {
        if input == nil || input.length != comp.F {
                return nil, errors.New("input must be non-nil and match F length")
        }

        // Use default params if none provided
        if params == nil {
                params = &CompressParams{MinRobustness: comp.robustness}
        }

        // Reuse pre-allocated output buffer
        comp.workOutput.Clear()
        output := comp.workOutput

        // ================================================================
        // STEP 1: Update Mask and Build Vectors (CCSDS Section 4)
        // ================================================================

        // Save previous mask (reuse prevMask field)
        comp.prevMask.CopyFrom(comp.mask)
        prevMask := comp.prevMask

        // Save previous build into workCombined temporarily
        comp.workCombined.CopyFrom(comp.build)
        prevBuild := comp.workCombined

        // Update build vector (Equation 6)
        if comp.t > 0 {
                updateBuildInternal(comp.build, input, comp.prevInput, comp.workChanges, params.NewMaskFlag, comp.t)
        }

        // Update mask vector (Equation 7)
        if comp.t > 0 {
                updateMaskInternal(comp.mask, input, comp.prevInput, prevBuild, comp.workChanges, params.NewMaskFlag)
        }

        // Compute change vector (Equation 8) - reuse workChange
        change := comp.workChange
        computeChangeInternal(change, comp.mask, prevMask, comp.t)

        // Store change in history (circular buffer)
        comp.changeHistory[comp.historyIndex].CopyFrom(change)

        // ================================================================
        // STEP 2: Encode Output Packet (CCSDS Section 5.3)
        // ot = ht || qt || ut
        // ================================================================

        // Calculate Xt (robustness window) - reuse workXt
        Xt := comp.computeRobustnessWindowInto(change, comp.workXt)

        // Calculate Vt (effective robustness)
        Vt := comp.computeEffectiveRobustness(change)

        // Calculate dt flag
        var dt int
        if !params.SendMaskFlag && !params.UncompressedFlag {
                dt = 1
        }

        // ================================================================
        // Component ht: Mask change information
        // ht = RLE(Xt) || BIT4(Vt) || et || kt || ct || dt
        // ================================================================

        // 1. RLE(Xt) - Run-length encode the robustness window
        RLEEncode(output, Xt)

        // 2. BIT4(Vt) - 4-bit effective robustness level
        output.AppendValue(uint64(Vt), 4)

        // 3. et, kt, ct - Only if Vt > 0 and there are mask changes
        if Vt > 0 && Xt.HammingWeight() > 0 {
                // Calculate et
                et := hasPositiveUpdates(Xt, comp.mask)
                output.AppendBit(et)

                if et != 0 {
                        // kt - Output '1' for positive updates (mask=0), '0' for negative
                        // Extract INVERTED mask values in forward order - reuse workInvMask
                        invertedMask := comp.workInvMask
                        for j := 0; j < comp.mask.length; j++ {
                                maskBit := comp.mask.GetBit(j)
                                if maskBit == 0 {
                                        invertedMask.SetBit(j, 1)
                                } else {
                                        invertedMask.SetBit(j, 0)
                                }
                        }
                        BitExtractForward(output, invertedMask, Xt)

                        // Calculate and encode ct
                        ct := comp.computeCtFlag(Vt, params.NewMaskFlag)
                        output.AppendBit(ct)
                }
        }

        // 4. dt - Flag indicating if both ft and rt are zero
        output.AppendBit(dt)

        // ================================================================
        // Component qt: Optional full mask
        // qt = empty if dt=1, '1' || RLE(<(Mt XOR (Mt<<))>) if ft=1, '0' otherwise
        // ================================================================

        if dt == 0 { // Only if dt = 0
                if params.SendMaskFlag {
                        output.AppendBit(1) // Flag: mask follows

                        // Encode mask as RLE(M XOR (M<<)) - reuse working buffers
                        leftShiftInto(comp.workMaskShifted, comp.mask)
                        comp.workMaskDiff.XORInto(comp.mask, comp.workMaskShifted)
                        RLEEncode(output, comp.workMaskDiff)
                } else {
                        output.AppendBit(0) // Flag: no mask
                }
        }

        // ================================================================
        // Component ut: Unpredictable bits or full input
        // ================================================================

        if params.UncompressedFlag {
                // '1' || COUNT(F) || It
                output.AppendBit(1) // Flag: full input follows
                CountEncode(output, comp.F)
                output.AppendBitVector(input)
        } else {
                if dt == 0 {
                        // '0' || BE(...)
                        output.AppendBit(0) // Flag: compressed
                }

                // Determine extraction mask based on ct
                ct := comp.computeCtFlag(Vt, params.NewMaskFlag)

                if ct != 0 && Vt > 0 {
                        // BE(It, (Xt OR Mt)) - extract bits where mask OR changes are set
                        comp.workExtractMask.ORInto(comp.mask, Xt)
                        BitExtract(output, input, comp.workExtractMask)
                } else {
                        // BE(It, Mt) - extract only unpredictable bits
                        BitExtract(output, input, comp.mask)
                }
        }

        // ================================================================
        // STEP 3: Update State for Next Cycle
        // ================================================================

        // Save current input and mask as previous for next iteration
        comp.prevInput.CopyFrom(input)
        comp.prevMask.CopyFrom(comp.mask)

        // Track new_mask_flag for ct calculation
        if params.NewMaskFlag {
                comp.newMaskFlagHistory[comp.flagHistoryIndex] = 1
        } else {
                comp.newMaskFlagHistory[comp.flagHistoryIndex] = 0
        }
        comp.flagHistoryIndex = (comp.flagHistoryIndex + 1) % MaxVtHistory

        // Advance time
        comp.t++

        // Advance history index (circular buffer)
        comp.historyIndex = (comp.historyIndex + 1) % MaxHistory

        return output.ToBytes(), nil
}

// computeRobustnessWindowInto computes Xt = OR of recent change vectors into dst.
func (comp *Compressor) computeRobustnessWindowInto(currentChange *BitVector, dst *BitVector) *BitVector {
        if comp.robustness == 0 || comp.t == 0 {
                // Xt = Dt (no reversal - RLE processes LSB to MSB directly)
                dst.CopyFrom(currentChange)
        } else {
                // Start with current change
                dst.CopyFrom(currentChange)

                // Determine how many historical changes to include
                numChanges := comp.t
                if comp.robustness < numChanges {
                        numChanges = comp.robustness
                }

                // OR with historical changes (going backwards from current)
                for i := 1; i <= numChanges; i++ {
                        // Calculate index of change from i iterations ago
                        histIdx := (comp.historyIndex + MaxHistory - i) % MaxHistory
                        // OR in place: dst = dst OR changeHistory[histIdx]
                        for w := 0; w < dst.numWords; w++ {
                                dst.data[w] |= comp.changeHistory[histIdx].data[w]
                        }
                }
        }

        return dst
}

// leftShiftInto computes left shift of src into dst.
func leftShiftInto(dst, src *BitVector) {
        // Word-level left shift (big-endian: MSB in high bits of first word)
        var carry uint32
        for i := src.numWords - 1; i >= 0; i-- {
                word := src.data[i]
                dst.data[i] = ((word << 1) | carry) & 0xFFFFFFFF
                carry = (word >> 31) & 1
        }
}

// computeEffectiveRobustness computes Vt = Rt + Ct.
func (comp *Compressor) computeEffectiveRobustness(currentChange *BitVector) int {
        _ = currentChange // Unused - Ct is computed from history

        Rt := comp.robustness
        Vt := Rt

        // For t > Rt, compute Ct
        if comp.t > Rt {
                // Count backwards through history starting from Rt+1 positions back
                Ct := 0

                maxI := 15
                if comp.t < maxI {
                        maxI = comp.t
                }

                for i := Rt + 1; i <= maxI; i++ {
                        histIdx := (comp.historyIndex + MaxHistory - i) % MaxHistory
                        if comp.changeHistory[histIdx].HammingWeight() > 0 {
                                break // Found a change, stop counting
                        }
                        Ct++
                        if Ct >= 15-Rt {
                                break // Cap at maximum Ct value
                        }
                }

                Vt = Rt + Ct
                if Vt > 15 {
                        Vt = 15 // Cap at 15 (4 bits)
                }
        }

        return Vt
}

// computeCtFlag computes ct flag for multiple mask updates.
func (comp *Compressor) computeCtFlag(Vt int, currentNewMaskFlag bool) int {
        if Vt == 0 {
                return 0
        }

        // Count how many times new_mask_flag was set
        count := 0

        // Include current packet's flag
        if currentNewMaskFlag {
                count++
        }

        // Check history for Vt previous entries
        iterationsToCheck := Vt
        if comp.t < iterationsToCheck {
                iterationsToCheck = comp.t
        }

        for i := 0; i < iterationsToCheck; i++ {
                // Calculate history index going backwards from previous
                histIdx := (comp.flagHistoryIndex + MaxVtHistory - 1 - i) % MaxVtHistory
                if comp.newMaskFlagHistory[histIdx] != 0 {
                        count++
                }
        }

        if count >= 2 {
                return 1
        }
        return 0
}

// hasPositiveUpdates checks for positive mask updates (et flag).
func hasPositiveUpdates(Xt, mask *BitVector) int {
        // et = 1 if any changed bits (in Xt) are predictable (mask bit = 0)
        for i := 0; i < Xt.length; i++ {
                bitChanged := Xt.GetBit(i)
                bitPredictable := mask.GetBit(i) == 0

                if bitChanged != 0 && bitPredictable {
                        return 1 // Found a positive update
                }
        }
        return 0
}

// Internal helper functions that modify vectors in place

func updateBuildInternal(build, inputVec, prevInput, workChanges *BitVector, newMaskFlag bool, t int) {
        if t == 0 || newMaskFlag {
                build.Zero()
        } else {
                // Bt = (It XOR It-1) OR Bt-1
                workChanges.XORInto(inputVec, prevInput)
                build.ORInto(workChanges, build)
        }
}

func updateMaskInternal(mask, inputVec, prevInput, buildPrev, workChanges *BitVector, newMaskFlag bool) {
        // Calculate changes: It XOR It-1 (into workChanges)
        workChanges.XORInto(inputVec, prevInput)

        if newMaskFlag {
                // Mt = changes OR Bt-1
                mask.ORInto(workChanges, buildPrev)
        } else {
                // Mt = changes OR Mt-1 (in place)
                for w := 0; w < mask.numWords; w++ {
                        mask.data[w] = workChanges.data[w] | mask.data[w]
                }
        }
}

func computeChangeInternal(change, mask, prevMask *BitVector, t int) {
        if t == 0 {
                // CCSDS Eq. 8: D0 = 0 — both encoder and decoder start from the
                // same user-specified M0, so there is no change to communicate
                change.Zero()
        } else {
                // Dt = Mt XOR Mt-1
                change.XORInto(mask, prevMask)
        }
}

package ccsds124

import (
        "fmt"
        "math/bits"
)

// CountDecode decodes a COUNT-encoded value.
//
// Decoding rules (inverse of CCSDS Equation 9):
//   - '0' -> 1
//   - '10' -> 0 (terminator)
//   - '110' + BIT5 -> value + 2 (range 2-33)
//   - '111' + BIT_E -> value + 2 (range 34+)
//
// Returns decoded positive integer (0 for terminator, 1+ for values).
func CountDecode(br *BitReader) (int, error) {
        // Read first bit
        firstBit, err := br.ReadBit()
        if err != nil {
                return 0, fmt.Errorf("COUNT decode: %w", err)
        }

        if firstBit == 0 {
                // Case 1: '0' -> 1
                return 1, nil
        }

        // First bit was 1, read second bit
        secondBit, err := br.ReadBit()
        if err != nil {
                return 0, fmt.Errorf("COUNT decode: %w", err)
        }

        if secondBit == 0 {
                // Case 2: '10' -> terminator (0)
                return 0, nil
        }

        // First two bits were '11', read third bit
        thirdBit, err := br.ReadBit()
        if err != nil {
                return 0, fmt.Errorf("COUNT decode: %w", err)
        }

        if thirdBit == 0 {
                // Case 3: '110' + BIT5 -> value + 2 (range 2-33)
                value, err := br.ReadBits(5)
                if err != nil {
                        return 0, fmt.Errorf("COUNT decode BIT5: %w", err)
                }
                return int(value) + 2, nil
        }

        // Case 4: '111' + BIT_E -> value + 2 (range 34+)
        // Need to determine E by reading bits until we can decode
        // E = 2*floor(log2(value)+1) - 6
        // We read bits incrementally to find the value

        // Start with 6 bits (minimum for value >= 32)
        e := 6
        value, err := br.ReadBits(e)
        if err != nil {
                return 0, fmt.Errorf("COUNT decode BIT_E: %w", err)
        }

        // Check if we have the right number of bits
        // For value v, E should be 2*floor(log2(v)+1) - 6
        for {
                // Calculate expected E for this value
                var expectedE int
                if value == 0 {
                        expectedE = 0
                } else {
                        // Find floor(log2(value))
                        logVal := 0
                        temp := value
                        for temp > 1 {
                                temp >>= 1
                                logVal++
                        }
                        expectedE = 2*(logVal+1) - 6
                }

                if expectedE == e {
                        // We have the right number of bits
                        break
                }

                // Need more bits
                e += 2
                // Read 2 more bits and shift in
                extra, err := br.ReadBits(2)
                if err != nil {
                        return 0, fmt.Errorf("COUNT decode extra bits: %w", err)
                }
                value = (value << 2) | extra
        }

        return int(value) + 2, nil
}

// RLEDecode decodes an RLE-encoded bit vector.
//
// Decoding rules (inverse of CCSDS Equation 10):
//   - Read COUNT values until terminator (0)
//   - Each COUNT value represents position delta to next '1' bit
func RLEDecode(br *BitReader, length int) (*BitVector, error) {
        result, err := NewBitVector(length)
        if err != nil {
                return nil, fmt.Errorf("RLE decode: %w", err)
        }

        // Start from end of vector
        position := length

        for {
                // Decode next count
                count, err := CountDecode(br)
                if err != nil {
                        return nil, fmt.Errorf("RLE decode: %w", err)
                }

                if count == 0 {
                        // Terminator reached
                        break
                }

                // Move position back by count. A delta beyond the remaining bit
                // position means the encoding is invalid for this vector length
                // (GOTCHAS #20): reject it instead of silently skipping.
                if count > position {
                        return nil, fmt.Errorf("RLE decode: invalid delta %d exceeds remaining bit position %d", count, position)
                }
                position -= count

                // Set the bit at this position
                result.SetBit(position, 1)
        }

        return result, nil
}

// BitInsert inserts bits into data at positions specified by mask (inverse of BE).
//
// Reads bits from reader and inserts them into data at positions
// where mask has '1' bits. Bits are read in reverse order (highest
// position to lowest) to match BE extraction order.
func BitInsert(br *BitReader, data, mask *BitVector) error {
        if data == nil || mask == nil {
                return fmt.Errorf("BitInsert: data and mask cannot be nil")
        }
        if data.length != mask.length {
                return fmt.Errorf("BitInsert: data and mask must have same length")
        }

        // Process words from last to first (high positions to low, matching BitExtract)
        for w := mask.numWords - 1; w >= 0; w-- {
                maskWord := mask.data[w]

                if maskWord == 0 {
                        continue
                }

                // Process from low bit index to high (= high position to low)
                for maskWord != 0 {
                        // Isolate lowest set bit
                        lsb := maskWord & uint32(-int32(maskWord))
                        bitPos := bits.TrailingZeros32(lsb)

                        bit, err := br.ReadBit()
                        if err != nil {
                                return fmt.Errorf("BitInsert: %w", err)
                        }

                        if bit != 0 {
                                data.data[w] |= uint32(1) << bitPos
                        } else {
                                data.data[w] &= ^(uint32(1) << bitPos)
                        }

                        maskWord ^= lsb
                }
        }

        return nil
}

// BitInsertForward inserts bits in forward order (for kt component).
//
// Same as BitInsert but reads bits from lowest position to highest.
func BitInsertForward(br *BitReader, data, mask *BitVector) error {
        if data == nil || mask == nil {
                return fmt.Errorf("BitInsertForward: data and mask cannot be nil")
        }
        if data.length != mask.length {
                return fmt.Errorf("BitInsertForward: data and mask must have same length")
        }

        // Process words from first to last (low positions to high)
        for w := 0; w < mask.numWords; w++ {
                maskWord := mask.data[w]

                if maskWord == 0 {
                        continue
                }

                // Process from high bit index to low (= low position to high)
                for maskWord != 0 {
                        highBit := 31 - bits.LeadingZeros32(maskWord)

                        bit, err := br.ReadBit()
                        if err != nil {
                                return fmt.Errorf("BitInsertForward: %w", err)
                        }

                        if bit != 0 {
                                data.data[w] |= uint32(1) << highBit
                        } else {
                                data.data[w] &= ^(uint32(1) << highBit)
                        }

                        maskWord &= ^(uint32(1) << highBit)
                }
        }

        return nil
}

package ccsds124

import (
        "bytes"
        "errors"
)

// Decompress decompresses CCSDS 124.0-B-1 compressed data.
//
// Parameters:
//   - data: Compressed input bytes
//   - packetSize: Size of each decompressed packet in bytes
//   - robustness: Robustness parameter R (must match compression)
//
// Returns decompressed data or an error.
func Decompress(data []byte, packetSize, robustness int) ([]byte, error) {
        if len(data) == 0 {
                return []byte{}, nil
        }
        if packetSize <= 0 {
                return nil, errors.New("packet size must be positive")
        }
        if robustness < 1 || robustness > 7 {
                return nil, errors.New("robustness must be between 1 and 7")
        }

        // Convert packet size from bytes to bits
        F := packetSize * 8

        // Create decompressor
        decomp, err := NewDecompressor(F, nil, robustness)
        if err != nil {
                return nil, err
        }

        // Decompress stream
        packets, err := decomp.DecompressStream(data, len(data)*8)
        if err != nil {
                return nil, err
        }

        // Concatenate output
        var output bytes.Buffer
        for _, packet := range packets {
                output.Write(packet)
        }

        return output.Bytes(), nil
}

package ccsds124

import (
        "errors"
        "fmt"
)

// Decompressor maintains state for CCSDS 124.0-B-1 decompression.
type Decompressor struct {
        // Configuration (immutable after init)
        F          int // Input vector length in bits
        robustness int // Rt: Base robustness level (0-7)

        // State (updated each cycle)
        mask        *BitVector
        initialMask *BitVector
        prevOutput  *BitVector
        Xt          *BitVector // Positive changes tracker

        // Cycle counter
        t int
}

// NewDecompressor creates a new decompressor.
func NewDecompressor(F int, initialMask *BitVector, robustness int) (*Decompressor, error) {
        if F <= 0 || F > MaxPacketLength {
                return nil, fmt.Errorf("F must be between 1 and %d bits", MaxPacketLength)
        }
        if robustness < 0 || robustness > MaxRobustness {
                return nil, fmt.Errorf("robustness must be between 0 and %d", MaxRobustness)
        }

        decomp := &Decompressor{
                F:          F,
                robustness: robustness,
        }

        // Initialize bit vectors
        var err error
        decomp.mask, err = NewBitVector(F)
        if err != nil {
                return nil, err
        }
        decomp.initialMask, _ = NewBitVector(F)
        decomp.prevOutput, _ = NewBitVector(F)
        decomp.Xt, _ = NewBitVector(F)

        // Set initial mask if provided
        if initialMask != nil {
                decomp.initialMask.CopyFrom(initialMask)
                decomp.mask.CopyFrom(initialMask)
        }

        // Reset state
        decomp.Reset()

        return decomp, nil
}

// Reset resets the decompressor to initial state.
func (decomp *Decompressor) Reset() {
        decomp.t = 0
        decomp.mask.CopyFrom(decomp.initialMask)
        decomp.prevOutput.Zero()
        decomp.Xt.Zero()
}

// DecompressPacket decompresses a single compressed packet.
func (decomp *Decompressor) DecompressPacket(reader *BitReader) (*BitVector, error) {
        if reader == nil {
                return nil, errors.New("reader must not be nil")
        }

        output, _ := NewBitVector(decomp.F)

        // Copy previous output as prediction base
        output.CopyFrom(decomp.prevOutput)

        // Clear positive changes tracker
        decomp.Xt.Zero()

        // ====================================================================
        // Parse ht: Mask change information
        // ht = RLE(Xt) || BIT4(Vt) || et || kt || ct || dt
        // ====================================================================

        // Decode RLE(Xt) - mask changes
        Xt, err := RLEDecode(reader, decomp.F)
        if err != nil {
                return nil, fmt.Errorf("failed to decode RLE(Xt): %w", err)
        }

        // Read BIT4(Vt) - effective robustness
        vtRaw, err := reader.ReadBits(4)
        if err != nil {
                return nil, fmt.Errorf("failed to read Vt: %w", err)
        }
        Vt := int(vtRaw & 0x0F)

        // Process et, kt, ct if Vt > 0 and there are changes
        ct := 0
        changeCount := Xt.HammingWeight()

        if Vt > 0 && changeCount > 0 {
                // Read et
                et, err := reader.ReadBit()
                if err != nil {
                        return nil, fmt.Errorf("failed to read et: %w", err)
                }

                if et == 1 {
                        // Read kt - determines positive/negative updates
                        // kt has one bit per change in Xt
                        ktBits := make([]int, 0, changeCount)

                        // Read kt bits (forward order)
                        for i := 0; i < decomp.F; i++ {
                                if Xt.GetBit(i) != 0 {
                                        bit, err := reader.ReadBit()
                                        if err != nil {
                                                return nil, fmt.Errorf("failed to read kt bit: %w", err)
                                        }
                                        ktBits = append(ktBits, bit)
                                }
                        }

                        // Apply mask updates based on kt
                        ktIdx := 0
                        for i := 0; i < decomp.F; i++ {
                                if Xt.GetBit(i) != 0 {
                                        // kt=1 means positive update (mask becomes 0)
                                        // kt=0 means negative update (mask becomes 1)
                                        if ktBits[ktIdx] != 0 {
                                                decomp.mask.SetBit(i, 0)
                                                decomp.Xt.SetBit(i, 1) // Track positive change
                                        } else {
                                                decomp.mask.SetBit(i, 1)
                                        }
                                        ktIdx++
                                }
                        }

                        // Read ct
                        ctBit, err := reader.ReadBit()
                        if err != nil {
                                return nil, fmt.Errorf("failed to read ct: %w", err)
                        }
                        ct = ctBit
                } else {
                        // et = 0: all updates are negative (mask bits become 1)
                        for i := 0; i < decomp.F; i++ {
                                if Xt.GetBit(i) != 0 {
                                        decomp.mask.SetBit(i, 1)
                                }
                        }
                }
        } else if Vt == 0 && changeCount > 0 {
                // Vt = 0: toggle mask bits at change positions
                for i := 0; i < decomp.F; i++ {
                        if Xt.GetBit(i) != 0 {
                                currentVal := decomp.mask.GetBit(i)
                                if currentVal == 0 {
                                        decomp.mask.SetBit(i, 1)
                                } else {
                                        decomp.mask.SetBit(i, 0)
                                }
                        }
                }
        }
        // else: No changes to apply (changeCount == 0)

        // Read dt
        dt, err := reader.ReadBit()
        if err != nil {
                return nil, fmt.Errorf("failed to read dt: %w", err)
        }

        // ====================================================================
        // Parse qt: Optional full mask
        // ====================================================================

        rt := 0

        // dt=1 means both ft=0 and rt=0 (optimization per CCSDS Eq. 13)
        // dt=0 means we need to read ft and rt from the stream
        if dt == 0 {
                // Read ft flag
                ft, err := reader.ReadBit()
                if err != nil {
                        return nil, fmt.Errorf("failed to read ft: %w", err)
                }

                if ft == 1 {
                        // Full mask follows: decode RLE(M XOR (M<<))
                        maskDiff, err := RLEDecode(reader, decomp.F)
                        if err != nil {
                                return nil, fmt.Errorf("failed to decode mask: %w", err)
                        }

                        // Reverse the horizontal XOR to get the actual mask.
                        // HXOR encoding: HXOR[i] = M[i] XOR M[i+1], with HXOR[F-1] = M[F-1]
                        // Reversal: start from LSB (position F-1) and work towards MSB (position 0)
                        // M[F-1] = HXOR[F-1] (just copy)
                        // M[i] = HXOR[i] XOR M[i+1] for i < F-1

                        // Copy LSB bit directly (position F-1 in bitvector)
                        current := maskDiff.GetBit(decomp.F - 1)
                        decomp.mask.SetBit(decomp.F-1, current)

                        // Process remaining bits from F-2 down to 0
                        for i := decomp.F - 1; i > 0; i-- {
                                pos := i - 1
                                hxorBit := maskDiff.GetBit(pos)
                                // M[pos] = HXOR[pos] XOR M[pos+1] = HXOR[pos] XOR current
                                current = hxorBit ^ current
                                decomp.mask.SetBit(pos, current)
                        }
                }

                // Read rt flag
                rtBit, err := reader.ReadBit()
                if err != nil {
                        return nil, fmt.Errorf("failed to read rt: %w", err)
                }
                rt = rtBit
        }

        // ====================================================================
        // Parse ut: Unpredictable bits or full input
        // ====================================================================

        if rt == 1 {
                // Full packet follows: COUNT(F) || It
                _, err := CountDecode(reader)
                if err != nil {
                        return nil, fmt.Errorf("failed to decode packet length: %w", err)
                }

                // Read full packet
                for i := 0; i < decomp.F; i++ {
                        bit, err := reader.ReadBit()
                        if err != nil {
                                return nil, fmt.Errorf("failed to read input bit %d: %w", i, err)
                        }
                        output.SetBit(i, bit)
                }
        } else {
                // Compressed: extract unpredictable bits
                var extractionMask *BitVector

                if ct == 1 && Vt > 0 {
                        // BE(It, (Xt OR Mt))
                        extractionMask = decomp.mask.OR(decomp.Xt)
                } else {
                        // BE(It, Mt)
                        extractionMask = decomp.mask.Copy()
                }

                // Insert unpredictable bits
                err := BitInsert(reader, output, extractionMask)
                if err != nil {
                        return nil, fmt.Errorf("failed to insert bits: %w", err)
                }
        }

        // ====================================================================
        // Update state for next cycle
        // ====================================================================

        decomp.prevOutput.CopyFrom(output)
        decomp.t++

        return output, nil
}

// DecompressStream decompresses multiple packets from a byte stream.
func (decomp *Decompressor) DecompressStream(data []byte, numBits int) ([][]byte, error) {
        if len(data) == 0 {
                return nil, errors.New("input data is empty")
        }

        // Reset decompressor
        decomp.Reset()

        // Initialize bit reader
        reader := NewBitReaderWithBits(data, numBits)

        // Output packet size in bytes
        packetBytes := (decomp.F + 7) / 8
        var outputs [][]byte

        // Decompress packets until input exhausted
        for reader.Remaining() > 0 {
                output, err := decomp.DecompressPacket(reader)
                if err != nil {
                        return outputs, err
                }

                // Convert to bytes
                outputBytes := output.ToBytes()
                if len(outputBytes) < packetBytes {
                        // Pad if necessary
                        padded := make([]byte, packetBytes)
                        copy(padded, outputBytes)
                        outputBytes = padded
                }
                outputs = append(outputs, outputBytes[:packetBytes])

                // Align to byte boundary for next packet
                reader.AlignByte()
        }

        return outputs, nil
}

// PacketIterator provides streaming decompression with an iterator pattern.
type PacketIterator struct {
        decomp      *Decompressor
        reader      *BitReader
        packetBytes int
        err         error
}

// NewPacketIterator creates a streaming packet iterator.
func (decomp *Decompressor) NewPacketIterator(data []byte, numBits int) *PacketIterator {
        decomp.Reset()
        return &PacketIterator{
                decomp:      decomp,
                reader:      NewBitReaderWithBits(data, numBits),
                packetBytes: (decomp.F + 7) / 8,
        }
}

// Next returns the next decompressed packet, or nil if done/error.
func (it *PacketIterator) Next() []byte {
        if it.err != nil || it.reader.Remaining() <= 0 {
                return nil
        }

        output, err := it.decomp.DecompressPacket(it.reader)
        if err != nil {
                it.err = err
                return nil
        }

        // Convert to bytes
        outputBytes := output.ToBytes()
        if len(outputBytes) < it.packetBytes {
                padded := make([]byte, it.packetBytes)
                copy(padded, outputBytes)
                outputBytes = padded
        }

        it.reader.AlignByte()
        return outputBytes[:it.packetBytes]
}

// Err returns any error encountered during iteration.
func (it *PacketIterator) Err() error {
        return it.err
}

// StreamPackets returns a channel that yields decompressed packets.
// The channel is closed when all packets are processed or on error.
func (decomp *Decompressor) StreamPackets(data []byte, numBits int) <-chan []byte {
        ch := make(chan []byte, 64) // Buffer some packets

        go func() {
                defer close(ch)

                decomp.Reset()
                reader := NewBitReaderWithBits(data, numBits)
                packetBytes := (decomp.F + 7) / 8

                for reader.Remaining() > 0 {
                        output, err := decomp.DecompressPacket(reader)
                        if err != nil {
                                return // Stop on error
                        }

                        outputBytes := output.ToBytes()
                        if len(outputBytes) < packetBytes {
                                padded := make([]byte, packetBytes)
                                copy(padded, outputBytes)
                                outputBytes = padded
                        }

                        ch <- outputBytes[:packetBytes]
                        reader.AlignByte()
                }
        }()

        return ch
}

package ccsds124

import (
        "errors"
        "math/bits"
)

// CountEncode implements CCSDS 124.0-B-1 Section 5.2.2, Table 5-1, Equation 9.
//
// Encodes positive integers 1 <= A <= 2^16 - 1:
//   - A = 1 -> '0'
//   - 2 <= A <= 33 -> '110' || BIT_5(A-2)
//   - A >= 34 -> '111' || BIT_E(A-2) where E = 2*floor(log2(A-2)+1) - 6
func CountEncode(bb *BitBuffer, A int) error {
        if A < 1 || A > 65535 {
                return errors.New("COUNT: A must be in range [1, 65535]")
        }

        if A == 1 {
                // Case 1: A = 1 -> '0'
                bb.AppendBit(0)
        } else if A <= 33 {
                // Case 2: 2 <= A <= 33 -> '110' || BIT_5(A-2)
                bb.AppendBit(1)
                bb.AppendBit(1)
                bb.AppendBit(0)

                // Append BIT_5(A-2) MSB-first - 5 bits
                value := A - 2
                for i := 4; i >= 0; i-- {
                        bit := (value >> i) & 1
                        bb.AppendBit(bit)
                }
        } else {
                // Case 3: A >= 34 -> '111' || BIT_E(A-2)
                bb.AppendBit(1)
                bb.AppendBit(1)
                bb.AppendBit(1)

                // Calculate E = 2*floor(log2(A-2)+1) - 6
                // Using bits.Len for fast integer log2: floor(log2(x)) = bits.Len(x) - 1
                value := A - 2
                E := (2 * bits.Len(uint(value))) - 6

                // Append BIT_E(A-2) MSB-first - E bits
                for i := E - 1; i >= 0; i-- {
                        bit := (value >> i) & 1
                        bb.AppendBit(bit)
                }
        }

        return nil
}

// CountEncodeTerminator writes the RLE terminator pattern '10'.
func CountEncodeTerminator(bb *BitBuffer) {
        bb.AppendBit(1)
        bb.AppendBit(0)
}

// DeBruijn lookup table for fast LSB finding (matches reference implementation)
var debruijnLookup = [32]int{
        1, 2, 29, 3, 30, 15, 25, 4, 31, 23, 21, 16,
        26, 18, 5, 9, 32, 28, 14, 24, 22, 20, 17, 8,
        27, 13, 19, 7, 12, 6, 11, 10,
}

// RLEEncode implements CCSDS 124.0-B-1 Section 5.2.3, Equation 10.
//
// RLE(a) = COUNT(C_0) || COUNT(C_1) || ... || COUNT(C_{H(a)-1}) || '10'
//
// where C_i = 1 + (count of consecutive '0' bits before i-th '1' bit)
// and H(a) = Hamming weight (number of '1' bits in a)
//
// Note: Trailing zeros are not encoded (deducible from vector length)
func RLEEncode(bb *BitBuffer, input *BitVector) error {
        if input == nil {
                return errors.New("RLE: input cannot be nil")
        }

        // Start from the end of the vector
        oldBitPosition := input.length

        // Process words in reverse order (from high to low)
        for word := input.numWords - 1; word >= 0; word-- {
                wordData := input.data[word]

                // Process all set bits in this word
                for wordData != 0 {
                        // Isolate the LSB: x = change & -change
                        lsb := wordData & uint32(-int32(wordData))

                        // Find LSB position using DeBruijn sequence
                        debruijnIndex := (lsb * 0x077CB531) >> 27
                        bitPositionInWord := debruijnLookup[debruijnIndex]

                        // Count from the other side (reference line 754)
                        bitPositionInWord = 32 - bitPositionInWord

                        // Calculate global bit position (reference line 756)
                        newBitPosition := (word * 32) + bitPositionInWord

                        // Calculate delta (number of zeros + 1)
                        delta := oldBitPosition - newBitPosition

                        // Encode the count
                        if err := CountEncode(bb, delta); err != nil {
                                return err
                        }

                        // Update old position for next iteration
                        oldBitPosition = newBitPosition

                        // Clear the processed bit
                        wordData ^= lsb
                }
        }

        // Append terminator '10'
        CountEncodeTerminator(bb)

        return nil
}

// BitExtract implements CCSDS 124.0-B-1 Section 5.2.4, Equation 11.
//
// BE(a, b) = a_{g_{H(b)-1}} || ... || a_{g_1} || a_{g_0}
//
// where g_i is the position of the i-th '1' bit in b (MSB to LSB order)
//
// Extracts bits from 'data' at positions where 'mask' has '1' bits.
// Output order: highest position to lowest position
func BitExtract(bb *BitBuffer, data, mask *BitVector) error {
        if data == nil || mask == nil {
                return errors.New("BitExtract: data and mask cannot be nil")
        }
        if data.length != mask.length {
                return errors.New("BitExtract: data and mask must have same length")
        }

        // Process words from last to first (high positions to low)
        // Within each word: bit 0 = highest position, bit 31 = lowest position
        for w := mask.numWords - 1; w >= 0; w-- {
                maskWord := mask.data[w]
                dataWord := data.data[w]

                if maskWord == 0 {
                        continue
                }

                // Extract bits from low bit index to high (= high position to low position)
                // Use isolate-LSB technique to process from bit 0 upward
                for maskWord != 0 {
                        // Isolate lowest set bit: lsb = x & -x
                        lsb := maskWord & uint32(-int32(maskWord))

                        // Get bit position using trailing zeros
                        bitPos := bits.TrailingZeros32(lsb)

                        // Extract data bit at that position
                        dataBit := int((dataWord >> bitPos) & 1)
                        bb.AppendBit(dataBit)

                        // Clear the processed bit
                        maskWord ^= lsb
                }
        }

        return nil
}

// BitExtractForward extracts bits in forward order (lowest position to highest).
// Used for kt component: processes mask values at changed positions
// in order from lowest position index to highest.
func BitExtractForward(bb *BitBuffer, data, mask *BitVector) error {
        if data == nil || mask == nil {
                return errors.New("BitExtractForward: data and mask cannot be nil")
        }
        if data.length != mask.length {
                return errors.New("BitExtractForward: data and mask must have same length")
        }

        // Process words from first to last (low positions to high)
        for w := 0; w < mask.numWords; w++ {
                maskWord := mask.data[w]
                dataWord := data.data[w]

                if maskWord == 0 {
                        continue
                }

                // Extract bits from high bit index to low (= low position to high position)
                for maskWord != 0 {
                        // Find highest set bit (corresponds to lowest position in this word's range)
                        highBit := 31 - bits.LeadingZeros32(maskWord)

                        dataBit := int((dataWord >> highBit) & 1)
                        bb.AppendBit(dataBit)

                        maskWord &= ^(uint32(1) << highBit)
                }
        }

        return nil
}

package ccsds124

// UpdateBuild updates the build vector (CCSDS Equation 6).
//
// Build vector accumulates bits that have changed over time.
// When newMaskFlag is set, the build is used to replace the mask
// and is then reset to zero.
//
// Equation:
//   - Bt = (It XOR It-1) OR Bt-1  (if t > 0 and newMaskFlag = 0)
//   - Bt = 0                      (if t = 0 or newMaskFlag = 1)
func UpdateBuild(build, inputVec, prevInput *BitVector, newMaskFlag bool, t int) {
        if t == 0 || newMaskFlag {
                // Reset build to zero
                build.Zero()
        } else {
                // Bt = (It XOR It-1) OR Bt-1
                // Calculate changes
                changes := inputVec.XOR(prevInput)

                // Update build: build = changes OR build
                build.ORInto(changes, build)
        }
}

// UpdateMask updates the mask vector (CCSDS Equation 7).
//
// The mask tracks which bits are unpredictable.
// When newMaskFlag is set, the mask is replaced with the build vector.
//
// Equation:
//   - Mt = (It XOR It-1) OR Mt-1  (if newMaskFlag = 0)
//   - Mt = (It XOR It-1) OR Bt-1  (if newMaskFlag = 1)
func UpdateMask(mask, inputVec, prevInput, buildPrev *BitVector, newMaskFlag bool) {
        // Calculate changes: It XOR It-1
        changes := inputVec.XOR(prevInput)

        if newMaskFlag {
                // Mt = changes OR Bt-1
                mask.ORInto(changes, buildPrev)
        } else {
                // Mt = changes OR Mt-1
                result := changes.OR(mask)
                mask.CopyFrom(result)
        }
}

// ComputeChange computes the change vector (CCSDS Equation 8).
//
// The change vector tracks which mask bits changed between time steps.
// This is used in encoding to communicate mask updates.
//
// Equation:
//   - Dt = Mt XOR Mt-1  (if t > 0)
//   - Dt = 0            (if t = 0, CCSDS Eq. 8: no change at initialization)
func ComputeChange(change, mask, prevMask *BitVector, t int) {
        if t == 0 {
                // CCSDS Eq. 8: D0 = 0 — both encoder and decoder start from the
                // same user-specified M0, so there is no change to communicate
                change.Zero()
        } else {
                // Dt = Mt XOR Mt-1
                change.XORInto(mask, prevMask)
        }
}

// ComputeKt computes the kt component for encoding.
//
// kt = BE(Mt, Dt) - bits of mask at positions where change occurred
// This tells the decompressor which mask values to expect.
func ComputeKt(bb *BitBuffer, mask, change *BitVector) error {
        return BitExtractForward(bb, mask, change)
}

// ApplyPrediction applies prediction to get predicted value.
//
// Equation:
//   - P(It) = It-1 AND Mt (prediction based on mask)
//   - If mask bit is 1, predict same as previous
//   - If mask bit is 0, predict 0
func ApplyPrediction(prevInput, mask *BitVector) *BitVector {
        return prevInput.AND(mask)
}

// ComputeResidual computes residual (difference from prediction).
//
// Equation:
//   - Rt = It XOR P(It)
//   - Rt = It XOR (It-1 AND Mt)
func ComputeResidual(input, prevInput, mask *BitVector) *BitVector {
        prediction := ApplyPrediction(prevInput, mask)
        return input.XOR(prediction)
}

// ApplyReconstruction reconstructs input from residual and prediction.
//
// Equation:
//   - It = Rt XOR P(It)
//   - It = Rt XOR (It-1 AND Mt)
func ApplyReconstruction(residual, prevInput, mask *BitVector) *BitVector {
        prediction := ApplyPrediction(prevInput, mask)
        return residual.XOR(prediction)
}