xref: /btstack/3rd-party/lc3-google/src/sns.c (revision 6897da5c53aac5b1f90f41b5b15d0bd43d61dfff)

/******************************************************************************
 *
 *  Copyright 2022 Google LLC
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at:
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 ******************************************************************************/

#include "sns.h"
#include "tables.h"


/* ----------------------------------------------------------------------------
 *  DCT-16
 * -------------------------------------------------------------------------- */

/**
 * Matrix of DCT-16 coefficients
 *
 * M[n][k] = 2f cos( Pi k (2n + 1) / 2N )
 *
 *   k = [0..N-1], n = [0..N-1], N = 16
 *   f = sqrt(1/4N) for k=0, sqrt(1/2N) otherwise
 */
static const float dct16_m[16][16] = {

    {  2.50000000e-01,  3.51850934e-01,  3.46759961e-01,  3.38329500e-01,
       3.26640741e-01,  3.11806253e-01,  2.93968901e-01,  2.73300467e-01,
       2.50000000e-01,  2.24291897e-01,  1.96423740e-01,  1.66663915e-01,
       1.35299025e-01,  1.02631132e-01,  6.89748448e-02,  3.46542923e-02 },

    {  2.50000000e-01,  3.38329500e-01,  2.93968901e-01,  2.24291897e-01,
       1.35299025e-01,  3.46542923e-02, -6.89748448e-02, -1.66663915e-01,
      -2.50000000e-01, -3.11806253e-01, -3.46759961e-01, -3.51850934e-01,
      -3.26640741e-01, -2.73300467e-01, -1.96423740e-01, -1.02631132e-01 },

     { 2.50000000e-01,  3.11806253e-01,  1.96423740e-01,  3.46542923e-02,
      -1.35299025e-01, -2.73300467e-01, -3.46759961e-01, -3.38329500e-01,
      -2.50000000e-01, -1.02631132e-01,  6.89748448e-02,  2.24291897e-01,
       3.26640741e-01,  3.51850934e-01,  2.93968901e-01,  1.66663915e-01 },

     { 2.50000000e-01,  2.73300467e-01,  6.89748448e-02, -1.66663915e-01,
      -3.26640741e-01, -3.38329500e-01, -1.96423740e-01,  3.46542923e-02,
       2.50000000e-01,  3.51850934e-01,  2.93968901e-01,  1.02631132e-01,
      -1.35299025e-01, -3.11806253e-01, -3.46759961e-01, -2.24291897e-01 },

    {  2.50000000e-01,  2.24291897e-01, -6.89748448e-02, -3.11806253e-01,
      -3.26640741e-01, -1.02631132e-01,  1.96423740e-01,  3.51850934e-01,
       2.50000000e-01, -3.46542923e-02, -2.93968901e-01, -3.38329500e-01,
      -1.35299025e-01,  1.66663915e-01,  3.46759961e-01,  2.73300467e-01 },

    {  2.50000000e-01,  1.66663915e-01, -1.96423740e-01, -3.51850934e-01,
      -1.35299025e-01,  2.24291897e-01,  3.46759961e-01,  1.02631132e-01,
      -2.50000000e-01, -3.38329500e-01, -6.89748448e-02,  2.73300467e-01,
       3.26640741e-01,  3.46542923e-02, -2.93968901e-01, -3.11806253e-01 },

    {  2.50000000e-01,  1.02631132e-01, -2.93968901e-01, -2.73300467e-01,
       1.35299025e-01,  3.51850934e-01,  6.89748448e-02, -3.11806253e-01,
      -2.50000000e-01,  1.66663915e-01,  3.46759961e-01,  3.46542923e-02,
      -3.26640741e-01, -2.24291897e-01,  1.96423740e-01,  3.38329500e-01 },

    {  2.50000000e-01,  3.46542923e-02, -3.46759961e-01, -1.02631132e-01,
       3.26640741e-01,  1.66663915e-01, -2.93968901e-01, -2.24291897e-01,
       2.50000000e-01,  2.73300467e-01, -1.96423740e-01, -3.11806253e-01,
       1.35299025e-01,  3.38329500e-01, -6.89748448e-02, -3.51850934e-01 },

    {  2.50000000e-01, -3.46542923e-02, -3.46759961e-01,  1.02631132e-01,
       3.26640741e-01, -1.66663915e-01, -2.93968901e-01,  2.24291897e-01,
       2.50000000e-01, -2.73300467e-01, -1.96423740e-01,  3.11806253e-01,
       1.35299025e-01, -3.38329500e-01, -6.89748448e-02,  3.51850934e-01 },

    {  2.50000000e-01, -1.02631132e-01, -2.93968901e-01,  2.73300467e-01,
       1.35299025e-01, -3.51850934e-01,  6.89748448e-02,  3.11806253e-01,
      -2.50000000e-01, -1.66663915e-01,  3.46759961e-01, -3.46542923e-02,
      -3.26640741e-01,  2.24291897e-01,  1.96423740e-01, -3.38329500e-01 },

    {  2.50000000e-01, -1.66663915e-01, -1.96423740e-01,  3.51850934e-01,
      -1.35299025e-01, -2.24291897e-01,  3.46759961e-01, -1.02631132e-01,
      -2.50000000e-01,  3.38329500e-01, -6.89748448e-02, -2.73300467e-01,
       3.26640741e-01, -3.46542923e-02, -2.93968901e-01,  3.11806253e-01 },

    {  2.50000000e-01, -2.24291897e-01, -6.89748448e-02,  3.11806253e-01,
      -3.26640741e-01,  1.02631132e-01,  1.96423740e-01, -3.51850934e-01,
       2.50000000e-01,  3.46542923e-02, -2.93968901e-01,  3.38329500e-01,
      -1.35299025e-01, -1.66663915e-01,  3.46759961e-01, -2.73300467e-01 },

    {  2.50000000e-01, -2.73300467e-01,  6.89748448e-02,  1.66663915e-01,
      -3.26640741e-01,  3.38329500e-01, -1.96423740e-01, -3.46542923e-02,
       2.50000000e-01, -3.51850934e-01,  2.93968901e-01, -1.02631132e-01,
      -1.35299025e-01,  3.11806253e-01, -3.46759961e-01,  2.24291897e-01 },

    {  2.50000000e-01, -3.11806253e-01,  1.96423740e-01, -3.46542923e-02,
      -1.35299025e-01,  2.73300467e-01, -3.46759961e-01,  3.38329500e-01,
      -2.50000000e-01,  1.02631132e-01,  6.89748448e-02, -2.24291897e-01,
       3.26640741e-01, -3.51850934e-01,  2.93968901e-01, -1.66663915e-01 },

    {  2.50000000e-01, -3.38329500e-01,  2.93968901e-01, -2.24291897e-01,
       1.35299025e-01, -3.46542923e-02, -6.89748448e-02,  1.66663915e-01,
      -2.50000000e-01,  3.11806253e-01, -3.46759961e-01,  3.51850934e-01,
      -3.26640741e-01,  2.73300467e-01, -1.96423740e-01,  1.02631132e-01 },

    {  2.50000000e-01, -3.51850934e-01,  3.46759961e-01, -3.38329500e-01,
       3.26640741e-01, -3.11806253e-01,  2.93968901e-01, -2.73300467e-01,
       2.50000000e-01, -2.24291897e-01,  1.96423740e-01, -1.66663915e-01,
       1.35299025e-01, -1.02631132e-01,  6.89748448e-02, -3.46542923e-02 },

};

/**
 * Forward DCT-16 transformation
 * x, y            Input and output 16 values
 */
LC3_HOT static void dct16_forward(const float *x, float *y)
{
    for (int i = 0, j; i < 16; i++)
        for (y[i] = 0, j = 0; j < 16; j++)
            y[i] += x[j] * dct16_m[j][i];
}

/**
 * Inverse DCT-16 transformation
 * x, y            Input and output 16 values
 */
LC3_HOT static void dct16_inverse(const float *x, float *y)
{
    for (int i = 0, j; i < 16; i++)
        for (y[i] = 0, j = 0; j < 16; j++)
            y[i] += x[j] * dct16_m[i][j];
}


/* ----------------------------------------------------------------------------
 *  Scale factors
 * -------------------------------------------------------------------------- */

/**
 * Scale factors
 * dt, sr          Duration and samplerate of the frame
 * nbytes          Size in bytes of the frame
 * eb              Energy estimation per bands
 * att             1: Attack detected  0: Otherwise
 * scf             Output 16 scale factors
 */
LC3_HOT static void compute_scale_factors(
    enum lc3_dt dt, enum lc3_srate sr, int nbytes,
    const float *eb, bool att, float *scf)
{
    /* Pre-emphasis gain table :
     * Ge[b] = 10 ^ (b * g_tilt) / 630 , b = [0..63] */

    static const float ge_14[LC3_MAX_BANDS] = { /* g_tilt = 14 */
        1.00000000e+00, 1.05250029e+00, 1.10775685e+00, 1.16591440e+00,
        1.22712524e+00, 1.29154967e+00, 1.35935639e+00, 1.43072299e+00,
        1.50583635e+00, 1.58489319e+00, 1.66810054e+00, 1.75567629e+00,
        1.84784980e+00, 1.94486244e+00, 2.04696827e+00, 2.15443469e+00,
        2.26754313e+00, 2.38658979e+00, 2.51188643e+00, 2.64376119e+00,
        2.78255940e+00, 2.92864456e+00, 3.08239924e+00, 3.24422608e+00,
        3.41454887e+00, 3.59381366e+00, 3.78248991e+00, 3.98107171e+00,
        4.19007911e+00, 4.41005945e+00, 4.64158883e+00, 4.88527357e+00,
        5.14175183e+00, 5.41169527e+00, 5.69581081e+00, 5.99484250e+00,
        6.30957344e+00, 6.64082785e+00, 6.98947321e+00, 7.35642254e+00,
        7.74263683e+00, 8.14912747e+00, 8.57695899e+00, 9.02725178e+00,
        9.50118507e+00, 1.00000000e+01, 1.05250029e+01, 1.10775685e+01,
        1.16591440e+01, 1.22712524e+01, 1.29154967e+01, 1.35935639e+01,
        1.43072299e+01, 1.50583635e+01, 1.58489319e+01, 1.66810054e+01,
        1.75567629e+01, 1.84784980e+01, 1.94486244e+01, 2.04696827e+01,
        2.15443469e+01, 2.26754313e+01, 2.38658979e+01, 2.51188643e+01 };

    static const float ge_18[LC3_MAX_BANDS] = { /* g_tilt = 18 */
        1.00000000e+00, 1.06800043e+00, 1.14062492e+00, 1.21818791e+00,
        1.30102522e+00, 1.38949549e+00, 1.48398179e+00, 1.58489319e+00,
        1.69266662e+00, 1.80776868e+00, 1.93069773e+00, 2.06198601e+00,
        2.20220195e+00, 2.35195264e+00, 2.51188643e+00, 2.68269580e+00,
        2.86512027e+00, 3.05994969e+00, 3.26802759e+00, 3.49025488e+00,
        3.72759372e+00, 3.98107171e+00, 4.25178630e+00, 4.54090961e+00,
        4.84969343e+00, 5.17947468e+00, 5.53168120e+00, 5.90783791e+00,
        6.30957344e+00, 6.73862717e+00, 7.19685673e+00, 7.68624610e+00,
        8.20891416e+00, 8.76712387e+00, 9.36329209e+00, 1.00000000e+01,
        1.06800043e+01, 1.14062492e+01, 1.21818791e+01, 1.30102522e+01,
        1.38949549e+01, 1.48398179e+01, 1.58489319e+01, 1.69266662e+01,
        1.80776868e+01, 1.93069773e+01, 2.06198601e+01, 2.20220195e+01,
        2.35195264e+01, 2.51188643e+01, 2.68269580e+01, 2.86512027e+01,
        3.05994969e+01, 3.26802759e+01, 3.49025488e+01, 3.72759372e+01,
        3.98107171e+01, 4.25178630e+01, 4.54090961e+01, 4.84969343e+01,
        5.17947468e+01, 5.53168120e+01, 5.90783791e+01, 6.30957344e+01 };

    static const float ge_22[LC3_MAX_BANDS] = { /* g_tilt = 22 */
        1.00000000e+00, 1.08372885e+00, 1.17446822e+00, 1.27280509e+00,
        1.37937560e+00, 1.49486913e+00, 1.62003281e+00, 1.75567629e+00,
        1.90267705e+00, 2.06198601e+00, 2.23463373e+00, 2.42173704e+00,
        2.62450630e+00, 2.84425319e+00, 3.08239924e+00, 3.34048498e+00,
        3.62017995e+00, 3.92329345e+00, 4.25178630e+00, 4.60778348e+00,
        4.99358789e+00, 5.41169527e+00, 5.86481029e+00, 6.35586411e+00,
        6.88803330e+00, 7.46476041e+00, 8.08977621e+00, 8.76712387e+00,
        9.50118507e+00, 1.02967084e+01, 1.11588399e+01, 1.20931568e+01,
        1.31057029e+01, 1.42030283e+01, 1.53922315e+01, 1.66810054e+01,
        1.80776868e+01, 1.95913107e+01, 2.12316686e+01, 2.30093718e+01,
        2.49359200e+01, 2.70237760e+01, 2.92864456e+01, 3.17385661e+01,
        3.43959997e+01, 3.72759372e+01, 4.03970086e+01, 4.37794036e+01,
        4.74450028e+01, 5.14175183e+01, 5.57226480e+01, 6.03882412e+01,
        6.54444792e+01, 7.09240702e+01, 7.68624610e+01, 8.32980665e+01,
        9.02725178e+01, 9.78309319e+01, 1.06022203e+02, 1.14899320e+02,
        1.24519708e+02, 1.34945600e+02, 1.46244440e+02, 1.58489319e+02 };

    static const float ge_26[LC3_MAX_BANDS] = { /* g_tilt = 26 */
        1.00000000e+00, 1.09968890e+00, 1.20931568e+00, 1.32987103e+00,
        1.46244440e+00, 1.60823388e+00, 1.76855694e+00, 1.94486244e+00,
        2.13874364e+00, 2.35195264e+00, 2.58641621e+00, 2.84425319e+00,
        3.12779366e+00, 3.43959997e+00, 3.78248991e+00, 4.15956216e+00,
        4.57422434e+00, 5.03022373e+00, 5.53168120e+00, 6.08312841e+00,
        6.68954879e+00, 7.35642254e+00, 8.08977621e+00, 8.89623710e+00,
        9.78309319e+00, 1.07583590e+01, 1.18308480e+01, 1.30102522e+01,
        1.43072299e+01, 1.57335019e+01, 1.73019574e+01, 1.90267705e+01,
        2.09235283e+01, 2.30093718e+01, 2.53031508e+01, 2.78255940e+01,
        3.05994969e+01, 3.36499270e+01, 3.70044512e+01, 4.06933843e+01,
        4.47500630e+01, 4.92111475e+01, 5.41169527e+01, 5.95118121e+01,
        6.54444792e+01, 7.19685673e+01, 7.91430346e+01, 8.70327166e+01,
        9.57089124e+01, 1.05250029e+02, 1.15742288e+02, 1.27280509e+02,
        1.39968963e+02, 1.53922315e+02, 1.69266662e+02, 1.86140669e+02,
        2.04696827e+02, 2.25102829e+02, 2.47543082e+02, 2.72220379e+02,
        2.99357729e+02, 3.29200372e+02, 3.62017995e+02, 3.98107171e+02 };

    static const float ge_30[LC3_MAX_BANDS] = { /* g_tilt = 30 */
        1.00000000e+00, 1.11588399e+00, 1.24519708e+00, 1.38949549e+00,
        1.55051578e+00, 1.73019574e+00, 1.93069773e+00, 2.15443469e+00,
        2.40409918e+00, 2.68269580e+00, 2.99357729e+00, 3.34048498e+00,
        3.72759372e+00, 4.15956216e+00, 4.64158883e+00, 5.17947468e+00,
        5.77969288e+00, 6.44946677e+00, 7.19685673e+00, 8.03085722e+00,
        8.96150502e+00, 1.00000000e+01, 1.11588399e+01, 1.24519708e+01,
        1.38949549e+01, 1.55051578e+01, 1.73019574e+01, 1.93069773e+01,
        2.15443469e+01, 2.40409918e+01, 2.68269580e+01, 2.99357729e+01,
        3.34048498e+01, 3.72759372e+01, 4.15956216e+01, 4.64158883e+01,
        5.17947468e+01, 5.77969288e+01, 6.44946677e+01, 7.19685673e+01,
        8.03085722e+01, 8.96150502e+01, 1.00000000e+02, 1.11588399e+02,
        1.24519708e+02, 1.38949549e+02, 1.55051578e+02, 1.73019574e+02,
        1.93069773e+02, 2.15443469e+02, 2.40409918e+02, 2.68269580e+02,
        2.99357729e+02, 3.34048498e+02, 3.72759372e+02, 4.15956216e+02,
        4.64158883e+02, 5.17947468e+02, 5.77969288e+02, 6.44946677e+02,
        7.19685673e+02, 8.03085722e+02, 8.96150502e+02, 1.00000000e+03 };

#if LC3_PLUS_HR

    static const float ge_34[LC3_MAX_BANDS] = { /* g_tilt = 34 */
        1.00000000e+00, 1.13231759e+00, 1.28214312e+00, 1.45179321e+00,
        1.64389099e+00, 1.86140669e+00, 2.10770353e+00, 2.38658979e+00,
        2.70237760e+00, 3.05994969e+00, 3.46483486e+00, 3.92329345e+00,
        4.44241419e+00, 5.03022373e+00, 5.69581081e+00, 6.44946677e+00,
        7.30284467e+00, 8.26913948e+00, 9.36329209e+00, 1.06022203e+01,
        1.20050806e+01, 1.35935639e+01, 1.53922315e+01, 1.74288945e+01,
        1.97350438e+01, 2.23463373e+01, 2.53031508e+01, 2.86512027e+01,
        3.24422608e+01, 3.67349426e+01, 4.15956216e+01, 4.70994540e+01,
        5.33315403e+01, 6.03882412e+01, 6.83786677e+01, 7.74263683e+01,
        8.76712387e+01, 9.92716858e+01, 1.12407076e+02, 1.27280509e+02,
        1.44121960e+02, 1.63191830e+02, 1.84784980e+02, 2.09235283e+02,
        2.36920791e+02, 2.68269580e+02, 3.03766364e+02, 3.43959997e+02,
        3.89471955e+02, 4.41005945e+02, 4.99358789e+02, 5.65432741e+02,
        6.40249439e+02, 7.24965701e+02, 8.20891416e+02, 9.29509790e+02,
        1.05250029e+03, 1.19176459e+03, 1.34945600e+03, 1.52801277e+03,
        1.73019574e+03, 1.95913107e+03, 2.21835857e+03, 2.51188643e+03 };

#endif /* LC3_PLUS_HR */

    static const float *ge_table[LC3_NUM_SRATE] = {
        [LC3_SRATE_8K    ] = ge_14, [LC3_SRATE_16K   ] = ge_18,
        [LC3_SRATE_24K   ] = ge_22, [LC3_SRATE_32K   ] = ge_26,
        [LC3_SRATE_48K   ] = ge_30,

#if LC3_PLUS_HR
        [LC3_SRATE_48K_HR] = ge_30, [LC3_SRATE_96K_HR] = ge_34,
#endif /* LC3_PLUS_HR */

    };

    float e[LC3_MAX_BANDS];

    /* --- Copy and padding --- */

    int nb = lc3_num_bands[dt][sr];
    int n4 = nb < 32 ? 32 % nb : 0;
    int n2 = nb < 32 ? nb - n4 : LC3_MAX_BANDS - nb;

    for (int i4 = 0; i4 < n4; i4++)
        e[4*i4 + 0] = e[4*i4 + 1] =
        e[4*i4 + 2] = e[4*i4 + 3] = eb[i4];

    for (int i2 = n4; i2 < n4+n2; i2++)
        e[2*(n4+i2) + 0] = e[2*(n4+i2) + 1] = eb[i2];

    memcpy(e + 4*n4 + 2*n2, eb + n4 + n2, (nb - n4 - n2) * sizeof(float));

    /* --- Smoothing, pre-emphasis and logarithm --- */

    const float *ge = ge_table[sr];

    float e0 = e[0], e1 = e[0], e2;
    float e_sum = 0;

    for (int i = 0; i < LC3_MAX_BANDS-1; ) {
        e[i] = (e0 * 0.25f + e1 * 0.5f + (e2 = e[i+1]) * 0.25f) * ge[i];
        e_sum += e[i++];

        e[i] = (e1 * 0.25f + e2 * 0.5f + (e0 = e[i+1]) * 0.25f) * ge[i];
        e_sum += e[i++];

        e[i] = (e2 * 0.25f + e0 * 0.5f + (e1 = e[i+1]) * 0.25f) * ge[i];
        e_sum += e[i++];
    }

    e[LC3_MAX_BANDS-1] = (e0 * 0.25f + e1 * 0.75f) * ge[LC3_MAX_BANDS-1];
    e_sum += e[LC3_MAX_BANDS-1];

    float noise_floor = fmaxf(e_sum * (1e-4f / 64), 0x1p-32f);

    for (int i = 0; i < LC3_MAX_BANDS; i++)
        e[i] = lc3_log2f(fmaxf(e[i], noise_floor)) * 0.5f;

    /* --- Grouping & scaling --- */

    float scf_sum;

    scf[0] = (e[0] + e[4]) * 1.f/12 +
             (e[0] + e[3]) * 2.f/12 +
             (e[1] + e[2]) * 3.f/12  ;
    scf_sum = scf[0];

    for (int i = 1; i < 15; i++) {
        scf[i] = (e[4*i-1] + e[4*i+4]) * 1.f/12 +
                 (e[4*i  ] + e[4*i+3]) * 2.f/12 +
                 (e[4*i+1] + e[4*i+2]) * 3.f/12  ;
        scf_sum += scf[i];
    }

    scf[15] = (e[59] + e[63]) * 1.f/12 +
              (e[60] + e[63]) * 2.f/12 +
              (e[61] + e[62]) * 3.f/12  ;
    scf_sum += scf[15];

    float cf = lc3_hr(sr) ? 0.6f : 0.85f;
    if (lc3_hr(sr) && 8 * nbytes >
            (dt < LC3_DT_10M ? 1150 * (int)(1 + dt) : 4400))
        cf *= dt < LC3_DT_10M ? 0.25f : 0.35f;

    for (int i = 0; i < 16; i++)
        scf[i] = cf * (scf[i] - scf_sum * 1.f/16);

    /* --- Attack handling --- */

    if (!att)
        return;

    float s0, s1 = scf[0], s2 = scf[1], s3 = scf[2], s4 = scf[3];
    float sn = s1 + s2;

    scf[0] = (sn += s3) * 1.f/3;
    scf[1] = (sn += s4) * 1.f/4;
    scf_sum = scf[0] + scf[1];

    for (int i = 2; i < 14; i++, sn -= s0) {
        s0 = s1, s1 = s2, s2 = s3, s3 = s4, s4 = scf[i+2];
        scf[i] = (sn += s4) * 1.f/5;
        scf_sum += scf[i];
    }

    scf[14] = (sn      ) * 1.f/4;
    scf[15] = (sn -= s1) * 1.f/3;
    scf_sum += scf[14] + scf[15];

    for (int i = 0; i < 16; i++)
        scf[i] = (dt == LC3_DT_7M5 ? 0.3f : 0.5f) *
                 (scf[i] - scf_sum * 1.f/16);
}

/**
 * Codebooks
 * scf             Input 16 scale factors
 * lf/hfcb_idx     Output the low and high frequency codebooks index
 */
LC3_HOT static void resolve_codebooks(
    const float *scf, int *lfcb_idx, int *hfcb_idx)
{
    float dlfcb_max = 0, dhfcb_max = 0;
    *lfcb_idx = *hfcb_idx = 0;

    for (int icb = 0; icb < 32; icb++) {
        const float *lfcb = lc3_sns_lfcb[icb];
        const float *hfcb = lc3_sns_hfcb[icb];
        float dlfcb = 0, dhfcb = 0;

        for (int i = 0; i < 8; i++) {
            dlfcb += (scf[  i] - lfcb[i]) * (scf[  i] - lfcb[i]);
            dhfcb += (scf[8+i] - hfcb[i]) * (scf[8+i] - hfcb[i]);
        }

        if (icb == 0 || dlfcb < dlfcb_max)
            *lfcb_idx = icb, dlfcb_max = dlfcb;

        if (icb == 0 || dhfcb < dhfcb_max)
            *hfcb_idx = icb, dhfcb_max = dhfcb;
    }
}

/**
 * Unit energy normalize pulse configuration
 * c               Pulse configuration
 * cn              Normalized pulse configuration
 */
LC3_HOT static void normalize(const int *c, float *cn)
{
    int c2_sum = 0;
    for (int i = 0; i < 16; i++)
        c2_sum += c[i] * c[i];

    float c_norm = 1.f / sqrtf(c2_sum);

    for (int i = 0; i < 16; i++)
        cn[i] = c[i] * c_norm;
}

/**
 * Sub-procedure of `quantize()`, add unit pulse
 * x, y, n         Transformed residual, and vector of pulses with length
 * start, end      Current number of pulses, limit to reach
 * corr, energy    Correlation (x,y) and y energy, updated at output
 */
LC3_HOT static void add_pulse(const float *x, int *y, int n,
    int start, int end, float *corr, float *energy)
{
    for (int k = start; k < end; k++) {
        float best_c2 = (*corr + x[0]) * (*corr + x[0]);
        float best_e = *energy + 2*y[0] + 1;
        int nbest = 0;

        for (int i = 1; i < n; i++) {
            float c2 = (*corr + x[i]) * (*corr + x[i]);
            float e  = *energy + 2*y[i] + 1;

            if (c2 * best_e > e * best_c2)
                best_c2 = c2, best_e = e, nbest = i;
        }

        *corr += x[nbest];
        *energy += 2*y[nbest] + 1;
        y[nbest]++;
    }
}

/**
 * Quantization of codebooks residual
 * scf             Input 16 scale factors, output quantized version
 * lf/hfcb_idx     Codebooks index
 * c, cn           Output 4 pulse configurations candidates, normalized
 * shape/gain_idx  Output selected shape/gain indexes
 */
LC3_HOT static void quantize(const float *scf, int lfcb_idx, int hfcb_idx,
    int (*c)[16], float (*cn)[16], int *shape_idx, int *gain_idx)
{
    /* --- Residual --- */

    const float *lfcb = lc3_sns_lfcb[lfcb_idx];
    const float *hfcb = lc3_sns_hfcb[hfcb_idx];
    float r[16], x[16];

    for (int i = 0; i < 8; i++) {
        r[  i] = scf[  i] - lfcb[i];
        r[8+i] = scf[8+i] - hfcb[i];
    }

    dct16_forward(r, x);

    /* --- Shape 3 candidate ---
     * Project to or below pyramid N = 16, K = 6,
     * then add unit pulses until you reach K = 6, over N = 16 */

    float xm[16];
    float xm_sum = 0;

    for (int i = 0; i < 16; i++) {
        xm[i] = fabsf(x[i]);
        xm_sum += xm[i];
    }

    float proj_factor = (6 - 1) / fmaxf(xm_sum, 1e-31f);
    float corr = 0, energy = 0;
    int npulses = 0;

    for (int i = 0; i < 16; i++) {
        c[3][i] = floorf(xm[i] * proj_factor);
        npulses += c[3][i];
        corr    += c[3][i] * xm[i];
        energy  += c[3][i] * c[3][i];
    }

    add_pulse(xm, c[3], 16, npulses, 6, &corr, &energy);
    npulses = 6;

    /* --- Shape 2 candidate ---
     * Add unit pulses until you reach K = 8 on shape 3 */

    memcpy(c[2], c[3], sizeof(c[2]));

    add_pulse(xm, c[2], 16, npulses, 8, &corr, &energy);
    npulses = 8;

    /* --- Shape 1 candidate ---
     * Remove any unit pulses from shape 2 that are not part of 0 to 9
     * Update energy and correlation terms accordingly
     * Add unit pulses until you reach K = 10, over N = 10 */

    memcpy(c[1], c[2], sizeof(c[1]));

    for (int i = 10; i < 16; i++) {
        c[1][i] = 0;
        npulses -= c[2][i];
        corr    -= c[2][i] * xm[i];
        energy  -= c[2][i] * c[2][i];
    }

    add_pulse(xm, c[1], 10, npulses, 10, &corr, &energy);
    npulses = 10;

    /* --- Shape 0 candidate ---
     * Add unit pulses until you reach K = 1, on shape 1 */

    memcpy(c[0], c[1], sizeof(c[0]));

    add_pulse(xm + 10, c[0] + 10, 6, 0, 1, &corr, &energy);

    /* --- Add sign and unit energy normalize --- */

    for (int j = 0; j < 16; j++)
        for (int i = 0; i < 4; i++)
            c[i][j] = x[j] < 0 ? -c[i][j] : c[i][j];

    for (int i = 0; i < 4; i++)
        normalize(c[i], cn[i]);

    /* --- Determe shape & gain index ---
     * Search the Mean Square Error, within (shape, gain) combinations */

    float mse_min = FLT_MAX;
    *shape_idx = *gain_idx = 0;

    for (int ic = 0; ic < 4; ic++) {
        const struct lc3_sns_vq_gains *cgains = lc3_sns_vq_gains + ic;
        float cmse_min = FLT_MAX;
        int cgain_idx = 0;

        for (int ig = 0; ig < cgains->count; ig++) {
            float g = cgains->v[ig];

            float mse = 0;
            for (int i = 0; i < 16; i++)
                mse += (x[i] - g * cn[ic][i]) * (x[i] - g * cn[ic][i]);

            if (mse < cmse_min) {
                cgain_idx = ig,
                cmse_min = mse;
            }
        }

        if (cmse_min < mse_min) {
            *shape_idx = ic, *gain_idx = cgain_idx;
            mse_min = cmse_min;
        }
    }
}

/**
 * Unquantization of codebooks residual
 * lf/hfcb_idx     Low and high frequency codebooks index
 * c               Table of normalized pulse configuration
 * shape/gain      Selected shape/gain indexes
 * scf             Return unquantized scale factors
 */
LC3_HOT static void unquantize(int lfcb_idx, int hfcb_idx,
    const float *c, int shape, int gain, float *scf)
{
    const float *lfcb = lc3_sns_lfcb[lfcb_idx];
    const float *hfcb = lc3_sns_hfcb[hfcb_idx];
    float g = lc3_sns_vq_gains[shape].v[gain];

    dct16_inverse(c, scf);

    for (int i = 0; i < 8; i++)
        scf[i] = lfcb[i] + g * scf[i];

    for (int i = 8; i < 16; i++)
        scf[i] = hfcb[i-8] + g * scf[i];
}

/**
 * Sub-procedure of `sns_enumerate()`, enumeration of a vector
 * c, n            Table of pulse configuration, and length
 * idx, ls         Return enumeration set
 */
static void enum_mvpq(const int *c, int n, int *idx, bool *ls)
{
    int ci, i;

    /* --- Scan for 1st significant coeff --- */

    for (i = 0, c += n; (ci = *(--c)) == 0 && i < 15; i++);

    *idx = 0;
    *ls = ci < 0;

    /* --- Scan remaining coefficients --- */

    unsigned j = LC3_ABS(ci);

    for (i++; i < n; i++, j += LC3_ABS(ci)) {

        if ((ci = *(--c)) != 0) {
            *idx = (*idx << 1) | *ls;
            *ls = ci < 0;
        }

        *idx += lc3_sns_mpvq_offsets[i][LC3_MIN(j, 10)];
    }
}

/**
 * Sub-procedure of `sns_deenumerate()`, deenumeration of a vector
 * idx, ls         Enumeration set
 * npulses         Number of pulses in the set
 * c, n            Table of pulses configuration, and length
 */
static void deenum_mvpq(int idx, bool ls, int npulses, int *c, int n)
{
    int i;

    /* --- Scan for coefficients --- */

    for (i = n-1; i >= 0 && idx; i--) {

        int ci = 0;

        for (ci = 0; idx < lc3_sns_mpvq_offsets[i][npulses - ci]; ci++);
        idx -= lc3_sns_mpvq_offsets[i][npulses - ci];

        *(c++) = ls ? -ci : ci;
        npulses -= ci;
        if (ci > 0) {
            ls = idx & 1;
            idx >>= 1;
        }
    }

    /* --- Set last significant --- */

    int ci = npulses;

    if (i-- >= 0)
        *(c++) = ls ? -ci : ci;

    while (i-- >= 0)
        *(c++) = 0;
}

/**
 * SNS Enumeration of PVQ configuration
 * shape           Selected shape index
 * c               Selected pulse configuration
 * idx_a, ls_a     Return enumeration set A
 * idx_b, ls_b     Return enumeration set B (shape = 0)
 */
static void enumerate(int shape, const int *c,
    int *idx_a, bool *ls_a, int *idx_b, bool *ls_b)
{
    enum_mvpq(c, shape < 2 ? 10 : 16, idx_a, ls_a);

    if (shape == 0)
        enum_mvpq(c + 10, 6, idx_b, ls_b);
}

/**
 * SNS Deenumeration of PVQ configuration
 * shape           Selected shape index
 * idx_a, ls_a     enumeration set A
 * idx_b, ls_b     enumeration set B (shape = 0)
 * c               Return pulse configuration
 */
static void deenumerate(int shape,
    int idx_a, bool ls_a, int idx_b, bool ls_b, int *c)
{
    int npulses_a = (const int []){ 10, 10, 8, 6 }[shape];

    deenum_mvpq(idx_a, ls_a, npulses_a, c, shape < 2 ? 10 : 16);

    if (shape == 0)
        deenum_mvpq(idx_b, ls_b, 1, c + 10, 6);
    else if (shape == 1)
        memset(c + 10, 0, 6 * sizeof(*c));
}


/* ----------------------------------------------------------------------------
 *  Filtering
 * -------------------------------------------------------------------------- */

/**
 * Spectral shaping
 * dt, sr          Duration and samplerate of the frame
 * scf_q           Quantized scale factors
 * inv             True on inverse shaping, False otherwise
 * x               Spectral coefficients
 * y               Return shapped coefficients
 *
 * `x` and `y` can be the same buffer
 */
LC3_HOT static void spectral_shaping(enum lc3_dt dt, enum lc3_srate sr,
    const float *scf_q, bool inv, const float *x, float *y)
{
    /* --- Interpolate scale factors --- */

    float scf[LC3_MAX_BANDS];
    float s0, s1 = inv ? -scf_q[0] : scf_q[0];

    scf[0] = scf[1] = s1;
    for (int i = 0; i < 15; i++) {
        s0 = s1, s1 = inv ? -scf_q[i+1] : scf_q[i+1];
        scf[4*i+2] = s0 + 0.125f * (s1 - s0);
        scf[4*i+3] = s0 + 0.375f * (s1 - s0);
        scf[4*i+4] = s0 + 0.625f * (s1 - s0);
        scf[4*i+5] = s0 + 0.875f * (s1 - s0);
    }
    scf[62] = s1 + 0.125f * (s1 - s0);
    scf[63] = s1 + 0.375f * (s1 - s0);

    int nb = lc3_num_bands[dt][sr];
    int n4 = nb < 32 ? 32 % nb : 0;
    int n2 = nb < 32 ? nb - n4 : LC3_MAX_BANDS - nb;

    for (int i4 = 0; i4 < n4; i4++)
        scf[i4] = 0.25f * (scf[4*i4+0] + scf[4*i4+1] +
                           scf[4*i4+2] + scf[4*i4+3]);

    for (int i2 = n4; i2 < n4+n2; i2++)
        scf[i2] = 0.5f * (scf[2*(n4+i2)] + scf[2*(n4+i2)+1]);

    memmove(scf + n4 + n2, scf + 4*n4 + 2*n2, (nb - n4 - n2) * sizeof(float));

    /* --- Spectral shaping --- */

    const int *lim = lc3_band_lim[dt][sr];

    for (int i = 0, ib = 0; ib < nb; ib++) {
        float g_sns = lc3_exp2f(-scf[ib]);

        for ( ; i < lim[ib+1]; i++)
            y[i] = x[i] * g_sns;
    }
}


/* ----------------------------------------------------------------------------
 *  Interface
 * -------------------------------------------------------------------------- */

/**
 * SNS analysis
 */
void lc3_sns_analyze(
    enum lc3_dt dt, enum lc3_srate sr, int nbytes,
    const float *eb, bool att, struct lc3_sns_data *data,
    const float *x, float *y)
{
    /* Processing steps :
     * - Determine 16 scale factors from bands energy estimation
     * - Get codebooks indexes that match thoses scale factors
     * - Quantize the residual with the selected codebook
     * - The pulse configuration `c[]` is enumerated
     * - Finally shape the spectrum coefficients accordingly */

    float scf[16], cn[4][16];
    int c[4][16];

    compute_scale_factors(dt, sr, nbytes, eb, att, scf);

    resolve_codebooks(scf, &data->lfcb, &data->hfcb);

    quantize(scf, data->lfcb, data->hfcb,
        c, cn, &data->shape, &data->gain);

    unquantize(data->lfcb, data->hfcb,
        cn[data->shape], data->shape, data->gain, scf);

    enumerate(data->shape, c[data->shape],
        &data->idx_a, &data->ls_a, &data->idx_b, &data->ls_b);

    spectral_shaping(dt, sr, scf, false, x, y);
}

/**
 * SNS synthesis
 */
void lc3_sns_synthesize(
    enum lc3_dt dt, enum lc3_srate sr,
    const lc3_sns_data_t *data, const float *x, float *y)
{
    float scf[16], cn[16];
    int c[16];

    deenumerate(data->shape,
        data->idx_a, data->ls_a, data->idx_b, data->ls_b, c);

    normalize(c, cn);

    unquantize(data->lfcb, data->hfcb, cn, data->shape, data->gain, scf);

    spectral_shaping(dt, sr, scf, true, x, y);
}

/**
 * Return number of bits coding the bitstream data
 */
int lc3_sns_get_nbits(void)
{
    return 38;
}

/**
 * Put bitstream data
 */
void lc3_sns_put_data(lc3_bits_t *bits, const struct lc3_sns_data *data)
{
    /* --- Codebooks --- */

    lc3_put_bits(bits, data->lfcb, 5);
    lc3_put_bits(bits, data->hfcb, 5);

    /* --- Shape, gain and vectors --- *
     * Write MSB bit of shape index, next LSB bits of shape and gain,
     * and MVPQ vectors indexes are muxed */

    int shape_msb = data->shape >> 1;
    lc3_put_bit(bits, shape_msb);

    if (shape_msb == 0) {
        const int size_a = 2390004;
        int submode = data->shape & 1;

        int mux_high = submode == 0 ?
            2 * (data->idx_b + 1) + data->ls_b : data->gain & 1;
        int mux_code = mux_high * size_a + data->idx_a;

        lc3_put_bits(bits, data->gain >> submode, 1);
        lc3_put_bits(bits, data->ls_a, 1);
        lc3_put_bits(bits, mux_code, 25);

    } else {
        const int size_a = 15158272;
        int submode = data->shape & 1;

        int mux_code = submode == 0 ?
            data->idx_a : size_a + 2 * data->idx_a + (data->gain & 1);

        lc3_put_bits(bits, data->gain >> submode, 2);
        lc3_put_bits(bits, data->ls_a, 1);
        lc3_put_bits(bits, mux_code, 24);
    }
}

/**
 * Get bitstream data
 */
int lc3_sns_get_data(lc3_bits_t *bits, struct lc3_sns_data *data)
{
    /* --- Codebooks --- */

    *data = (struct lc3_sns_data){
        .lfcb = lc3_get_bits(bits, 5),
        .hfcb = lc3_get_bits(bits, 5)
    };

    /* --- Shape, gain and vectors --- */

    int shape_msb = lc3_get_bit(bits);
    data->gain = lc3_get_bits(bits, 1 + shape_msb);
    data->ls_a = lc3_get_bit(bits);

    int mux_code = lc3_get_bits(bits, 25 - shape_msb);

    if (shape_msb == 0) {
        const int size_a = 2390004;

        if (mux_code >= size_a * 14)
            return -1;

        data->idx_a = mux_code % size_a;
        mux_code = mux_code / size_a;

        data->shape = (mux_code < 2);

        if (data->shape == 0) {
            data->idx_b = (mux_code - 2) / 2;
            data->ls_b  = (mux_code - 2) % 2;
        } else {
            data->gain = (data->gain << 1) + (mux_code % 2);
        }

    } else {
        const int size_a = 15158272;

        if (mux_code >= size_a + 1549824)
            return -1;

        data->shape = 2 + (mux_code >= size_a);
        if (data->shape == 2) {
            data->idx_a = mux_code;
        } else {
            mux_code -= size_a;
            data->idx_a = mux_code / 2;
            data->gain = (data->gain << 1) + (mux_code % 2);
        }
    }

    return 0;
}