audio_codec/

g722.rs

use super::{Decoder, Encoder, PcmBuf, Sample};

pub enum Bitrate {
    Mode1_64000,
    Mode2_56000,
    Mode3_48000,
}

// Quantization decision thresholds used in the encoder
const QUANT_DECISION_LEVEL: [i32; 32] = [
    0, 35, 72, 110, 150, 190, 233, 276, 323, 370, 422, 473, 530, 587, 650, 714, 786, 858, 940,
    1023, 1121, 1219, 1339, 1458, 1612, 1765, 1980, 2195, 2557, 2919, 0, 0,
];

// Negative quantization interval indices
const QUANT_INDEX_NEG: [i32; 32] = [
    0, 63, 62, 31, 30, 29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11,
    10, 9, 8, 7, 6, 5, 4, 0,
];

// Positive quantization interval indices
const QUANT_INDEX_POS: [i32; 32] = [
    0, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39,
    38, 37, 36, 35, 34, 33, 32, 0,
];

// Scale factor adaptation table for low band
const SCALE_FACTOR_ADJUST_LOW: [i32; 8] = [-60, -30, 58, 172, 334, 538, 1198, 3042];

// Mapping from 4 bits of low band code to 3 bits of logarithmic scale factor
const LOG_SCALE_FACTOR_MAP: [i32; 16] = [0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0];

// Inverse logarithmic base for computing the scale factor
const INV_LOG_BASE: [i32; 32] = [
    2048, 2093, 2139, 2186, 2233, 2282, 2332, 2383, 2435, 2489, 2543, 2599, 2656, 2714, 2774, 2834,
    2896, 2960, 3025, 3091, 3158, 3228, 3298, 3371, 3444, 3520, 3597, 3676, 3756, 3838, 3922, 4008,
];

// Quantizer multipliers for 4-bit low band
const QUANT_MULT_LOW_4BIT: [i32; 16] = [
    0, -20456, -12896, -8968, -6288, -4240, -2584, -1200, 20456, 12896, 8968, 6288, 4240, 2584,
    1200, 0,
];

// Quantizer multipliers for 2-bit high band
const QUANT_MULT_HIGH_2BIT: [i32; 4] = [-7408, -1616, 7408, 1616];

// QMF filter coefficients for band splitting and reconstruction
const QMF_FILTER_COEFS: [i32; 12] = [3, -11, 12, 32, -210, 951, 3876, -805, 362, -156, 53, -11];

// Negative high band quantization indices
const HIGH_QUANT_INDEX_NEG: [i32; 3] = [0, 1, 0];

// Positive high band quantization indices
const HIGH_QUANT_INDEX_POS: [i32; 3] = [0, 3, 2];

// Scale factor adaptation table for high band
const SCALE_FACTOR_ADJUST_HIGH: [i32; 3] = [0, -214, 798];

// Mapping from 2 bits of high band code to 2 bits of logarithmic scale factor
const HIGH_LOG_SCALE_MAP: [i32; 4] = [2, 1, 2, 1];

// Quantizer multipliers for 5-bit quantization (56kbps mode)
const QUANT_MULT_56K: [i32; 32] = [
    -280, -280, -23352, -17560, -14120, -11664, -9752, -8184, -6864, -5712, -4696, -3784, -2960,
    -2208, -1520, -880, 23352, 17560, 14120, 11664, 9752, 8184, 6864, 5712, 4696, 3784, 2960, 2208,
    1520, 880, 280, -280,
];

// Quantizer multipliers for 6-bit quantization (64kbps mode)
const QUANT_MULT_64K: [i32; 64] = [
    -136, -136, -136, -136, -24808, -21904, -19008, -16704, -14984, -13512, -12280, -11192, -10232,
    -9360, -8576, -7856, -7192, -6576, -6000, -5456, -4944, -4464, -4008, -3576, -3168, -2776,
    -2400, -2032, -1688, -1360, -1040, -728, 24808, 21904, 19008, 16704, 14984, 13512, 12280,
    11192, 10232, 9360, 8576, 7856, 7192, 6576, 6000, 5456, 4944, 4464, 4008, 3576, 3168, 2776,
    2400, 2032, 1688, 1360, 1040, 728, 432, 136, -432, -136,
];

impl Bitrate {
    fn bits_per_sample(&self) -> i32 {
        match self {
            Bitrate::Mode1_64000 => 8,
            Bitrate::Mode2_56000 => 7,
            Bitrate::Mode3_48000 => 6,
        }
    }
}

/// G.722 ADPCM band state structure used in the codec
/// Each band (lower and upper) uses an independent state structure
#[derive(Default)]
struct G722Band {
    /// Current signal prediction value (signal estimate)
    signal_estimate: i32,
    /// Pole filter output (result from IIR filter part)
    pole_filter_output: i32,
    /// Zero filter output (result from FIR filter part)
    zero_filter_output: i32,
    /// Reconstructed signal history [current, previous, previous-1]
    reconstructed_signal: [i32; 3],
    /// Pole filter coefficients [unused(0), a1, a2]
    pole_coefficients: [i32; 3],
    /// Temporary pole filter coefficients [unused(0), a1', a2']
    pole_coefficients_temp: [i32; 3],
    /// Partially reconstructed signal history [current, previous, previous-1]
    partial_reconstructed: [i32; 3],
    /// Difference signal history [current, previous, ..., previous-5]
    difference_signal: [i32; 7],
    /// Zero filter coefficients [unused(0), b1, b2, ..., b6]
    zero_coefficients: [i32; 7],
    /// Temporary zero filter coefficients [unused(0), b1', b2', ..., b6']
    zero_coefficients_temp: [i32; 7],
    /// Log scale factor (used for quantization and dequantization)
    log_scale_factor: i32,
    /// Quantizer step size (used for adaptive quantization)
    quantizer_step_size: i32,
}

#[inline(always)]
fn saturate(amp: i32) -> i32 {
    amp.clamp(i16::MIN as i32, i16::MAX as i32)
}

/// Process Block 4 operations for G.722 ADPCM algorithm
/// This function performs the predictor adaptation and reconstruction steps
/// as defined in the G.722 standard
#[inline]
fn block4(band: &mut G722Band, d: i32) {
    // Block 4, RECONS - Reconstruct the signal
    band.difference_signal[0] = d;
    band.reconstructed_signal[0] = saturate(band.signal_estimate + d);

    // Block 4, PARREC - Partial reconstruction
    let partial_rec0 = saturate(band.zero_filter_output + d);
    band.partial_reconstructed[0] = partial_rec0;

    // Block 4, UPPOL2 - Update second predictor coefficient
    let s0 = partial_rec0 >> 15;
    let s1 = band.partial_reconstructed[1] >> 15;
    let s2 = band.partial_reconstructed[2] >> 15;

    let a1_scaled = saturate(band.pole_coefficients[1] << 2);

    let mut a2_update = if s0 == s1 { -a1_scaled } else { a1_scaled };
    a2_update = a2_update.min(32767);

    let mut a2_adj = a2_update >> 7;
    a2_adj += if s0 == s2 { 128 } else { -128 };
    a2_adj += (band.pole_coefficients[2] * 32512) >> 15;

    band.pole_coefficients_temp[2] = a2_adj.clamp(-12288, 12288);

    // Block 4, UPPOL1 - Update first predictor coefficient
    let sign_factor = if s0 == s1 { 192 } else { -192 };
    let leakage = (band.pole_coefficients[1] * 32640) >> 15;
    band.pole_coefficients_temp[1] = saturate(sign_factor + leakage);

    let limit = saturate(15360 - band.pole_coefficients_temp[2]);
    band.pole_coefficients_temp[1] = band.pole_coefficients_temp[1].clamp(-limit, limit);

    // Block 4, UPZERO - Update zero section (FIR) coefficients
    let step_size = if d == 0 { 0 } else { 128 };
    let sd = d >> 15;

    {
        macro_rules! update_zero {
            ($i:expr) => {
                let sz = band.difference_signal[$i] >> 15;
                let adj = if sz == sd { step_size } else { -step_size };
                let leakage = (band.zero_coefficients[$i] * 32640) >> 15;
                band.zero_coefficients_temp[$i] = saturate(adj + leakage);
            };
        }
        update_zero!(1);
        update_zero!(2);
        update_zero!(3);
        update_zero!(4);
        update_zero!(5);
        update_zero!(6);
    }

    // Block 4, DELAYA - Delay updates for filter memory
    band.difference_signal[6] = band.difference_signal[5];
    band.difference_signal[5] = band.difference_signal[4];
    band.difference_signal[4] = band.difference_signal[3];
    band.difference_signal[3] = band.difference_signal[2];
    band.difference_signal[2] = band.difference_signal[1];
    band.difference_signal[1] = d;

    band.zero_coefficients[1] = band.zero_coefficients_temp[1];
    band.zero_coefficients[2] = band.zero_coefficients_temp[2];
    band.zero_coefficients[3] = band.zero_coefficients_temp[3];
    band.zero_coefficients[4] = band.zero_coefficients_temp[4];
    band.zero_coefficients[5] = band.zero_coefficients_temp[5];
    band.zero_coefficients[6] = band.zero_coefficients_temp[6];

    band.reconstructed_signal[2] = band.reconstructed_signal[1];
    band.reconstructed_signal[1] = band.reconstructed_signal[0];
    band.partial_reconstructed[2] = band.partial_reconstructed[1];
    band.partial_reconstructed[1] = partial_rec0;

    band.pole_coefficients[1] = band.pole_coefficients_temp[1];
    band.pole_coefficients[2] = band.pole_coefficients_temp[2];

    // Block 4, FILTEP - Pole section (IIR) filtering
    let r1_adj = saturate(band.reconstructed_signal[1] << 1);
    let pole1 = (band.pole_coefficients[1] * r1_adj) >> 15;

    let r2_adj = saturate(band.reconstructed_signal[2] << 1);
    let pole2 = (band.pole_coefficients[2] * r2_adj) >> 15;

    band.pole_filter_output = saturate(pole1 + pole2);

    // Block 4, FILTEZ - Zero section (FIR) filtering
    let mut zero_out = (band.zero_coefficients[1] * saturate(band.difference_signal[1] << 1)) >> 15;
    zero_out += (band.zero_coefficients[2] * saturate(band.difference_signal[2] << 1)) >> 15;
    zero_out += (band.zero_coefficients[3] * saturate(band.difference_signal[3] << 1)) >> 15;
    zero_out += (band.zero_coefficients[4] * saturate(band.difference_signal[4] << 1)) >> 15;
    zero_out += (band.zero_coefficients[5] * saturate(band.difference_signal[5] << 1)) >> 15;
    zero_out += (band.zero_coefficients[6] * saturate(band.difference_signal[6] << 1)) >> 15;

    band.zero_filter_output = saturate(zero_out);

    // Block 4, PREDIC - Prediction
    band.signal_estimate = saturate(band.pole_filter_output + band.zero_filter_output);
}

pub struct G722Encoder {
    packed: bool,
    eight_k: bool,
    bits_per_sample: i32,
    x: [i32; 24],
    band: [G722Band; 2],
    out_buffer: u32,
    out_bits: i32,
}

pub struct G722Decoder {
    packed: bool,
    eight_k: bool,
    bits_per_sample: i32,
    x: [i32; 24],
    band: [G722Band; 2],
    in_buffer: u32,
    in_bits: i32,
}

impl G722Encoder {
    pub fn new() -> Self {
        Self::with_options(Bitrate::Mode1_64000, false, false)
    }

    /// Creates an encoder with specified bitrate and options
    pub fn with_options(rate: Bitrate, eight_k: bool, packed: bool) -> Self {
        let mut encoder = Self {
            packed,
            eight_k,
            bits_per_sample: rate.bits_per_sample(),
            x: [0; 24],
            band: [G722Band::default(), G722Band::default()],
            out_buffer: 0,
            out_bits: 0,
        };

        // Initialize band states with correct starting values
        encoder.band[0].log_scale_factor = 32 << 2; // Initial det value for lower band
        encoder.band[1].log_scale_factor = 8 << 2; // Initial det value for upper band

        encoder
    }

    /// Encode 16-bit PCM samples into G.722 format
    /// This function follows the G.722 standard algorithm exactly
    fn g722_encode(&mut self, amp: &[i16]) -> Vec<u8> {
        // Pre-allocate output buffer with appropriate capacity
        let mut output = Vec::with_capacity(amp.len() / 2 + 1);

        // Initialize processing variables
        let mut input_idx = 0;

        if self.eight_k {
            while input_idx < amp.len() {
                // 8kHz mode - Just use input directly with scaling
                let xlow = amp[input_idx] as i32 >> 1;
                input_idx += 1;

                // 8kHz mode - only low band matters
                let code = self.encode_low_band(xlow, true);
                self.output_code(code, &mut output);
            }
        } else {
            // Process all input samples in 16kHz mode
            // Use chunks_exact(2) for better performance and to avoid bound checks
            let chunks = amp.chunks_exact(2);
            let rem = chunks.remainder();

            for chunk in chunks {
                // Shuffle buffer down to make room for new samples
                self.x.copy_within(2..24, 0);

                // Add new samples to buffer
                self.x[22] = chunk[0] as i32;
                self.x[23] = chunk[1] as i32;

                // Apply QMF filter to split input into bands
                // Unrolled loop for 12 coefficients
                let mut sumodd = self.x[0] * QMF_FILTER_COEFS[0];
                sumodd += self.x[2] * QMF_FILTER_COEFS[1];
                sumodd += self.x[4] * QMF_FILTER_COEFS[2];
                sumodd += self.x[6] * QMF_FILTER_COEFS[3];
                sumodd += self.x[8] * QMF_FILTER_COEFS[4];
                sumodd += self.x[10] * QMF_FILTER_COEFS[5];
                sumodd += self.x[12] * QMF_FILTER_COEFS[6];
                sumodd += self.x[14] * QMF_FILTER_COEFS[7];
                sumodd += self.x[16] * QMF_FILTER_COEFS[8];
                sumodd += self.x[18] * QMF_FILTER_COEFS[9];
                sumodd += self.x[20] * QMF_FILTER_COEFS[10];
                sumodd += self.x[22] * QMF_FILTER_COEFS[11];

                let mut sumeven = self.x[1] * QMF_FILTER_COEFS[11];
                sumeven += self.x[3] * QMF_FILTER_COEFS[10];
                sumeven += self.x[5] * QMF_FILTER_COEFS[9];
                sumeven += self.x[7] * QMF_FILTER_COEFS[8];
                sumeven += self.x[9] * QMF_FILTER_COEFS[7];
                sumeven += self.x[11] * QMF_FILTER_COEFS[6];
                sumeven += self.x[13] * QMF_FILTER_COEFS[5];
                sumeven += self.x[15] * QMF_FILTER_COEFS[4];
                sumeven += self.x[17] * QMF_FILTER_COEFS[3];
                sumeven += self.x[19] * QMF_FILTER_COEFS[2];
                sumeven += self.x[21] * QMF_FILTER_COEFS[1];
                sumeven += self.x[23] * QMF_FILTER_COEFS[0];

                // Scale filter outputs to get low and high bands
                let xlow = (sumeven + sumodd) >> 14;
                let xhigh = (sumeven - sumodd) >> 14;

                // 16kHz mode - encode both bands
                let ilow = self.encode_low_band(xlow, false);
                let ihigh = self.encode_high_band(xhigh);
                let code = (ihigh << 6 | ilow) >> (8 - self.bits_per_sample);

                // Output the encoded code
                self.output_code(code, &mut output);
            }

            if !rem.is_empty() {
                self.x.copy_within(2..24, 0);
                self.x[22] = rem[0] as i32;
                self.x[23] = 0;
                let mut sumodd = self.x[0] * QMF_FILTER_COEFS[0];
                sumodd += self.x[2] * QMF_FILTER_COEFS[1];
                sumodd += self.x[4] * QMF_FILTER_COEFS[2];
                sumodd += self.x[6] * QMF_FILTER_COEFS[3];
                sumodd += self.x[8] * QMF_FILTER_COEFS[4];
                sumodd += self.x[10] * QMF_FILTER_COEFS[5];
                sumodd += self.x[12] * QMF_FILTER_COEFS[6];
                sumodd += self.x[14] * QMF_FILTER_COEFS[7];
                sumodd += self.x[16] * QMF_FILTER_COEFS[8];
                sumodd += self.x[18] * QMF_FILTER_COEFS[9];
                sumodd += self.x[20] * QMF_FILTER_COEFS[10];
                sumodd += self.x[22] * QMF_FILTER_COEFS[11];

                let mut sumeven = self.x[1] * QMF_FILTER_COEFS[11];
                sumeven += self.x[3] * QMF_FILTER_COEFS[10];
                sumeven += self.x[5] * QMF_FILTER_COEFS[9];
                sumeven += self.x[7] * QMF_FILTER_COEFS[8];
                sumeven += self.x[9] * QMF_FILTER_COEFS[7];
                sumeven += self.x[11] * QMF_FILTER_COEFS[6];
                sumeven += self.x[13] * QMF_FILTER_COEFS[5];
                sumeven += self.x[15] * QMF_FILTER_COEFS[4];
                sumeven += self.x[17] * QMF_FILTER_COEFS[3];
                sumeven += self.x[19] * QMF_FILTER_COEFS[2];
                sumeven += self.x[21] * QMF_FILTER_COEFS[1];
                sumeven += self.x[23] * QMF_FILTER_COEFS[0];

                let xlow = (sumeven + sumodd) >> 14;
                let xhigh = (sumeven - sumodd) >> 14;
                let ilow = self.encode_low_band(xlow, false);
                let ihigh = self.encode_high_band(xhigh);
                let code = (ihigh << 6 | ilow) >> (8 - self.bits_per_sample);
                self.output_code(code, &mut output);
            }
        }

        // Handle any remaining bits in the output buffer
        if self.packed && self.out_bits > 0 {
            output.push((self.out_buffer & 0xFF) as u8);
        }

        output
    }

    /// Encode low band sample and update state
    /// Returns the encoded low band bits
    #[inline]
    fn encode_low_band(&mut self, xlow: i32, is_eight_k: bool) -> i32 {
        // Block 1L, SUBTRA - Calculate difference signal
        let el = saturate(xlow - self.band[0].signal_estimate);

        // Block 1L, QUANTL - Quantize difference signal
        let wd = el.abs().wrapping_sub((el >> 31) & 1);

        // Find quantization interval using linear search (more predictable for audio)
        let lsf = self.band[0].log_scale_factor;
        let mut quantization_idx = 1;
        while quantization_idx < 30 {
            let decision_level = (QUANT_DECISION_LEVEL[quantization_idx] * lsf) >> 12;
            if wd < decision_level {
                break;
            }
            quantization_idx += 1;
        }

        // Select output bits based on sign
        let ilow = if el < 0 {
            QUANT_INDEX_NEG[quantization_idx]
        } else {
            QUANT_INDEX_POS[quantization_idx]
        };

        // Block 2L, INVQAL - Inverse quantize for prediction
        let ril = ilow >> 2;
        let wd2 = QUANT_MULT_LOW_4BIT[ril as usize];
        let dlow = (self.band[0].log_scale_factor * wd2) >> 15;

        // Block 3L, LOGSCL - Update scale factor
        let il4 = LOG_SCALE_FACTOR_MAP[ril as usize];
        let mut nb = (self.band[0].quantizer_step_size * 127) >> 7;
        nb += SCALE_FACTOR_ADJUST_LOW[il4 as usize];
        self.band[0].quantizer_step_size = nb.clamp(0, 18432);

        // Block 3L, SCALEL - Compute new quantizer scale factor
        let wd1 = self.band[0].quantizer_step_size >> 6 & 31;
        let wd2 = 8 - (self.band[0].quantizer_step_size >> 11);
        let wd3 = if wd2 < 0 {
            INV_LOG_BASE[wd1 as usize] << -wd2
        } else {
            INV_LOG_BASE[wd1 as usize] >> wd2
        };
        self.band[0].log_scale_factor = wd3 << 2;

        // Apply predictor adaptation (ADPCM core algorithm)
        block4(&mut self.band[0], dlow);

        // Return appropriate value based on mode
        if is_eight_k {
            ((0xc0 | ilow) >> 8) - self.bits_per_sample
        } else {
            ilow
        }
    }

    /// Encode high band sample and update state
    /// Returns the encoded high band bits
    #[inline]
    fn encode_high_band(&mut self, xhigh: i32) -> i32 {
        // Block 1H, SUBTRA - Calculate difference signal
        let eh = saturate(xhigh - self.band[1].signal_estimate);

        // Block 1H, QUANTH - Quantize difference signal
        let wd = if eh >= 0 { eh } else { -(eh + 1) };
        let decision_level = (564 * self.band[1].log_scale_factor) >> 12;

        // Determine quantization level for high band (2-bit)
        let mih = if wd >= decision_level { 2 } else { 1 };
        let ihigh = if eh < 0 {
            HIGH_QUANT_INDEX_NEG[mih as usize]
        } else {
            HIGH_QUANT_INDEX_POS[mih as usize]
        };

        // Block 2H, INVQAH - Inverse quantize for prediction
        let wd2 = QUANT_MULT_HIGH_2BIT[ihigh as usize];
        let dhigh = (self.band[1].log_scale_factor * wd2) >> 15;

        // Block 3H, LOGSCH - Update scale factor
        let ih2 = HIGH_LOG_SCALE_MAP[ihigh as usize];
        let mut nb = (self.band[1].quantizer_step_size * 127) >> 7;
        nb += SCALE_FACTOR_ADJUST_HIGH[ih2 as usize];
        self.band[1].quantizer_step_size = nb.clamp(0, 22528);

        // Block 3H, SCALEH - Compute quantizer scale factor
        let wd1 = self.band[1].quantizer_step_size >> 6 & 31;
        let wd2 = 10 - (self.band[1].quantizer_step_size >> 11);
        let wd3 = if wd2 < 0 {
            INV_LOG_BASE[wd1 as usize] << -wd2
        } else {
            INV_LOG_BASE[wd1 as usize] >> wd2
        };
        self.band[1].log_scale_factor = wd3 << 2;

        // Apply predictor adaptation (ADPCM core algorithm)
        block4(&mut self.band[1], dhigh);

        ihigh
    }

    /// Add encoded bits to the output buffer
    fn output_code(&mut self, code: i32, output: &mut Vec<u8>) {
        if self.packed {
            // Pack the code bits across byte boundaries
            self.out_buffer |= (code as u32) << self.out_bits;
            self.out_bits += self.bits_per_sample;

            // When we have at least 8 bits, output a byte
            if self.out_bits >= 8 {
                output.push((self.out_buffer & 0xFF) as u8);
                self.out_bits -= 8;
                self.out_buffer >>= 8;
            }
        } else {
            // Direct byte-aligned output
            output.push(code as u8);
        }
    }
}

impl G722Decoder {
    pub fn new() -> Self {
        Self::with_options(Bitrate::Mode1_64000, false, false)
    }

    pub fn with_options(rate: Bitrate, packed: bool, eight_k: bool) -> Self {
        Self {
            packed,
            eight_k,
            bits_per_sample: rate.bits_per_sample(),
            x: Default::default(),
            band: Default::default(),
            in_buffer: 0,
            in_bits: 0,
        }
    }

    /// Extracts the next G.722 code from the input data stream
    #[inline]
    fn extract_code(&mut self, data: &[u8], idx: &mut usize) -> i32 {
        if self.packed {
            // When packed, bits are combined across bytes
            if self.in_bits < self.bits_per_sample {
                self.in_buffer |= (data[*idx] as u32) << self.in_bits;
                *idx += 1;
                self.in_bits += 8;
            }
            let code = (self.in_buffer & ((1 << self.bits_per_sample) - 1) as u32) as i32;
            self.in_buffer >>= self.bits_per_sample;
            self.in_bits -= self.bits_per_sample;
            code
        } else {
            // Direct byte-based access when not packed
            let code = data[*idx] as i32;
            *idx += 1;
            code
        }
    }

    /// Parses the G.722 code into low-band word and high-band index based on bit rate
    #[inline]
    fn parse_code(&self, code: i32) -> (i32, i32, i32) {
        // Returns (wd1, ihigh, wd2) tuple: low-band word, high-band index, and scaled value
        match self.bits_per_sample {
            7 => {
                // 56 kbit/s mode
                let wd1 = code & 0x1f;
                let ihigh = (code >> 5) & 0x3;
                let wd2 = QUANT_MULT_56K[wd1 as usize];
                (wd1 >> 1, ihigh, wd2)
            }
            6 => {
                // 48 kbit/s mode
                let wd1 = code & 0xf;
                let ihigh = (code >> 4) & 0x3;
                let wd2 = QUANT_MULT_LOW_4BIT[wd1 as usize];
                (wd1, ihigh, wd2)
            }
            _ => {
                // 64 kbit/s mode (default)
                let wd1 = code & 0x3f;
                let ihigh = (code >> 6) & 0x3;
                let wd2 = QUANT_MULT_64K[wd1 as usize];
                (wd1 >> 2, ihigh, wd2)
            }
        }
    }

    /// Process the low band component of the G.722 stream
    #[inline]
    fn process_low_band(&mut self, wd1: i32, wd2: i32) -> i32 {
        // Block 5L, LOW BAND INVQBL - Inverse quantization for low band
        let dequant = (self.band[0].log_scale_factor * wd2) >> 15;

        // Block 5L, RECONS - Reconstruction of low band signal
        let rlow = self.band[0].signal_estimate + dequant;

        // Block 6L, LIMIT - Limiting to valid range
        let rlow = rlow.clamp(-16384, 16383);

        // Block 2L, INVQAL - Inverse adaptive quantizer for prediction
        let wd2 = QUANT_MULT_LOW_4BIT[wd1 as usize];
        let dlowt = (self.band[0].log_scale_factor * wd2) >> 15;

        // Block 3L, LOGSCL - Compute log scale factor
        let wd2 = LOG_SCALE_FACTOR_MAP[wd1 as usize];
        let mut wd1 = (self.band[0].quantizer_step_size * 127) >> 7;
        wd1 += SCALE_FACTOR_ADJUST_LOW[wd2 as usize];
        self.band[0].quantizer_step_size = wd1.clamp(0, 18432);

        // Block 3L, SCALEL - Compute quantizer scale factor
        let wd1 = (self.band[0].quantizer_step_size >> 6) & 31;
        let wd2 = 8 - (self.band[0].quantizer_step_size >> 11);
        let wd3 = if wd2 < 0 {
            INV_LOG_BASE[wd1 as usize] << -wd2
        } else {
            INV_LOG_BASE[wd1 as usize] >> wd2
        };
        self.band[0].log_scale_factor = wd3 << 2;

        // Apply predictor adaptation
        block4(&mut self.band[0], dlowt);

        rlow
    }

    /// Process the high band component of the G.722 stream
    #[inline]
    fn process_high_band(&mut self, ihigh: i32) -> i32 {
        // Block 2H, INVQAH - Inverse quantizer for high band
        let wd2 = QUANT_MULT_HIGH_2BIT[ihigh as usize];
        let dhigh = (self.band[1].log_scale_factor * wd2) >> 15;

        // Block 5H, RECONS - Reconstruction of high band signal
        let rhigh = dhigh + self.band[1].signal_estimate;

        // Block 6H, LIMIT - Limiting to valid range
        let rhigh = rhigh.clamp(-16384, 16383);

        // Block 2H, INVQAH - Adaptation logic
        let wd2 = HIGH_LOG_SCALE_MAP[ihigh as usize];
        let mut wd1 = (self.band[1].quantizer_step_size * 127) >> 7;
        wd1 += SCALE_FACTOR_ADJUST_HIGH[wd2 as usize];
        self.band[1].quantizer_step_size = wd1.clamp(0, 22528);

        // Block 3H, SCALEH - Compute quantizer scale factor
        let wd1 = (self.band[1].quantizer_step_size >> 6) & 31;
        let wd2 = 10 - (self.band[1].quantizer_step_size >> 11);
        let wd3 = if wd2 < 0 {
            INV_LOG_BASE[wd1 as usize] << -wd2
        } else {
            INV_LOG_BASE[wd1 as usize] >> wd2
        };
        self.band[1].log_scale_factor = wd3 << 2;

        // Apply predictor adaptation
        block4(&mut self.band[1], dhigh);

        rhigh
    }

    /// Apply QMF synthesis filter to combine low and high band signals
    #[inline]
    fn apply_qmf_synthesis(&mut self, rlow: i32, rhigh: i32) -> [i16; 2] {
        // Shift filter state
        self.x.copy_within(2..24, 0);

        // Set new filter state values
        self.x[22] = rlow + rhigh;
        self.x[23] = rlow - rhigh;

        // Apply QMF synthesis filter (unrolled loop for 12 coefficients)
        let mut xout2 = self.x[0] * QMF_FILTER_COEFS[0];
        xout2 += self.x[2] * QMF_FILTER_COEFS[1];
        xout2 += self.x[4] * QMF_FILTER_COEFS[2];
        xout2 += self.x[6] * QMF_FILTER_COEFS[3];
        xout2 += self.x[8] * QMF_FILTER_COEFS[4];
        xout2 += self.x[10] * QMF_FILTER_COEFS[5];
        xout2 += self.x[12] * QMF_FILTER_COEFS[6];
        xout2 += self.x[14] * QMF_FILTER_COEFS[7];
        xout2 += self.x[16] * QMF_FILTER_COEFS[8];
        xout2 += self.x[18] * QMF_FILTER_COEFS[9];
        xout2 += self.x[20] * QMF_FILTER_COEFS[10];
        xout2 += self.x[22] * QMF_FILTER_COEFS[11];

        let mut xout1 = self.x[1] * QMF_FILTER_COEFS[11];
        xout1 += self.x[3] * QMF_FILTER_COEFS[10];
        xout1 += self.x[5] * QMF_FILTER_COEFS[9];
        xout1 += self.x[7] * QMF_FILTER_COEFS[8];
        xout1 += self.x[9] * QMF_FILTER_COEFS[7];
        xout1 += self.x[11] * QMF_FILTER_COEFS[6];
        xout1 += self.x[13] * QMF_FILTER_COEFS[5];
        xout1 += self.x[15] * QMF_FILTER_COEFS[4];
        xout1 += self.x[17] * QMF_FILTER_COEFS[3];
        xout1 += self.x[19] * QMF_FILTER_COEFS[2];
        xout1 += self.x[21] * QMF_FILTER_COEFS[1];
        xout1 += self.x[23] * QMF_FILTER_COEFS[0];

        // Return reconstructed samples with proper scaling
        [saturate(xout1 >> 11) as i16, saturate(xout2 >> 11) as i16]
    }

    /// Decodes a G.722 frame and returns PCM samples
    /// This is the main decoding function that processes G.722 encoded data
    pub fn decode_frame(&mut self, data: &[u8]) -> PcmBuf {
        let mut output = Vec::with_capacity(data.len() * 2);
        let mut idx = 0;

        if self.eight_k {
            while idx < data.len() {
                let code = self.extract_code(data, &mut idx);
                let (wd1, _, wd2) = self.parse_code(code);
                let rlow = self.process_low_band(wd1, wd2);
                output.push((rlow << 1) as i16);
            }
        } else {
            while idx < data.len() {
                let code = self.extract_code(data, &mut idx);
                let (wd1, ihigh, wd2) = self.parse_code(code);
                let rlow = self.process_low_band(wd1, wd2);
                let rhigh = self.process_high_band(ihigh);
                let pcm = self.apply_qmf_synthesis(rlow, rhigh);
                output.extend_from_slice(&pcm);
            }
        }
        output
    }
}

impl Encoder for G722Encoder {
    fn encode(&mut self, samples: &[Sample]) -> Vec<u8> {
        self.g722_encode(samples)
    }

    fn sample_rate(&self) -> u32 {
        16000 // G.722 encoding sample rate is 16kHz
    }

    fn channels(&self) -> u16 {
        1 // G.722 is mono encoding
    }
}

impl Decoder for G722Decoder {
    fn decode(&mut self, data: &[u8]) -> PcmBuf {
        self.decode_frame(data)
    }

    fn sample_rate(&self) -> u32 {
        16000
    }

    fn channels(&self) -> u16 {
        1
    }
}