/*
    * Sbc.cpp
    * SBC (Sub-Band Codec) encoder — fixed-point implementation
    * Based on the Bluetooth SIG specification (A2DP v1.3, Appendix B)
    * All arithmetic is 32-bit fixed-point (Q15.16 or Q1.30)
    * Copyright (c) 2026 Daniel Hammer
*/

#include "Sbc.hpp"
#include <Libraries/Memory.hpp>

namespace Drivers::USB::Bluetooth::Sbc {

    // =========================================================================
    // Analysis filterbank tables
    // =========================================================================

    // SBC 8-subband prototype window, the 80 coefficients from the A2DP spec
    // (Annex B), natural order, signs built in, Q31 fixed point.  Values match
    // the spec floats exactly (e.g. [1] = 0x52173/2^31 = 1.56575398E-04).
    // Verified on host: feeding subband-center sines concentrates energy in the
    // right band with >63 dB rejection and overall gain 0.9997.
    static const int32_t g_proto8[80] = {
        (int32_t)0x00000000, (int32_t)0x00052173, (int32_t)0x000B3F71, (int32_t)0x00122C7D,
        (int32_t)0x001AFF89, (int32_t)0x00255A62, (int32_t)0x003060F4, (int32_t)0x003A72E7,
        (int32_t)0x0041EC6A, (int32_t)0x0044EF48, (int32_t)0x00415B75, (int32_t)0x0034F8B6,
        (int32_t)0x001D8FD2, (int32_t)0xFFFA2413, (int32_t)0xFFC9F10E, (int32_t)0xFF8D6793,
        (int32_t)0x00B97348, (int32_t)0x01071B96, (int32_t)0x0156B3CA, (int32_t)0x01A1B38B,
        (int32_t)0x01E0224C, (int32_t)0x0209291F, (int32_t)0x02138653, (int32_t)0x01F5F424,
        (int32_t)0x01A7ECEF, (int32_t)0x01223EBA, (int32_t)0x005FD0FF, (int32_t)0xFF5EEB73,
        (int32_t)0xFE20435D, (int32_t)0xFCA86E7E, (int32_t)0xFAFF95FC, (int32_t)0xF9312891,
        (int32_t)0x08B4307A, (int32_t)0x0A9F3E9A, (int32_t)0x0C7D59B6, (int32_t)0x0E3BB16F,
        (int32_t)0x0FC721F9, (int32_t)0x110ECEF0, (int32_t)0x120435FA, (int32_t)0x129C226F,
        (int32_t)0x12CF6C75, (int32_t)0x129C226F, (int32_t)0x120435FA, (int32_t)0x110ECEF0,
        (int32_t)0x0FC721F9, (int32_t)0x0E3BB16F, (int32_t)0x0C7D59B6, (int32_t)0x0A9F3E9A,
        (int32_t)0xF74BCF86, (int32_t)0xF9312891, (int32_t)0xFAFF95FC, (int32_t)0xFCA86E7E,
        (int32_t)0xFE20435D, (int32_t)0xFF5EEB73, (int32_t)0x005FD0FF, (int32_t)0x01223EBA,
        (int32_t)0x01A7ECEF, (int32_t)0x01F5F424, (int32_t)0x02138653, (int32_t)0x0209291F,
        (int32_t)0x01E0224C, (int32_t)0x01A1B38B, (int32_t)0x0156B3CA, (int32_t)0x01071B96,
        (int32_t)0xFF468CB8, (int32_t)0xFF8D6793, (int32_t)0xFFC9F10E, (int32_t)0xFFFA2413,
        (int32_t)0x001D8FD2, (int32_t)0x0034F8B6, (int32_t)0x00415B75, (int32_t)0x0044EF48,
        (int32_t)0x0041EC6A, (int32_t)0x003A72E7, (int32_t)0x003060F4, (int32_t)0x00255A62,
        (int32_t)0x001AFF89, (int32_t)0x00122C7D, (int32_t)0x000B3F71, (int32_t)0x00052173,
    };

    // Analysis matrixing for 8 subbands (A2DP spec Annex B), Q14:
    // M[k][i] = cos((i - 4) * (2k + 1) * pi / 16) * 2^14
    // (The (i-4) phase is the one whose aliasing cancels in the reference
    // decoder's synthesis; (i+4) passes an energy-concentration test equally
    // but reconstructs with frequency-dependent distortion.)
    static const int16_t g_cosMatrix8[8][16] = {
        {  11585,  13623,  15137,  16069,  16384,  16069,  15137,  13623,  11585,   9102,   6270,   3196,      0,  -3196,  -6270,  -9102 },
        { -11585,  -3196,   6270,  13623,  16384,  13623,   6270,  -3196, -11585, -16069, -15137,  -9102,      0,   9102,  15137,  16069 },
        { -11585, -16069,  -6270,   9102,  16384,   9102,  -6270, -16069, -11585,   3196,  15137,  13623,      0, -13623, -15137,  -3196 },
        {  11585,  -9102, -15137,   3196,  16384,   3196, -15137,  -9102,  11585,  13623,  -6270, -16069,      0,  16069,   6270, -13623 },
        {  11585,   9102, -15137,  -3196,  16384,  -3196, -15137,   9102,  11585, -13623,  -6270,  16069,      0, -16069,   6270,  13623 },
        { -11585,  16069,  -6270,  -9102,  16384,  -9102,  -6270,  16069, -11585,  -3196,  15137, -13623,      0,  13623, -15137,   3196 },
        { -11585,   3196,   6270, -13623,  16384, -13623,   6270,   3196, -11585,  16069, -15137,   9102,      0,  -9102,  15137, -16069 },
        {  11585, -13623,  15137, -16069,  16384, -16069,  15137, -13623,  11585,  -9102,   6270,  -3196,      0,   3196,  -6270,   9102 },
    };

    // =========================================================================
    // CRC-8 table (SBC spec CRC polynomial: x^8 + x^4 + x^3 + x^2 + 1)
    // =========================================================================

    static uint8_t SbcCrc8(const uint8_t* data, uint32_t len, uint8_t bits_last_byte) {
        uint8_t crc = 0x0F;
        for (uint32_t i = 0; i < len; i++) {
            uint8_t byte = data[i];
            uint8_t nbits = (i == len - 1) ? bits_last_byte : 8;
            for (uint8_t bit = 0; bit < nbits; bit++) {
                uint8_t msb = (crc >> 7) & 1;
                crc <<= 1;
                if (((byte >> (7 - bit)) & 1) ^ msb) {
                    crc ^= 0x1D;
                }
            }
        }
        return crc;
    }

    // =========================================================================
    // Init
    // =========================================================================

    void Init(SbcEncoder* enc, uint32_t sampleRate, uint8_t channels, uint8_t /*bitsPerSample*/) {
        memset(enc, 0, sizeof(SbcEncoder));

        enc->Subbands = SBC_SUBBANDS;
        enc->Blocks = SBC_BLOCKS;
        enc->Bitpool = SBC_BITPOOL;
        enc->AllocMethod = ALLOC_LOUDNESS;

        if (channels >= 2) {
            enc->Channels = 2;
            enc->ChannelMode = MODE_JOINT_STEREO;
        } else {
            enc->Channels = 1;
            enc->ChannelMode = MODE_MONO;
        }

        switch (sampleRate) {
            case 16000: enc->Frequency = FREQ_16000; break;
            case 32000: enc->Frequency = FREQ_32000; break;
            case 44100: enc->Frequency = FREQ_44100; break;
            default:    enc->Frequency = FREQ_48000; break;
        }

        enc->SamplesPerFrame = enc->Blocks * enc->Subbands;

        // Calculate frame size
        // For joint stereo: 4 + (4 * subbands * channels) / 8 + ceil(blocks * bitpool / 8) + subbands/8
        uint32_t headerBits = 32 + (4 * enc->Subbands * enc->Channels);
        if (enc->ChannelMode == MODE_JOINT_STEREO) {
            headerBits += enc->Subbands;  // join bits
        }
        uint32_t dataBits = enc->Blocks * enc->Bitpool;
        enc->FrameSize = (headerBits + dataBits + 7) / 8;
    }

    void Configure(SbcEncoder* enc, uint8_t octet0, uint8_t octet1, uint8_t bitpool) {
        // octet0: SamplingFreq[7:4] | ChannelMode[3:0]
        if      (octet0 & 0x80) enc->Frequency = FREQ_16000;
        else if (octet0 & 0x40) enc->Frequency = FREQ_32000;
        else if (octet0 & 0x20) enc->Frequency = FREQ_44100;
        else                    enc->Frequency = FREQ_48000;   // b4 (48k) or default

        if      (octet0 & 0x08) { enc->ChannelMode = MODE_MONO;         enc->Channels = 1; }
        else if (octet0 & 0x04) { enc->ChannelMode = MODE_DUAL_CHANNEL; enc->Channels = 2; }
        else if (octet0 & 0x02) { enc->ChannelMode = MODE_STEREO;       enc->Channels = 2; }
        else                    { enc->ChannelMode = MODE_JOINT_STEREO; enc->Channels = 2; }  // b0

        // octet1: BlockLength[7:4] | Subbands[3:2] | Alloc[1:0]
        if      (octet1 & 0x80) enc->Blocks = 4;
        else if (octet1 & 0x40) enc->Blocks = 8;
        else if (octet1 & 0x20) enc->Blocks = 12;
        else                    enc->Blocks = 16;               // b4 (16) or default
        enc->Subbands    = (octet1 & 0x08) ? 4 : 8;             // b3=4, else 8
        enc->AllocMethod = (octet1 & 0x02) ? ALLOC_SNR : ALLOC_LOUDNESS;  // b1=SNR, else Loudness
        if (bitpool) enc->Bitpool = bitpool;

        enc->SamplesPerFrame = enc->Blocks * enc->Subbands;
        uint32_t headerBits = 32 + (4 * enc->Subbands * enc->Channels);
        if (enc->ChannelMode == MODE_JOINT_STEREO) headerBits += enc->Subbands;
        uint32_t dataBits = enc->Blocks * enc->Bitpool;
        enc->FrameSize = (headerBits + dataBits + 7) / 8;
    }

    // =========================================================================
    // Analysis filter bank (8 subbands)
    // =========================================================================

    // Direct implementation of the spec pseudocode (8 subbands):
    //   shift X by 8; insert the 8 new samples NEWEST-FIRST (X[0] = newest)
    //   Y[i]  = sum_{j=0..4} X[i+16j] * C[i+16j]        (window, 80 taps)
    //   S[k]  = sum_{i=0..15} M[k][i] * Y[i]            (cosine matrixing)
    // Fixed point: X holds 16-bit samples, C is Q31, M is Q14.  The window
    // accumulator keeps Y at sample*2^15 (>>16 off Q31), the matrix adds 14
    // bits, so >>30 would land S at HALF input-sample units -- the 0.5x
    // analysis gain the SBC synthesis filterbank expects (its synthesis
    // doubles; verified against the BlueZ reference decoder: 1.0x decodes
    // 6 dB hot).  We shift FRAC_BITS less to carry fractional precision
    // through quantization: at bitpool 53 the quantizer step is often finer
    // than one input-sample unit, so integer subband samples would be the
    // noise floor (costs ~10 dB on sweeps).  Scale factors and the quantizer
    // account for FRAC_BITS; full-scale input still peaks at sf = 14.
    // Extra fractional bits carried in sb_samples below the half-input-unit
    // point (subband sample value = signal * 2^FRAC_BITS).
    constexpr int FRAC_BITS = 12;

    static void AnalysisFilter(SbcEncoder* enc, const int16_t* pcm, int ch,
                               int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS]) {
        int32_t* X = enc->X[ch];
        for (int blk = 0; blk < enc->Blocks; blk++) {
            for (int i = 79; i >= 8; i--) X[i] = X[i - 8];
            const int16_t* in = pcm + blk * 8 * enc->Channels + ch;
            for (int i = 0; i < 8; i++) {
                X[i] = (int32_t)in[(7 - i) * enc->Channels];
            }

            int32_t Y[16];
            for (int i = 0; i < 16; i++) {
                int64_t acc = 0;
                for (int j = 0; j < 5; j++) {
                    acc += (int64_t)X[i + 16 * j] * g_proto8[i + 16 * j];
                }
                Y[i] = (int32_t)((acc + (1 << 15)) >> 16);
            }

            for (int k = 0; k < 8; k++) {
                int64_t s = 0;
                for (int i = 0; i < 16; i++) {
                    s += (int64_t)Y[i] * g_cosMatrix8[k][i];
                }
                sb_samples[blk][k] =
                    (int32_t)((s + ((int64_t)1 << (29 - FRAC_BITS))) >> (30 - FRAC_BITS));
            }
        }
    }

    // =========================================================================
    // Bit allocation (Loudness method)
    // =========================================================================

    // Compute the 4-bit scale factor for each subband of one channel: the number
    // of bits needed to represent the largest-magnitude subband sample.  Must be
    // run on the FINAL sb_samples (after any joint-stereo transform), because the
    // decoder re-derives the bit allocation from these transmitted scale factors.
    static void ComputeScaleFactors(SbcEncoder* enc,
                                     int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS],
                                     int32_t scale_factors[SBC_SUBBANDS]) {
        for (int sb = 0; sb < enc->Subbands; sb++) {
            int32_t maxVal = 0;
            for (int blk = 0; blk < enc->Blocks; blk++) {
                int32_t val = sb_samples[blk][sb];
                if (val < 0) val = -val;
                if (val > maxVal) maxVal = val;
            }
            int32_t sf = 0, tmp = maxVal >> FRAC_BITS;
            while (tmp > 0) { sf++; tmp >>= 1; }
            scale_factors[sb] = sf;
        }
    }

    // Loudness offset table for 8 subbands (SBC spec), indexed by sampling freq.
    static const int8_t g_loudnessOffset8[4][8] = {
        {-2, 0, 0, 0, 0, 0, 0, 1},   // 16kHz
        {-3, 0, 0, 0, 0, 0, 1, 2},   // 32kHz
        {-4, 0, 0, 0, 0, 0, 1, 2},   // 44.1kHz
        {-4, 0, 0, 0, 0, 0, 1, 2},   // 48kHz
    };

    // =========================================================================
    // Bit allocation -- canonical A2DP / BlueZ algorithm
    // =========================================================================
    // The per-subband bit widths are NOT transmitted; the decoder re-derives
    // them from the scale factors + bitpool + allocation method using exactly
    // this algorithm, so the encoder must reproduce it bit-for-bit or the
    // sample bitstream desyncs into noise.  For STEREO/JOINT the two channels
    // share one bitpool (scope = both channels); for MONO each channel is
    // independent.  (The old hand-rolled version over-allocated and overran the
    // frame buffer.)
    static void AllocateScope(SbcEncoder* enc,
                              int32_t bitneed[SBC_CHANNELS][SBC_SUBBANDS],
                              uint8_t bits[SBC_CHANNELS][SBC_SUBBANDS],
                              int c0, int c1) {
        int sub = enc->Subbands;

        int maxBitneed = 0;
        for (int c = c0; c < c1; c++)
            for (int sb = 0; sb < sub; sb++)
                if (bitneed[c][sb] > maxBitneed) maxBitneed = bitneed[c][sb];

        int bitcount = 0, slicecount = 0, bitslice = maxBitneed + 1;
        do {
            bitslice--;
            bitcount += slicecount;
            slicecount = 0;
            for (int c = c0; c < c1; c++) {
                for (int sb = 0; sb < sub; sb++) {
                    if (bitneed[c][sb] > bitslice + 1 && bitneed[c][sb] < bitslice + 16)
                        slicecount++;
                    else if (bitneed[c][sb] == bitslice + 1)
                        slicecount += 2;
                }
            }
        } while (bitcount + slicecount < enc->Bitpool);

        if (bitcount + slicecount == enc->Bitpool) {
            bitcount += slicecount;
            bitslice--;
        }

        for (int c = c0; c < c1; c++) {
            for (int sb = 0; sb < sub; sb++) {
                if (bitneed[c][sb] < bitslice + 2) {
                    bits[c][sb] = 0;
                } else {
                    int b = bitneed[c][sb] - bitslice;
                    bits[c][sb] = (uint8_t)(b > 16 ? 16 : b);
                }
            }
        }

        // Distribute leftover bits in spec order (subband-major, channel-minor).
        int twoCh = (c1 - c0 == 2) ? 1 : 0;
        int c = c0, sb = 0;
        while (bitcount < enc->Bitpool && sb < sub) {
            if (bits[c][sb] >= 2 && bits[c][sb] < 16) {
                bits[c][sb]++;
                bitcount++;
            } else if (bitneed[c][sb] == bitslice + 1 && enc->Bitpool > bitcount + 1) {
                bits[c][sb] = 2;
                bitcount += 2;
            }
            if (twoCh) { if (c == c0) c = c0 + 1; else { c = c0; sb++; } }
            else sb++;
        }
        c = c0; sb = 0;
        while (bitcount < enc->Bitpool && sb < sub) {
            if (bits[c][sb] < 16) { bits[c][sb]++; bitcount++; }
            if (twoCh) { if (c == c0) c = c0 + 1; else { c = c0; sb++; } }
            else sb++;
        }
    }

    static void BitAllocation(SbcEncoder* enc,
                              int32_t scale_factors[SBC_CHANNELS][SBC_SUBBANDS],
                              uint8_t bits[SBC_CHANNELS][SBC_SUBBANDS]) {
        int sub = enc->Subbands;
        int32_t bitneed[SBC_CHANNELS][SBC_SUBBANDS];

        for (int c = 0; c < enc->Channels; c++) {
            for (int sb = 0; sb < sub; sb++) {
                if (enc->AllocMethod == ALLOC_LOUDNESS) {
                    if (scale_factors[c][sb] == 0) {
                        bitneed[c][sb] = -5;
                    } else {
                        int loud = scale_factors[c][sb] - g_loudnessOffset8[enc->Frequency][sb];
                        bitneed[c][sb] = (loud > 0) ? (loud / 2) : loud;
                    }
                } else {
                    bitneed[c][sb] = scale_factors[c][sb];
                }
            }
        }

        bool shared = (enc->ChannelMode == MODE_STEREO || enc->ChannelMode == MODE_JOINT_STEREO);
        if (shared && enc->Channels == 2) {
            AllocateScope(enc, bitneed, bits, 0, 2);
        } else {
            for (int c = 0; c < enc->Channels; c++) AllocateScope(enc, bitneed, bits, c, c + 1);
        }
    }

    // =========================================================================
    // Bit packing helpers
    // =========================================================================

    struct BitWriter {
        uint8_t* Data;
        uint32_t BitPos;
    };

    static void WriteBits(BitWriter* bw, uint32_t value, uint8_t nbits) {
        for (int i = nbits - 1; i >= 0; i--) {
            uint32_t bytePos = bw->BitPos / 8;
            uint8_t bitOff = 7 - (bw->BitPos % 8);
            if (value & (1u << i)) {
                bw->Data[bytePos] |= (1u << bitOff);
            }
            bw->BitPos++;
        }
    }

    // =========================================================================
    // Encode
    // =========================================================================

    uint32_t Encode(SbcEncoder* enc, const int16_t* pcm, uint8_t* out) {
        int32_t sb_samples[SBC_CHANNELS][SBC_BLOCKS][SBC_SUBBANDS];
        int32_t scale_factors[SBC_CHANNELS][SBC_SUBBANDS] = {};
        uint8_t bits[SBC_CHANNELS][SBC_SUBBANDS] = {};

        memset(out, 0, enc->FrameSize);

        // 1. Analysis filterbank per channel.
        for (int ch = 0; ch < enc->Channels; ch++) {
            AnalysisFilter(enc, pcm, ch, sb_samples[ch]);
        }

        // 2. Scale factors from the subband samples.
        for (int ch = 0; ch < enc->Channels; ch++) {
            ComputeScaleFactors(enc, sb_samples[ch], scale_factors[ch]);
        }

        // 3. Joint-stereo decision + mid/side transform.  A band is joined
        //    when it saves scale-factor bits: sfL + sfR > sfM + sfS (the
        //    reference encoder's criterion).  Updates BOTH samples and scale
        //    factors before bit allocation: the decoder re-derives the bit
        //    widths from the transmitted scale factors, which must reflect
        //    the FINAL (post-transform) samples, or the bitstream desyncs.
        //    The last subband never joins (carries the side channel's escape).
        uint8_t joint = 0;
        if (enc->ChannelMode == MODE_JOINT_STEREO) {
            for (int sb = 0; sb < enc->Subbands - 1; sb++) {
                int32_t mid[SBC_BLOCKS], side[SBC_BLOCKS];
                int32_t maxMid = 0, maxSide = 0;
                for (int blk = 0; blk < enc->Blocks; blk++) {
                    int32_t l = sb_samples[0][blk][sb], r = sb_samples[1][blk][sb];
                    mid[blk] = (l >> 1) + (r >> 1);
                    side[blk] = (l >> 1) - (r >> 1);
                    int32_t am = mid[blk] < 0 ? -mid[blk] : mid[blk];
                    int32_t as = side[blk] < 0 ? -side[blk] : side[blk];
                    if (am > maxMid) maxMid = am;
                    if (as > maxSide) maxSide = as;
                }
                int32_t sfm = 0, tmp = maxMid >> FRAC_BITS;
                while (tmp > 0) { sfm++; tmp >>= 1; }
                int32_t sfs = 0;
                tmp = maxSide >> FRAC_BITS;
                while (tmp > 0) { sfs++; tmp >>= 1; }

                if (scale_factors[0][sb] + scale_factors[1][sb] > sfm + sfs) {
                    joint |= (uint8_t)(1 << (enc->Subbands - 1 - sb));
                    scale_factors[0][sb] = sfm;
                    scale_factors[1][sb] = sfs;
                    for (int blk = 0; blk < enc->Blocks; blk++) {
                        sb_samples[0][blk][sb] = mid[blk];
                        sb_samples[1][blk][sb] = side[blk];
                    }
                }
            }
        }

        // 4. Bit allocation (canonical; decoder re-derives the same widths).
        BitAllocation(enc, scale_factors, bits);

        // 5a. Frame header.  In the FRAME HEADER allocation_method is 0=Loudness,
        //     1=SNR -- the OPPOSITE polarity of the codec-capability bit.  Since
        //     ALLOC_LOUDNESS==1 internally, emit (Loudness ? 0 : 1).
        out[0] = 0x9C;
        uint8_t blkCode = (enc->Blocks == 4 ? 0 : enc->Blocks == 8 ? 1 : enc->Blocks == 12 ? 2 : 3);
        out[1] = (uint8_t)((enc->Frequency << 6) | (blkCode << 4)
               | (enc->ChannelMode << 2)
               | ((enc->AllocMethod == ALLOC_LOUDNESS ? 0u : 1u) << 1)
               | (enc->Subbands == 8 ? 1u : 0u));
        out[2] = enc->Bitpool;
        // out[3] = CRC, filled after the join + scale-factor bits are packed.

        BitWriter bw = {out, 32};   // packing starts at byte 4 (after 4-byte header)

        // 5b. Join flags (joint stereo only).
        if (enc->ChannelMode == MODE_JOINT_STEREO) {
            WriteBits(&bw, joint, enc->Subbands);
        }

        // 5c. Scale factors, 4 bits each (ch0 all subbands, then ch1).
        for (int ch = 0; ch < enc->Channels; ch++) {
            for (int sb = 0; sb < enc->Subbands; sb++) {
                uint32_t sf = (uint32_t)scale_factors[ch][sb];
                if (sf > 15) sf = 15;
                WriteBits(&bw, sf, 4);
            }
        }

        // 5d. CRC-8 over the LOGICAL stream: byte1, byte2, then the join +
        //     scale-factor bits.  Those bits physically start at out[4] (out[3]
        //     is the CRC slot itself), so build a contiguous buffer that SKIPS
        //     out[3].  Computing over &out[1] as one span (the old bug) folds the
        //     zero CRC byte into the CRC and a compliant sink drops every frame.
        uint32_t sfBits = (enc->ChannelMode == MODE_JOINT_STEREO ? (uint32_t)enc->Subbands : 0u)
                        + (uint32_t)enc->Channels * enc->Subbands * 4;
        uint32_t sfBytes = (sfBits + 7) / 8;
        uint8_t crcbuf[2 + (SBC_CHANNELS * SBC_SUBBANDS * 4 + SBC_SUBBANDS + 7) / 8];
        crcbuf[0] = out[1];
        crcbuf[1] = out[2];
        for (uint32_t i = 0; i < sfBytes; i++) crcbuf[2 + i] = out[4 + i];
        uint32_t crcTotalBits = 16 + sfBits;
        out[3] = SbcCrc8(crcbuf, 2 + sfBytes, (crcTotalBits % 8) ? (crcTotalBits % 8) : 8);

        // 5e. Quantize + pack audio samples (block-major, then channel, subband).
        for (int blk = 0; blk < enc->Blocks; blk++) {
            for (int ch = 0; ch < enc->Channels; ch++) {
                for (int sb = 0; sb < enc->Subbands; sb++) {
                    if (bits[ch][sb] == 0) continue;
                    int32_t sf = scale_factors[ch][sb];
                    int32_t sample = sb_samples[ch][blk][sb];
                    uint32_t levels = (1u << bits[ch][sb]) - 1;
                    // q = levels * (sample + range) / (2 * range), truncating,
                    // with range = 2^(sf + FRAC_BITS) -- same mapping as the
                    // reference encoder.  The fractional sample bits feed
                    // straight into the quantizer's precision.
                    int32_t range = 1 << (sf + FRAC_BITS);
                    int32_t quantized = (int32_t)
                        (((int64_t)(sample + range) * levels) >> (sf + FRAC_BITS + 1));
                    if (quantized < 0) quantized = 0;
                    if (quantized > (int32_t)levels) quantized = (int32_t)levels;
                    WriteBits(&bw, (uint32_t)quantized, bits[ch][sb]);
                }
            }
        }

        // SBC frames are fixed-size for a given configuration.
        return enc->FrameSize;
    }

    // =========================================================================
    // Queries
    // =========================================================================

    uint32_t GetFrameSize(const SbcEncoder* enc) {
        return enc->FrameSize;
    }

    uint32_t GetSamplesPerFrame(const SbcEncoder* enc) {
        return enc->SamplesPerFrame;
    }

}
