/*====================================================================================
    EVS Codec 3GPP TS26.443 Jun 30, 2015. Version CR 26.443-0006
  ====================================================================================*/

#include <math.h>
#include "options.h"
#include "cnst.h"
#include "rom_com.h"
#include "prot.h"
#include "stl.h"

/*-------------------------------------------------------------------*
 * Local definitions
 *-------------------------------------------------------------------*/

#define OFFSET 21
#define STEP   9

typedef unsigned long long ui64_t;

/*-------------------------------------------------------------------*
 * own_cos()
 *
 *  Bit-exact cosine
 *-------------------------------------------------------------------*/

short own_cos(
    const short x
)
{
    short a[4] = {14967,   -25518,   3415,   32351};
    int b;

    b = a[0];
    b = (a[1] + ((b * x) >> 16)) << 1;
    b = a[2] + ((b * x) >> 15);
    b = a[3] + ((b * x) >> 15);

    return (short)b;
}

/*-------------------------------------------------------------------*
 * get_angle_res()
 *
 *
 *-------------------------------------------------------------------*/

short get_angle_res(short dim, short bits)
{
    short bits_min = 28;
    short bits_max = 96;

    short angle_res, angle_bits;

    angle_bits = min(bits-bits_max, (bits+(2*dim-1)*bits_min)/(2*dim-1));
    angle_bits = min(56, angle_bits);

    if (angle_bits<4)
    {
        angle_res = 1;
    }
    else
    {
        angle_res = pow2_angle_res[angle_bits&0x7]>>(14-(angle_bits>>3));
        angle_res = (angle_res+1)>>1<<1;
    }

    return angle_res;
}

/*-------------------------------------------------------------------*
 * get_pulse ()
 *
 *
 *-------------------------------------------------------------------*/

short get_pulse(
    const short q
)
{
    short s, m, k;

    if (q <= OFFSET)
    {
        return q;
    }
    else
    {
        s = (short)((q - OFFSET)*(1.0f/STEP));
        m = q - (OFFSET + s*STEP);
        k = (255 >> (8-s)) << 1;

        return min(KMAX, OFFSET + STEP*k + (1 << (s+1))*m);
    }
}

/*-------------------------------------------------------------------*
 * bits2pulses()
 *
 *
 *-------------------------------------------------------------------*/

short bits2pulses(
    const short N,
    const short bits,
    const short strict_bits
)
{
    const unsigned char *tab;
    short B, mid, low, high, q;
    short i;

    tab = hBitsN[N];
    low = 1;
    high = tab[0];
    B = bits - 1;

    if (tab[high] <= B)
    {
        q =  high;
    }
    else
    {
        for (i = 0; i < 6; i++)
        {
            mid = (low + high)>>1;

            if (tab[mid] >= B)
            {
                high = mid;
            }
            else
            {
                low = mid;
            }
        }

        if (strict_bits || B - tab[low] <= tab[high] - B)
        {
            q = low;
        }
        else
        {
            q = high;
        }
    }

    return q;
}

/*-------------------------------------------------------------------*
 * bits2pulses()
 *
 *
 *-------------------------------------------------------------------*/

short pulses2bits(
    const short N,
    const short P
)
{
    const unsigned char *tab;
    short bits;

    tab = hBitsN[N];

    if (P == 0)
    {
        bits = 0;
    }
    else
    {
        bits = tab[P] + 1;
    }

    return bits;
}


/*-------------------------------------------------------------------*
 * local_norm_s()
 *
 *
 *-------------------------------------------------------------------*/
static
short local_norm_s (       /* o : n shifts needed to normalize */
    short short_var      /* i : signed 16 bit variable */
)
{
    short short_res;

    if (short_var == -1 )
    {
        return (16-1);
    }
    else
    {
        if (short_var == 0)
        {
            return 0;
        }

        else
        {
            if (short_var < 0)
            {
                short_var = ~short_var;
            }

            for (short_res = 0; short_var < 0x4000; short_res++)
            {
                short_var <<= 1;
            }
        }
    }

    return (short_res);
}


/*--------------------------------------------------------------------------*
 * log2_div()
 *
 *  Logarithm of division
 *--------------------------------------------------------------------------*/

short  log2_div(        /* o : Log2 of division, Q11 */
    short input_s,      /* i : Numerator         Q15 */
    short input_c       /* i : Denominator       Q15 */
)
{
    Word16 mc, nc, ms, ns, d, z;
    Word16 result;
    Word32 acc;

    /* log2|sin(x)/cos(x)| = log2|sin(x)| - log2(cos(x)|
     *                     = log2|ms*2^-ns| - log2|mc*2^-nc|, where 0.5 <= ms < 1.0 and 0.5 <= mc < 1.0
     *                     = log2|ms| - ns - log2|mc| + nc
     *
     * Approximate log2(y) by a 2nd order least square fit polynomial. Then,
     *
     * log2|sin(x)/cos(x)| ~ (a0*ms^2 + a1*ms + a2) - ns - (a0*mc^2 + a1*mc + a2) + nc
     *                     = a0*(ms^2 - mc^2) + a1*(ms - mc) - ns + nc
     *                     = a0*(ms + mc)*(ms - mc) + a1*(ms - mc) - ns + nc
     *                     = (a0*(ms + mc) + a1)*(ms - mc) - ns + nc
     *                     = (a0*ms + a0*mc + a1)*(ms - mc) - ns + nc
     */
    ns = local_norm_s(input_s );    /* exponent*/
    nc = local_norm_s(input_c );    /* exponent */

    ms  = input_s << ns;   /* mantissa */
    mc  = input_c << nc;   /* mantissa */

    acc = L_mac(538500224L, mc, -2776);  /* a0*mc + a1, acc(Q27), a0(Q11), a1(Q27)*/
    z = mac_r(acc, ms, -2776);           /* z in Q11, a0 in Q11 */
    d = sub(ms, mc);                     /* d in Q15 */
    z = mult_r(z, d);                    /* z in Q11 */

    result = add(z, shl(sub(nc, ns), 11));

    return result;
}


/*--------------------------------------------------------------------------*
 * apply_gain()
 *
 * Apply gain
 *--------------------------------------------------------------------------*/

void apply_gain
(
    const short *ord,                       /* i  : Indices for energy order                     */
    const short *band_start,                /* i  : Sub band start indices                       */
    const short *band_end,                  /* i  : Sub band end indices                         */
    const short num_sfm,                    /* i  : Number of bands                              */
    const float *gains,                     /* i  : Band gain vector                             */
    float *xq                         /* i/o: Float synthesis / Gain adjusted synth        */
)
{
    short band,i;
    float g;

    for ( band = 0; band < num_sfm; band++)
    {
        g = gains[ord[band]];

        for( i = band_start[band]; i < band_end[band]; i++)
        {
            xq[i] *= g;
        }
    }

    return;
}


/*--------------------------------------------------------------------------*
 * fine_gain_quant()
 *
 * Fine gain quantization
 *--------------------------------------------------------------------------*/

void fine_gain_quant
(
    Encoder_State *st,
    const short *ord,                       /* i  : Indices for energy order                     */
    const short num_sfm,                    /* i  : Number of bands                              */
    const short *gain_bits,                 /* i  : Gain adjustment bits per sub band            */
    float *fg_pred,                   /* i/o: Predicted gains / Corrected gains            */
    const float *gopt                       /* i  : Optimal gains                                */
)
{
    short band;
    short gbits;
    short idx;
    float gain_db,gain_dbq;
    float err;

    for ( band = 0; band < num_sfm; band++)
    {
        gbits = gain_bits[ord[band]];
        if ( fg_pred[band] != 0 && gbits > 0 )
        {
            err = gopt[band] / fg_pred[band];
            gain_db = 20.0f*(float)log10(err);
            idx = (short) squant(gain_db, &gain_dbq, finegain[gbits-1], gain_cb_size[gbits-1]);
            push_indice( st, IND_PVQ_FINE_GAIN, idx, gbits );

            /* Update prediced gain with quantized correction */
            fg_pred[band] *= (float)pow(10, gain_dbq * 0.05f);
        }
    }

    return;
}

/*-------------------------------------------------------------------*
 * srt_vec_ind()
 *
 * sort vector and save sorting indices
 *-------------------------------------------------------------------*/

void srt_vec_ind (
    const short *linear,      /* i: linear input */
    short *srt,         /* o: sorted output*/
    short *I,           /* o: index for sorted output  */
    short length        /* i: length of vector */
)
{
    float linear_f[MAX_SRT_LEN];
    float srt_f[MAX_SRT_LEN];

    mvs2r(linear, linear_f, length);
    srt_vec_ind_f(linear_f, srt_f, I, length);
    mvr2s(srt_f, srt, length);

    return;
}

/*-------------------------------------------------------------------*
 * srt_vec_ind_f()
 *
 * sort vector and save sorting indices, using float input values
 *-------------------------------------------------------------------*/

void srt_vec_ind_f(
    const float *linear,      /* i: linear input */
    float *srt,         /* o: sorted output*/
    short *I,           /* o: index for sorted output  */
    short length        /* i: length of vector */
)
{
    short pos,npos;
    short idxMem;
    float valMem;

    /*initialize */
    for (pos = 0; pos < length; pos++)
    {
        I[pos] = pos;
    }

    mvr2r(linear, srt,length);

    /* now iterate */
    for (pos = 0; pos < (length - 1); pos++)
    {
        for (npos = (pos + 1); npos < length;  npos++)
        {
            if (srt[npos] < srt[pos])
            {
                idxMem    = I[pos];
                I[pos]    = I[npos];
                I[npos]   = idxMem;

                valMem    = srt[pos];
                srt[pos]  = srt[npos];
                srt[npos] = valMem;
            }
        }
    }

    return;
}


unsigned int UMult_32_32(unsigned int UL_var1, unsigned int UL_var2)
{
    ui64_t tmp;

    tmp = (ui64_t)UL_var1 * (ui64_t)UL_var2;
    return (unsigned int)(tmp >> 32);
}

/*-------------------------------------------------------------------*
 *  UL_div
 *
 *  Calculate UL_num/UL_den. UL_num assumed to be Q31, UL_den assumed
 *  to be Q32, then result is in Q32.
 *-------------------------------------------------------------------*/
static unsigned int UL_div(const unsigned int UL_num, const unsigned int UL_den)
{
    unsigned int UL_e, UL_Q;
    unsigned int UL_msb;
    short i;

    UL_e = 0xffffffff - UL_den;
    UL_Q = UL_num;

    for (i = 0; i < 5; i++)
    {
        UL_msb = UMult_32_32(UL_Q, UL_e);
        UL_Q = UL_Q + UL_msb;
        UL_e = UMult_32_32(UL_e, UL_e);
    }

    return UL_Q;
}
/*-------------------------------------------------------------------*
 *  UL_inverse
 *
 *  Calculate inverse of UL_val. Output in Q_exp.
 *-------------------------------------------------------------------*/
unsigned int UL_inverse(const unsigned int UL_val, short *exp)
{
    unsigned int UL_tmp;

    /* *exp = norm_ul(UL_val);*/
    *exp = 31 - log2_i(UL_val);
    UL_tmp = UL_val << (*exp);   /* Q32*/

    *exp = 32 + 31 - *exp;

    return UL_div(0x80000000, UL_tmp);
}

/*-----------------------------------------------------------------------------
 * ratio()
 *
 * Divide the numerator by the denominator.
 *----------------------------------------------------------------------------*/
Word16 ratio(const Word32 numer, const Word32 denom, Word16 *expo)
{
    Word16 expNumer, expDenom;
    Word16 manNumer, manDenom;
    Word16 quotient;

    expDenom = norm_l(denom);                     /* exponent */
    manDenom = extract_h(L_shl(denom, expDenom)); /* mantissa */
    expNumer = norm_l(numer);                     /* exponent */
    manNumer = extract_h(L_shl(numer, expNumer)); /* mantissa */
    manNumer = shr(manNumer, 1); /* Ensure the numerator < the denominator */
    quotient = div_s(manNumer, manDenom); /* in Q14 */

    *expo = sub(expNumer, expDenom);

    return quotient; /* Q14 */
}

/*-----------------------------------------------------------------------------
 * atan2_fx():
 *
 * Approximates arctan piecewise with various 4th to 5th order least square fit
 * polynomials for input in 5 segments:
 *   - 0.0 to 1.0
 *   - 1.0 to 2.0
 *   - 2.0 to 4.0
 *   - 4.0 to 8.0
 *   - 8.0 to infinity
 *---------------------------------------------------------------------------*/
Word16 atan2_fx(  /* o: Angle between 0 and EVS_PI/2 radian (Q14) */
    const Word32 y,  /* i: Argument must be positive (Q15) */
    const Word32 x   /* i: Q15 */
)
{
    Word32 acc, arg;
    Word16 man, expo, reciprocal;
    Word16 angle, w, z;

    IF (L_sub(x, 0) == 0)
    {
        return 25736; /* EVS_PI/2 in Q14 */
    }
    man = ratio(y, x, &expo); /*  man in Q14 */
    expo = sub(expo, (15 - 14)); /*  Now, man is considered in Q15 */
    arg = L_shr((Word32)man, expo);

    IF (L_shr(arg, 3+15) != 0)
    /*===============================*
     *      8.0 <= x < infinity      *
     *===============================*/
    {
        /* atan(x) = EVS_PI/2 - 1/x + 1/(3x^3) - 1/(5x^5) + ...
         *         ~ EVS_PI/2 - 1/x, for x >= 8.
         */
        expo = norm_l(arg);
        man = extract_h(L_shl(arg, expo));
        reciprocal = div_s(0x3fff, man);
        expo = sub(15 + 1, expo);
        reciprocal = shr(reciprocal, expo);   /*  Q14*/
        angle = sub(25736, reciprocal);       /* Q14   (EVS_PI/2 - 1/x) */

        /* For 8.0 <= x < 10.0, 1/(5x^5) is not completely negligible.
         * For more accurate result, add very small correction term.
         */
        IF (L_sub(L_shr(arg, 15), 10L) < 0)
        {
            angle = add(angle, 8); /* Add tiny correction term. */
        }
    }
    ELSE IF (L_shr(arg, 2+15) != 0)
    /*==========================*
     *      4.0 <= x < 8.0      *
     *==========================*/
    {
        /* interval: [3.999, 8.001]
         * atan(x) ~ (((a0*x     +   a1)*x   + a2)*x   + a3)*x   + a4
         *         = (((a0*8*y   +   a1)*8*y + a2)*8*y + a3)*8*y + a4   Substitute 8*y -> x
         *         = (((a0*8^3*y + a1*8^2)*y + a2*8)*y + a3)*8*y + a4
         *         = (((    c0*y +     c1)*y +   c2)*y + c3)*8*y + c4,
         *  where y = x/8
         *   and a0 = -1.28820869667651e-04, a1 = 3.88263533346295e-03,
         *       a2 = -4.64216306484597e-02, a3 = 2.75986060068931e-01,
         *       a4 = 7.49208077809799e-01.
         */
        w = extract_l(L_shr(arg, 3));              /* Q15  y = x/8 */
        acc = 533625337L;                          /* Q31  c1 = a1*8^2 */       move32();
        z = mac_r(acc, w, -2161);                  /* Q15  c0 = a0*8^3 */
        acc = -797517542L;                         /* Q31  c2 = a2*8 */         move32();
        z = mac_r(acc, w, z);                      /* Q15 */
        acc = 592675551L;                          /* Q31  c3 = a3 */           move32();
        z = mac_r(acc, w, z);                      /* z (in:Q15, out:Q12) */
        acc = 201114012L;                          /* Q28  c4 = a4 */           move32();
        acc = L_mac(acc, w, z);                    /* Q28 */
        angle = extract_l(L_shr(acc, (28 - 14)));  /* Q14 result of atan(x), where 4 <= x < 8 */
    }
    ELSE IF (L_shr(arg, 1+15) != 0)
    /*==========================*
     *      2.0 <= x < 4.0      *
     *==========================*/
    {
        /* interval: [1.999, 4.001]
         * atan(x) ~ (((a0*x    + a1)*x   +   a2)*x   + a3)*x   + a4
         *         = (((a0*4*y  + a1)*4*y +   a2)*4*y + a3)*4*y + a4   Substitute 4*y -> x
         *         = (((a0*16*y + a1*4)*y +   a2)*4*y + a3)*4*y + a4
         *         = (((a0*32*y + a1*8)*y + a2*2)*2*y + a3)*4*y + a4
         *         = (((   c0*y +   c1)*y +   c2)*2*y + c3)*4*y + c4,
         *  where y = x/4
         *   and a0 = -0.00262378195660943, a1 = 0.04089687039888652,
         *       a2 = -0.25631148958325911, a3 = 0.81685854627399479,
         *       a4 = 0.21358070563097167
         * */
        w = extract_l(L_shr(arg, 2));              /* Q15  y = x/4 */
        acc = 702602883L;                          /* Q31  c1 = a1*8 */         move32();
        z = mac_r(acc, w, -2751);                  /* Q15  c0 = a0*32 */
        acc = -1100849465L;                        /* Q31  c2 = a2*2 */         move32();
        z = mac_r(acc, w, z);                      /* z (in:Q15, out:Q14) */
        acc = 877095185L;                          /* Q30  c3 = a3 */           move32();
        z = mac_r(acc, w, z);                      /* z (in:Q14, out:Q12) */
        acc = 57332634L;                           /* Q28  c4 = a4 */           move32();
        acc = L_mac(acc, w, z);                    /* Q28 */
        angle = extract_l(L_shr(acc, (28 - 14)));  /* Q14  result of atan(x) where 2 <= x < 4 */
    }
    ELSE IF (L_shr(arg, 15) != 0)
    /*==========================*
     *      1.0 <= x < 2.0      *
     *==========================*/
    {
        /* interval: [0.999, 2.001]
         * atan(x) ~ (((a0*x   +  1)*x   + a2)*x   +   a3)*x   + a4
         *         = (((a0*2*y + a1)*2*y + a2)*2*y +   a3)*2*y + a4    Substitute 2*y -> x
         *         = (((a0*4*y + a1*2)*y + a2)*2*y +   a3)*2*y + a4
         *         = (((a0*4*y + a1*2)*y + a2)*y   + a3/2)*4*y + a4
         *         = (((  c0*y +   c1)*y + c2)*y   +   c3)*4*y + c4,
         *  where y = x/2
         *   and a0 = -0.0160706457245251, a1 = 0.1527106504065224,
         *       a2 = -0.6123208404800871, a3 = 1.3307896976322915,
         *       a4 = -0.0697089375247448
         */
        w = extract_l(L_shr(arg, 1));              /* Q15  y= x/2 */
        acc = 655887249L;                          /* Q31  c1 = a1*2 */         move32();
        z = mac_r(acc, w, -2106);                  /* Q15  c0 = a0*4 */
        acc = -1314948992L;                        /* Q31  c2 = a2 */           move32();
        z = mac_r(acc, w, z);
        acc = 1428924557L;                         /* Q31  c3 = a3/2 */         move32();
        z = mac_r(acc, w, z);                      /* z (in:Q15, out:Q13) */
        acc = -37424701L;                          /* Q29  c4 = a4 */           move32();
        acc = L_mac(acc, w, z);                    /* Q29 */
        angle = extract_l(L_shr(acc, (29 - 14)));  /* Q14  result of atan(x) where 1 <= x < 2 */
    }
    ELSE
    /*==========================*
     *      0.0 <= x < 1.0      *
     *==========================*/
    {
        /* interval: [-0.001, 1.001]
         * atan(x) ~ ((((a0*x   +   a1)*x   + a2)*x + a3)*x + a4)*x + a5
         *         = ((((a0*2*x + a1*2)*x/2 + a2)*x + a3)*x + a4)*x + a5
         *         = ((((  c0*x +   c1)*x/2 + c2)*x + c3)*x + c4)*x + c5
         *  where
         *    a0 = -5.41182677118661e-02, a1 = 2.76690449232515e-01,
         *    a2 = -4.63358392562492e-01, a3 = 2.87188466598566e-02,
         *    a4 =  9.97438122814383e-01, a5 = 5.36158556179092e-05.
         */
        w = extract_l(arg);                         /* Q15 */
        acc = 1188376431L;                          /* Q31  c1 = a1*2 */        move32();
        z = mac_r(acc, w, -3547);                   /* Q15  c0 = a0*2 */
        acc = -995054571L;                          /* Q31  c2 = a2 */          move32();
        z = extract_h(L_mac0(acc, w, z));           /* Q15  non-fractional mode multiply */
        acc = 61673254L;                            /* Q31  c3 = a3 */          move32();
        z = mac_r(acc, w, z);
        acc = 2141982059L;                          /* Q31  c4 = a4 */          move32();
        z = mac_r(acc, w, z);
        acc = 115139L;                              /* Q31  c5 = a5 */          move32();
        acc = L_mac(acc, w, z);                     /* Q31 */
        angle = extract_l(L_shr(acc, 31 - 14));     /* Q14  result of atan(x), where 0 <= x < 1 */
    }

    return angle;  /* Q14 between 0 and EVS_PI/2 radian. */
}