FEC_HQ_phase_ecu.c

/******************************************************************************************************

   (C) 2022 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
   Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
   Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
   Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
   contributors to this repository. All Rights Reserved.

   This software is protected by copyright law and by international treaties.
   The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
   Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
   Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
   Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
   contributors to this repository retain full ownership rights in their respective contributions in
   the software. This notice grants no license of any kind, including but not limited to patent
   license, nor is any license granted by implication, estoppel or otherwise.

   Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
   contributions.

   This software is provided "AS IS", without any express or implied warranties. The software is in the
   development stage. It is intended exclusively for experts who have experience with such software and
   solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
   and fitness for a particular purpose are hereby disclaimed and excluded.

   Any dispute, controversy or claim arising under or in relation to providing this software shall be
   submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
   accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
   the United Nations Convention on Contracts on the International Sales of Goods.

*******************************************************************************************************/

/*====================================================================================
    EVS Codec 3GPP TS26.443 Nov 04, 2021. Version 12.14.0 / 13.10.0 / 14.6.0 / 15.4.0 / 16.3.0
  ====================================================================================*/

#include <stdint.h>
#include "options.h"
#ifdef DEBUGGING
#include "debug.h"
#endif
#include <math.h>
#include "rom_dec.h"
#include "rom_com.h"
#include "cnst.h"
#include "prot.h"
#include "wmops.h"

/*---------------------------------------------------------------------*
 * Local constants
 *---------------------------------------------------------------------*/

#define FEC_MAX                512
#define FEC_NB_PULSE_MAX       20
#define FEC_FFT_MAX_SIZE       512
#define FEC_DCIM_FILT_SIZE_MAX 60

#define PHASE_DITH ( PI2 )

#define DELTA_CORR             6 /* Range for phase correction around peak */
#define THRESH_TR_dB           10.0f
#define THRESH_TR_LIN          (float) pow( 10.0f, THRESH_TR_dB / 10.0f )
#define THRESH_TR_LIN_INV      (float) pow( 10.0f, -THRESH_TR_dB / 10.0f )
#define MAX_INCREASE_GRPOW     0.0f /* maximum amplification in case of transients */
#define MAX_INCREASE_GRPOW_LIN (float) pow( 10.0f, MAX_INCREASE_GRPOW / 10.0f )

#define PHASE_DITH_SCALE (float) pow( 2.0, -16.0 ) /* for scaling random short values  to  +/- pi */

#define BURST_PHDITH_THRESH     ( 4 - 1 ) /* speech start phase dither with <burst_phdith_thresh> losses in a row */
#define BURST_PHDITH_RAMPUP_LEN 2         /* speech ramp up degree of phase dither over a length of <burst_phdith_rampup_len> frames */
#define BURST_ATT_THRESH        ( 3 - 1 ) /* speech start attenuate with <burst_att_thresh> losses in a row */
#define ATT_PER_FRAME           4         /* speech attenuation in dB */
#define BETA_MUTE_THR           10        /* time threshold to start beta-noise attenuation */
#define BETA_MUTE_FAC           0.5f      /* attenuation factor per additional bad frame */

#define LGW32k 7
#define LGW16k 6
#define LGW48k LGW32k + 1 /* Use the same frequency groups as for SWB + 1 */

#define L_TRANA_LOG32k 8
#define L_TRANA_LOG16k 7

#define DELTA_CORR_F0_INT 2         /* Constant controls the bin range where Jacobsen is used */
#define ST_PFIND_SENS     0.93f     /* peakfinder sensitivity */
#define L_PROT_NS         32000000L /* Prototype frame length in nanoseconds (32 ms) */
#define PH_ECU_CORR_LIMIT 0.85f     /* Correlation limit for IVAS Phase ECU activation */
#define PH_ECU_N_LIMIT    56        /* fec_alg analysis frame limit for IVAS Phase ECU activation */
#define PFIND_SENS        0.97f     /* peakfinder sensitivity */

/*---------------------------------------------------------------------*
 * Local functions
 *---------------------------------------------------------------------*/

static int16_t rand_phase( const int16_t seed, float *sin_F, float *cos_F );
static float imax2_jacobsen_mag( const float *y_re, const float *y_im );

/*-------------------------------------------------------------------*
 * mult_rev2()
 *
 * Multiplication of two vectors second vector is multiplied in reverse order
 *-------------------------------------------------------------------*/

static void mult_rev2(
    const float x1[], /* i  : Input vector 1                                   */
    const float x2[], /* i  : Input vector 2                                   */
    float y[],        /* o  : Output vector that contains vector 1 .* vector 2 */
    const int16_t N   /* i  : Vector length                                    */
)
{
    int16_t i, j;

    for ( i = 0, j = N - 1; i < N; i++, j-- )
    {
        y[i] = x1[i] * x2[j];
    }

    return;
}


/*-------------------------------------------------------------------*
 * fft_spec2()
 *
 * Square magnitude of fft spectrum
 *-------------------------------------------------------------------*/

static void fft_spec2(
    float x[],      /* i/o: Input vector: complex spectrum -> square magnitude spectrum  */
    const int16_t N /* i  : Vector length                                                */
)
{
    int16_t i, j;

    for ( i = 1, j = N - 1; i < N / 2; i++, j-- )
    {
        x[i] = x[i] * x[i] + x[j] * x[j];
    }

    x[0] *= x[0];
    x[N / 2] *= x[N / 2];

    return;
}

/*------------------------------------------------------------------*
 * rand_phase()
 *
 * randomized phase in form of sin and cos components
 *------------------------------------------------------------------*/

/*! r: Updated seed from RNG */
static int16_t rand_phase(
    const int16_t seed, /* i  : RNG seed                          */
    float *sin_F,       /* o  : random phase sin value            */
    float *cos_F        /* o  : random phase cos value            */
)
{
    const float *sincos = sincos_t_ext + 128;
    int16_t seed2 = seed;
    own_random( &seed2 );

    if ( seed2 & 0x40 )
    {
        *sin_F = sincos[seed2 >> 8];
    }
    else
    {
        *sin_F = -sincos[seed2 >> 8];
    }

    if ( seed2 & 0x80 )
    {
        *cos_F = sincos[-( seed2 >> 8 )];
    }
    else
    {
        *cos_F = -sincos[-( seed2 >> 8 )];
    }

    return seed2;
}

/*-----------------------------------------------------------------------------
 * imax2_jacobsen_mag()
 *
 * refine peak interpolation using jacobsen and periodic speca ana windows
 *----------------------------------------------------------------------------*/

/*! r: The location, relative to the middle of the 3 given data point, of the maximum. (Q15)*/
float imax2_jacobsen_mag(
    const float *y_re, /* i  : The 3 given data points. real part order -1 0 1                       */
    const float *y_im  /* i  : The 3 given data points. imag part order  1 0 -1 (from FFT)           */
)
{
    float posi;
    const float *pY;
    float y_m1_re, y_0_re, y_p1_re;
    float y_m1_im, y_0_im, y_p1_im;
    float N_re, N_im;
    float D_re, D_im;
    float numer, denom;

/* Jacobsen estimates peak offset relative y_0 using
 *                 X_m1 - X_p1
 *  d = REAL ( ------------------- ) * c_jacob
 *              2*X_0 - X_m1 -Xp1
 *
 *  Where c_jacob is a window  dependent constant
 */
#define C_JACOB 1.1453f /*  % assume 0.1875 hammrect window 'symmetric' */

    /* Get the bin parameters into variables */
    pY = y_re;
    y_m1_re = *pY++;
    y_0_re = *pY++;
    y_p1_re = *pY++;

    /* Same for imaginary parts - note reverse order from FFT */
    pY = y_im;
    y_p1_im = *pY++;
    y_0_im = *pY++;
    y_m1_im = *pY++;

    /* prepare numerator real and imaginary parts*/
    N_re = y_m1_re - y_p1_re;
    N_im = y_m1_im - y_p1_im;

    /* prepare denominator real and imaginary parts */

    D_re = 2 * y_0_re - y_m1_re - y_p1_re;
    D_im = 2 * y_0_im - y_m1_im - y_p1_im;

    /* REAL part of complex division  */
    numer = N_re * D_re + N_im * D_im;
    denom = D_re * D_re + D_im * D_im;

    test();
    if ( numer != 0 && denom != 0 )
    {
        posi = numer / denom * C_JACOB;
    }
    else
    {
        posi = 0; /* flat top,  division is not possible choose center freq */
    }

    return posi;
}


/*------------------------------------------------------------------*
 * trans_ana()
 *
 * Transient analysis
 *------------------------------------------------------------------*/

static void trans_ana(
    const float *xfp,            /* i  : Input signal                                         */
    float *mag_chg,              /* i/o: Magnitude modification                               */
    float *ph_dith,              /* i/o: Phase dither                                         */
    float *mag_chg_1st,          /* i/o: per band magnitude modifier for transients           */
    const int16_t output_frame,  /* i  : Frame length                                         */
    const int16_t time_offs,     /* i  : Time offset                                          */
    const float est_mus_content, /* i  : 0.0=speech_like ... 1.0=Music    (==st->env_stab )   */
    const int16_t last_fec,      /* i  : signal that previous frame was concealed with fec_alg*/
    float *alpha,                /* o  : Magnitude modification factors for fade to average   */
    float *beta,                 /* o  : Magnitude modification factors for fade to average   */
    float *beta_mute,            /* o  : Factor for long-term mute                            */
    float Xavg[LGW_MAX]          /* o  : Frequency group average gain to fade to              */
)
{
    const float *w_hamm;
    float grp_pow_chg, att_val, att_degree;
    float xfp_left[L_TRANA48k], xfp_right[L_TRANA48k];
    float gr_pow_left[LGW_MAX], gr_pow_right[LGW_MAX];
    const float *xfp_;
    int16_t Ltrana, Ltrana_2, Lprot, LtranaLog = 0, Lgw, k, burst_len;
    int16_t att_always[LGW_MAX]; /* fixed attenuation per frequency group if set to 1*/
    int16_t burst_phdith_thresh;
    int16_t burst_att_thresh;
    float att_per_frame;
    int16_t tr_dec[LGW_MAX];

    /* check burst error */
    burst_len = time_offs / output_frame + 1;

    set_s( att_always, 0, LGW_MAX );
    *ph_dith = 0.0f;

    /* softly shift attenuation just a bit later for estimated "stable" music_content */
    burst_phdith_thresh = BURST_PHDITH_THRESH + (int16_t) ( est_mus_content * 1.0f + 0.5f );
    burst_att_thresh = BURST_ATT_THRESH + (int16_t) ( est_mus_content * 1.0f + 0.5f );
    att_per_frame = (float) ( ATT_PER_FRAME - (int16_t) ( est_mus_content * 1.0f + 0.5f ) ); /* only slighty less att for music */
    att_per_frame *= 0.1f;

    if ( burst_len > burst_phdith_thresh )
    {
        /* increase degree of dither */
        *ph_dith = PHASE_DITH * min( 1.0f, ( (float) burst_len - (float) burst_phdith_thresh ) / (float) BURST_PHDITH_RAMPUP_LEN );
    }

    att_degree = 0;
    if ( burst_len > burst_att_thresh )
    {
        set_s( att_always, 1, LGW_MAX );

        /* increase degree of attenuation */
        if ( burst_len - burst_att_thresh <= PH_ECU_MUTE_START )
        {
            att_degree = (float) ( burst_len - burst_att_thresh ) * att_per_frame;
        }
        else
        {
            att_degree = (float) PH_ECU_MUTE_START * att_per_frame + ( burst_len - burst_att_thresh - PH_ECU_MUTE_START ) * 6.0206f;
        }
    }

    Lprot = ( 2 * output_frame * 4 ) / 5; /* 4/5==1024/1280, keep mult within short */
    Ltrana = Lprot / QUOT_LPR_LTR;
    Ltrana_2 = Ltrana / 2;

    if ( output_frame == L_FRAME48k )
    {
        w_hamm = w_hamm48k_2;
        Lgw = LGW48k;
    }
    else if ( output_frame == L_FRAME32k )
    {
        w_hamm = w_hamm32k_2;
        LtranaLog = L_TRANA_LOG32k;
        Lgw = LGW32k;
    }
    else
    {
        w_hamm = w_hamm16k_2;
        LtranaLog = L_TRANA_LOG16k;
        Lgw = LGW16k;
    }

    if ( burst_len <= 1 || ( burst_len == 2 && last_fec ) )
    {
        set_f( alpha, 1.0f, LGW_MAX );
        set_f( beta, 0.0f, LGW_MAX );
        *beta_mute = BETA_MUTE_FAC_INI;

        /* apply hamming window */
        v_mult( xfp, w_hamm, xfp_left, Ltrana_2 );
        mult_rev2( xfp + Ltrana_2, w_hamm, xfp_left + Ltrana_2, Ltrana_2 );

        xfp_ = xfp + Lprot - Ltrana;
        v_mult( xfp_, w_hamm, xfp_right, Ltrana_2 );
        mult_rev2( xfp_ + Ltrana_2, w_hamm, xfp_right + Ltrana_2, Ltrana_2 );

        /* spectrum */
        if ( output_frame == L_FRAME48k )
        {
            fft3( xfp_left, xfp_left, Ltrana );
            fft3( xfp_right, xfp_right, Ltrana );
        }
        else
        {
            fft_rel( xfp_left, Ltrana, LtranaLog );
            fft_rel( xfp_right, Ltrana, LtranaLog );
        }

        /* square representation */
        fft_spec2( xfp_left, Ltrana );
        fft_spec2( xfp_right, Ltrana );

        /* band powers in frequency groups
        exclude bin at 0 and at EVS_PI from calculation */
        xfp_left[Ltrana_2] = 0.0f;
        xfp_right[Ltrana_2] = 0.0f;
    }

    for ( k = 0; k < Lgw; k++ )
    {
        if ( burst_len <= 1 || ( burst_len == 2 && last_fec ) )
        {
            gr_pow_left[k] = sum_f( xfp_left + gw[k], gw[k + 1] - gw[k] );
            gr_pow_right[k] = sum_f( xfp_right + gw[k], gw[k + 1] - gw[k] );

            /* check if transient in any of the bands */
            gr_pow_left[k] += FLT_MIN; /* otherwise div by zero may occur */
            gr_pow_right[k] += FLT_MIN;

            Xavg[k] = (float) ( sqrt( 0.5f * ( gr_pow_left[k] + gr_pow_right[k] ) / (float) ( gw[k + 1] - gw[k] ) ) );

            grp_pow_chg = gr_pow_right[k] / gr_pow_left[k];

            /* dither phase in case of transient */
            /* separate transition detection and application of forced burst dithering */
            tr_dec[k] = ( grp_pow_chg > THRESH_TR_LIN ) || ( grp_pow_chg < THRESH_TR_LIN_INV );

            /* magnitude modification */
            if ( tr_dec[k] || att_always[k] )
            {
                att_val = min( MAX_INCREASE_GRPOW_LIN, grp_pow_chg );
                att_val = (float) sqrt( att_val );
                mag_chg_1st[k] = att_val;
                mag_chg[k] = att_val;
            }
            else
            {
                mag_chg_1st[k] = 1.0f;
                mag_chg[k] = 1.0f;
            }
        }
        else
        {
            if ( burst_len < OFF_FRAMES_LIMIT )
            {
                mag_chg[k] = mag_chg_1st[k] * (float) pow( 10.0, -att_degree / 20.0 );
            }
            else
            {
                mag_chg[k] = 0;
            }
            if ( burst_len > BETA_MUTE_THR )
            {
                *beta_mute *= BETA_MUTE_FAC;
            }
            alpha[k] = mag_chg[k];
            beta[k] = (float) ( sqrt( 1.0f - SQR( alpha[k] ) ) * *beta_mute );
            if ( k >= LGW32k - 1 )
            {
                beta[k] *= 0.1f;
            }
            else if ( k >= LGW16k - 1 )
            {
                beta[k] *= 0.5f;
            }
        }
    }

    return;
}


/*------------------------------------------------------------------*
 * peakfinder()
 *
 * Peak-picking algorithm
 *------------------------------------------------------------------*/

void peakfinder(
    const float *x0,        /* i  : vector from which the maxima will be found                     */
    const int16_t len0,     /* i  : length of input vector                                         */
    int16_t *plocs,         /* o  : the indicies of the identified peaks in x0                     */
    int16_t *cInd,          /* o  : number of identified peaks                                     */
    const float sel,        /* i  : The amount above surrounding data for a peak to be identified  */
    const int16_t endpoints /* i  : Flag to include endpoints in peak search                       */
)
{
    float minMag, tempMag, leftMin;
    float dx0[L_PROT48k_2], x[L_PROT48k_2 + 1], peakMag[MAX_PLOCS];
    int16_t k, i, len, tempLoc = 0, foundPeak, ii, xInd;
    int16_t *ind, indarr[L_PROT48k_2 + 1], peakLoc[MAX_PLOCS];

    ind = indarr;

    /* Find derivative */
    v_sub( x0 + 1, x0, dx0, len0 - 1 );

    /* This is so we find the first of repeated values */
    for ( i = 0; i < len0 - 1; i++ )
    {
        if ( dx0[i] == 0.0f )
        {
            dx0[i] = -1.0e-12f;
        }
    }

    /* Find where the derivative changes sign
       Include endpoints in potential peaks and valleys */
    k = 0;

    if ( endpoints )
    {
        x[k] = x0[0];
        ind[k++] = 0;
    }

    for ( i = 1; i < len0 - 1; i++ )
    {
        if ( dx0[i - 1] * dx0[i] < 0 )
        {
            ind[k] = i;
            x[k++] = x0[i];
        }
    }

    if ( endpoints )
    {
        ind[k] = len0 - 1;
        x[k++] = x0[len0 - 1];
    }
    /* x only has the peaks, valleys, and endpoints */
    len = k;
    minimum( x, len, &minMag );

    if ( ( len > 2 ) || ( !endpoints && ( len > 0 ) ) )
    {
        /* Set initial parameters for loop */
        tempMag = minMag;
        foundPeak = 0;
        leftMin = minMag;

        if ( endpoints )
        {
            /* Deal with first point a little differently since tacked it on
               Calculate the sign of the derivative since we taked the first point
               on it does not necessarily alternate like the rest. */

            /* The first point is larger or equal to the second */
            if ( x[0] >= x[1] )
            {
                ii = -1;
                if ( x[1] >= x[2] ) /* x[1] is not extremum -> overwrite with x[0] */
                {
                    x[1] = x[0];
                    ind[1] = ind[0];
                    ind++;
                    len--;
                }
            }
            else /* First point is smaller than the second */
            {
                ii = 0;
                if ( x[1] < x[2] ) /* x[1] is not extremum -> overwrite with x[0] */
                {
                    x[1] = x[0];
                    ind[1] = ind[0];
                    ind++;
                    len--;
                }
            }
        }
        else
        {
            ii = -1; /* First point is a peak */
            if ( len >= 2 )
            {
                if ( x[1] >= x[0] )
                {
                    ii = 0; /* First point is a valley, skip it */
                }
            }
        }
        *cInd = 0;

        /* Loop through extrema which should be peaks and then valleys */
        while ( ii < len - 1 )
        {
            ii++; /* This is a peak */

            /*Reset peak finding if we had a peak and the next peak is bigger
              than the last or the left min was small enough to reset.*/
            if ( foundPeak )
            {
                tempMag = minMag;
                foundPeak = 0;
            }

            /* Make sure we don't iterate past the length of our vector */
            if ( ii == len - 1 )
            {
                break; /* We assign the last point differently out of the loop */
            }

            /* Found new peak that was larger than temp mag and selectivity larger
               than the minimum to its left. */
            if ( ( x[ii] > tempMag ) && ( x[ii] > leftMin + sel ) )
            {
                tempLoc = ii;
                tempMag = x[ii];
            }

            ii++; /* Move onto the valley */

            /* Come down at least sel from peak */
            if ( !foundPeak && ( tempMag > sel + x[ii] ) )
            {
                foundPeak = 1; /* We have found a peak */
                leftMin = x[ii];
                peakLoc[*cInd] = tempLoc; /* Add peak to index */
                peakMag[*cInd] = tempMag;
                ( *cInd )++;
            }
            else if ( x[ii] < leftMin ) /* New left minimum */
            {
                leftMin = x[ii];
            }
        }

        /* Check end point */
        if ( x[len - 1] > tempMag && x[len - 1] > leftMin + sel )
        {
            peakLoc[*cInd] = len - 1;
            peakMag[*cInd] = x[len - 1];
            ( *cInd )++;
        }
        else if ( !foundPeak && tempMag > minMag ) /* Check if we still need to add the last point */
        {
            peakLoc[*cInd] = tempLoc;
            peakMag[*cInd] = tempMag;
            ( *cInd )++;
        }

        /* Create output */
        for ( i = 0; i < *cInd; i++ )
        {
            plocs[i] = ind[peakLoc[i]];
        }
    }
    else
    {
        if ( endpoints )
        {
            /* This is a monotone function where an endpoint is the only peak */
            xInd = ( x[0] > x[1] ) ? 0 : 1;
            peakMag[0] = x[xInd];
            if ( peakMag[0] > minMag + sel )
            {
                plocs[0] = ind[xInd];
                *cInd = 1;
            }
            else
            {
                *cInd = 0;
            }
        }
        else
        {
            /* Input constant or all zeros -- no peaks found */
            *cInd = 0;
        }
    }

    return;
}


/*-------------------------------------------------------------------*
 * imax_pos()
 *
 * Get interpolated maximum position
 *-------------------------------------------------------------------*/

/*! r: interpolated maximum position */
float imax_pos(
    const float *y /* i  : Input vector for peak interpolation */
)
{
    float posi, y1, y2, y3, y3_y1, y2i;
    float ftmp_den1, ftmp_den2;
    /* Seek the extrema of the parabola P(x) defined by 3 consecutive points so that P([-1 0 1]) = [y1 y2 y3] */
    y1 = y[0];
    y2 = y[1];
    y3 = y[2];
    y3_y1 = y3 - y1;
    ftmp_den1 = ( y1 + y3 - 2 * y2 );
    ftmp_den2 = ( 4 * y2 - 2 * y1 - 2 * y3 );

    if ( ftmp_den2 == 0.0f || ftmp_den1 == 0.0f )
    {
        return ( 0.0f ); /* early exit with left-most value */
    }

    y2i = -0.125f * SQR( y3_y1 ) / ( ftmp_den1 ) + y2;
    /* their corresponding normalized locations */
    posi = y3_y1 / ( ftmp_den2 );
    /* Interpolated maxima if locations are not within [-1,1], calculated extrema are ignored */
    if ( posi >= 1.0f || posi <= -1.0f )
    {
        posi = y3 > y1 ? 1.0f : -1.0f;
    }
    else
    {
        if ( y1 >= y2i )
        {
            posi = ( y1 > y3 ) ? -1.0f : 1.0f;
        }
        else if ( y3 >= y2i )
        {
            posi = 1.0f;
        }
    }

    return posi + 1.0f;
}


/*-------------------------------------------------------------------*
 * spec_ana()
 *
 * Spectral analysis
 *-------------------------------------------------------------------*/

static void spec_ana(
    const float *prevsynth,     /* i  : Input signal                                    */
    int16_t *plocs,             /* o  : The indicies of the identified peaks            */
    float *plocsi,              /* o  : Interpolated positions of the identified peaks  */
    int16_t *num_plocs,         /* o  : Number of identified peaks                      */
    float *X_sav,               /* o  : Stored fft spectrum                             */
    const int16_t output_frame, /* i  : Frame length                                    */
    const int16_t bwidth,       /* i  : Encoded bandwidth                               */
    const int16_t element_mode, /* i  : IVAS element mode                               */
    float *noise_fac,           /* o  : for few peaks zeroing valleys decision making   */
    const float pcorr )
{
    int16_t i, Lprot, LprotLog2 = 0, hamm_len2 = 0, Lprot2_1, m;
    float *pPlocsi;
    int16_t *pPlocs;
    int16_t currPlocs, endPlocs, Lprot2p1, nJacob;
    int16_t n, k;
    int16_t st_point;
    int16_t end_point;

    float sig, noise, nsr;
    float window_corr_step, window_corr;
    const float *w_hamm = NULL;
    float xfp[L_PROT48k];
    float Xmax, Xmin, sel;
    int16_t stop_band_start;
    int16_t stop_band_length;

    Lprot = 2 * output_frame * L_PROT32k / 1280;
    Lprot2_1 = Lprot / 2 + 1;

    if ( output_frame == L_FRAME48k )
    {
        w_hamm = w_hamm_sana48k_2;
        hamm_len2 = L_PROT_HAMM_LEN2_48k;
    }
    else if ( output_frame == L_FRAME32k )
    {
        w_hamm = w_hamm_sana32k_2;
        hamm_len2 = L_PROT_HAMM_LEN2_32k;
        LprotLog2 = 10;
    }
    else
    {
        w_hamm = w_hamm_sana16k_2;
        hamm_len2 = L_PROT_HAMM_LEN2_16k;
        LprotLog2 = 9;
    }

    /* Apply hamming-rect window */
    mvr2r( prevsynth + hamm_len2, xfp + hamm_len2, Lprot - 2 * hamm_len2 );
    if ( element_mode == EVS_MONO )
    {
        v_mult( prevsynth, w_hamm, xfp, hamm_len2 );
        mult_rev2( prevsynth + Lprot - hamm_len2, w_hamm, xfp + Lprot - hamm_len2, hamm_len2 );
    }
    else
    {
        window_corr = w_hamm[0];
        window_corr_step = w_hamm[0] / hamm_len2;
        for ( i = 0; i < hamm_len2; i++ )
        {
            xfp[i] = prevsynth[i] * ( w_hamm[i] - window_corr );
            xfp[Lprot - i - 1] = prevsynth[Lprot - i - 1] * ( w_hamm[i] - window_corr );
            window_corr -= window_corr_step;
        }
    }

    /* Spectrum */
    if ( output_frame == L_FRAME48k )
    {
        fft3( xfp, xfp, Lprot );
    }
    else
    {
        fft_rel( xfp, Lprot, LprotLog2 );
    }

    /* Apply zeroing of non-coded FFT spectrum */
    if ( output_frame > inner_frame_tbl[bwidth] )
    {
        stop_band_start = 128 << bwidth;
        stop_band_length = Lprot - ( stop_band_start << 1 );
        stop_band_start = stop_band_start + 1;
        set_f( xfp + stop_band_start, 0, stop_band_length );
    }

    mvr2r( xfp, X_sav, Lprot );

    /* Magnitude representation */
    fft_spec2( xfp, Lprot );

    for ( i = 0; i < Lprot2_1; i++ )
    {
        xfp[i] = (float) sqrt( (double) xfp[i] );
    }

    /* Find maxima */
    maximum( xfp, Lprot2_1, &Xmax );
    minimum( xfp, Lprot2_1, &Xmin );
    if ( element_mode == EVS_MONO )
    {
        sel = ( Xmax - Xmin ) * ( 1.0f - PFIND_SENS );
    }
    else
    {
        sel = ( Xmax - Xmin ) * ( 1.0f - ST_PFIND_SENS );
    }

    peakfinder( xfp, Lprot2_1, plocs, num_plocs, sel, TRUE ); /* NB peak at xfp[0] and xfp Lprot2_1-1 may occur */

    /* Currently not the pitch correlation but some LF correlation */
    if ( element_mode != EVS_MONO && *num_plocs > 50 && pcorr < 0.6f )
    {
        *num_plocs = 0;
    }

    if ( element_mode == EVS_MONO )
    {
        /* Refine peaks */
        for ( m = 0; m < *num_plocs; m++ )
        {
            if ( plocs[m] == 0 )
            {
                plocsi[m] = plocs[m] + imax_pos( &xfp[plocs[m]] );
            }
            else if ( plocs[m] == Lprot / 2 )
            {
                plocsi[m] = plocs[m] - 2 + imax_pos( &xfp[plocs[m] - 2] );
            }
            else
            {
                plocsi[m] = plocs[m] - 1 + imax_pos( &xfp[plocs[m] - 1] );
            }
        }
    }
    else
    {

        Lprot2p1 = Lprot / 2 + 1;

        /* Refine peaks */
        pPlocsi = plocsi;
        pPlocs = plocs;
        n = *num_plocs; /* number of peaks to process */

        /* Special case-- The very 1st peak if it is at 0 index position (DC) */
        /* With DELTA_CORR_F0_INT == 2 one needs to handle both *pPlocs==0 and *pPlocs==1 */
        if ( n > 0 && *pPlocs == 0 ) /* Very 1st peak position possible to have a peak at 0/DC index position. */
        {
            *pPlocsi++ = *pPlocs + imax_pos( &xfp[*pPlocs] );
            pPlocs++;
            n = n - 1;
        }

        if ( n > 0 && *pPlocs == 1 ) /* Also 2nd peak position uses DC which makes jacobsen unsuitable. */
        {
            *pPlocsi++ = *pPlocs - 1 + imax_pos( &xfp[*pPlocs - 1] );
            currPlocs = *pPlocs++;
            n = n - 1;
        }

        /* All remaining peaks except the very last two possible integer positions */
        currPlocs = *pPlocs++;
        endPlocs = Lprot2p1 - DELTA_CORR_F0_INT; /* last *pPlocs position for Jacobsen */

        /* precompute number of turns based on endpoint integer location  and make into  a proper for loop */
        if ( n > 0 )
        {
            nJacob = n;
            if ( sub( endPlocs, plocs[sub( *num_plocs, 1 )] ) <= 0 )
            {
                nJacob = sub( nJacob, 1 );
            }

            for ( k = 0; k < nJacob; k++ )
            {
                *pPlocsi++ = currPlocs + imax2_jacobsen_mag( &( X_sav[currPlocs - 1] ), &( X_sav[Lprot - 1 - currPlocs] ) );
                currPlocs = *pPlocs++;
            }
            n = n - nJacob;
        }

        /* At this point there should at most two plocs left to process */
        /* the position before fs/2 and fs/2 both use the same magnitude points */
        if ( n > 0 )
        {
            /* [ . . .            .  .  .  . ]   Lprot/2+1 positions  */
            /*   |                   |     |           */
            /*   0         (Lprot/2-2)     (Lprot/2)   */

            if ( currPlocs == ( Lprot2p1 - DELTA_CORR_F0_INT ) ) /* Also 2nd last peak position uses fs/2  which makes jacobsen less suitable. */
            {
                *pPlocsi++ = currPlocs - 1 + imax_pos( &xfp[currPlocs - 1] );
                currPlocs = *pPlocs++;
                n = n - 1;
            }

            /* Here the only remaining point would be a  fs/2 plocs */
            /*    pXfp = xfp + sub(Lprot2,1); already set just a reminder where it
             * whould point */
            if ( n > 0 ) /* fs/2 which makes special case . */
            {
                *pPlocsi++ = currPlocs - 2 + imax_pos( &xfp[currPlocs - 2] );
                currPlocs = *pPlocs++;
                n = n - 1;
            }
        }

        /* For few peaks decide noise floor attenuation */
        if ( *num_plocs < 3 && *num_plocs > 0 )
        {
            sig = sum_f( xfp, Lprot2_1 ) + EPSILON;

            /*excluding peaks and neighboring bins*/
            for ( i = 0; i < *num_plocs; i++ )
            {
                st_point = max( 0, plocs[i] - DELTA_CORR );
                end_point = min( Lprot2_1 - 1, plocs[i] + DELTA_CORR );
                set_f( &xfp[st_point], 0.0f, end_point - st_point + 1 );
            }
            noise = sum_f( xfp, Lprot2_1 ) + EPSILON;
            nsr = noise / sig;

            if ( nsr < 0.03f )
            {
                *noise_fac = 0.5f;
            }
            else
            {
                *noise_fac = 1.0f;
            }
        }
    }

    return;
}

/*-------------------------------------------------------------------*
 * subst_spec()
 *
 * Substitution spectrum calculation
 *-------------------------------------------------------------------*/

static void subst_spec(
    const int16_t *plocs,           /* i  : The indicies of the identified peaks               */
    const float *plocsi,            /* i  : Interpolated positions of the identified peaks     */
    int16_t *num_plocs,             /* i/o: Number of identified peaks                         */
    const int16_t time_offs,        /* i  : Time offset                                        */
    float *X,                       /* i/o: FFT spectrum                                       */
    const float *mag_chg,           /* i  : Magnitude modification                             */
    const float ph_dith,            /* i  : Phase dither                                       */
    const int16_t *is_trans,        /* i  : Transient flags                                    */
    const int16_t output_frame,     /* i  : Frame length                                       */
    int16_t *seed,                  /* i/o: Random seed                                        */
    const float *alpha,             /* i  : Magnitude modification factors for fade to average */
    const float *beta,              /* i  : Magnitude modification factors for fade to average */
    float beta_mute,                /* i  : Factor for long-term mute                          */
    const float Xavg[LGW_MAX],      /* i  : Frequency group averages to fade to                */
    const int16_t element_mode,     /* i  : IVAS element mode                                  */
    const int16_t ph_ecu_lookahead, /* i  : Phase ECU lookahead                                */
    const float noise_fac           /* i  : noise factor                                       */

)
{
    const float *sincos;
    int16_t Xph_short;
    float corr_phase[MAX_PLOCS], Xph;
    float Lprot_1, cos_F, sin_F, tmp;
    int16_t Lprot, Lecu, m, i, e, im_ind, delta_corr_up, delta_corr_dn, delta_tmp;
    float mag_chg_local; /* for peak attenuation in burst */
    int16_t k;
    float one_peak_flag_mask;
    float alpha_local;
    float beta_local;


    sincos = sincos_t_ext + 128;
    Lprot = (int16_t) ( L_PROT32k * output_frame / 640 );
    Lprot_1 = 1.0f / Lprot;
    Lecu = output_frame * 2;

    /* Correction phase of the identified peaks */
    if ( is_trans[0] || is_trans[1] )
    {
        *num_plocs = 0;
    }
    else
    {
        tmp = PI2 * ( Lecu - ( Lecu - Lprot ) / 2 + NS2SA( output_frame * FRAMES_PER_SEC, PH_ECU_ALDO_OLP2_NS ) - ph_ecu_lookahead - output_frame / 2 + time_offs ) * Lprot_1;
        for ( m = 0; m < *num_plocs; m++ )
        {
            corr_phase[m] = plocsi[m] * tmp;
        }
    }
    one_peak_flag_mask = 1; /* all ones mask -> keep  */
    if ( element_mode != EVS_MONO )
    {
        if ( ( *num_plocs > 0 ) && sub( *num_plocs, 3 ) < 0 )
        {
            one_peak_flag_mask = noise_fac; /* all zeroes  mask -> zero  */