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

#include <stdlib.h>
#include <stdarg.h>
#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <assert.h>
#include "options.h"
#include "prot.h"


/*------------------------------------------------------------------*
 * own_random()
 *
 * Random generator
 *------------------------------------------------------------------*/

short own_random(       /* o  : output random value */
    short *seed          /* i/o: random seed         */
)
{
    *seed = (short) (*seed * 31821L + 13849L);

    return(*seed);
}

/*---------------------------------------------------------------------
 * sign()
 *
 *---------------------------------------------------------------------*/

float sign(             /* o  : sign of x (+1/-1) */
    const float x       /* i  : input value of x  */
)
{
    if (x < 0.0f)
    {
        return -1.0f;
    }
    else
    {
        return 1.0f;
    }
}

/*---------------------------------------------------------------------
 * log2_f()
 *
 *---------------------------------------------------------------------*/

float log2_f(            /* o  : logarithm2 of x */
    const float x        /* i  : input value of x   */
)
{
    return (float)(log(x)/log(2.0f));
}

/*---------------------------------------------------------------------
 * log2_i()
 *
 *---------------------------------------------------------------------*/

short log2_i(                 /* o  : integer logarithm2 of x */
    const unsigned int x      /* i  : input value of x        */
)
{
    unsigned int v;
    unsigned int mask[] = {0x2, 0xc, 0xf0, 0xff00, 0xffff0000};
    short shift[] = {1, 2, 4, 8, 16};
    short i, r;

    v = x;
    r = 0;
    for (i = 4; i >= 0; i--)
    {
        if (v & mask[i])
        {
            v >>= shift[i];
            r |= shift[i];
        }
    }

    return r;
}

/*---------------------------------------------------------------------
 * sum_s()
 * sum_f()
 *
 *---------------------------------------------------------------------*/

short sum_s(           /* o  : sum of all vector elements            */
    const short *vec,  /* i  : input vector                          */
    const short lvec   /* i  : length of input vector                */
)
{
    short i;
    short tmp;

    tmp = 0;
    for( i=0; i<lvec; i++ )
    {
        tmp += vec[i];
    }

    return tmp;
}


float sum_f(           /* o  : sum of all vector elements            */
    const float *vec,  /* i  : input vector                          */
    const short lvec   /* i  : length of input vector                */
)
{
    short i;
    float tmp;

    tmp = 0.0f;
    for( i=0; i<lvec; i++ )
    {
        tmp += vec[i];
    }

    return tmp;
}

/*----------------------------------------------------------------------
 * sum2_f()
 *
 *---------------------------------------------------------------------*/

float sum2_f(          /* o  : sum of all squared vector elements    */
    const float *vec,  /* i  : input vector                          */
    const short lvec   /* i  : length of input vector                */
)
{
    short i;
    float tmp;

    tmp = 0.0f;
    for( i=0; i<lvec; i++ )
    {
        tmp += vec[i] * vec[i];
    }

    return tmp;
}

/*-------------------------------------------------------------------*
 * set_c()
 * set_s()
 * set_f()
 * set_i()
 *
 * Set the vector elements to a value
 *-------------------------------------------------------------------*/

void set_c(
    char y[],  /* i/o: Vector to set                       */
    const char a,    /* i  : Value to set the vector to          */
    const short N     /* i  : Length of the vector                */
)
{
    short i;

    for (i=0 ; i<N ; i++)
    {
        y[i] = a;
    }

    return;
}


void set_s(
    short y[],  /* i/o: Vector to set                       */
    const short a,    /* i  : Value to set the vector to          */
    const short N     /* i  : Length of the vector                */
)
{
    short i;

    for (i=0 ; i<N ; i++)
    {
        y[i] = a;
    }

    return;
}

void set_i(
    int y[],  /* i/o: Vector to set                       */
    const int a,    /* i  : Value to set the vector to          */
    const short N     /* i  : Length of the vector                */
)
{
    short i;

    for (i=0 ; i<N ; i++)
    {
        y[i] = a;
    }

    return;
}

void set_f(
    float y[],  /* i/o: Vector to set                       */
    const float a,    /* i  : Value to set the vector to          */
    const short N     /* i  : Lenght of the vector                */
)
{
    short i ;

    for (i=0 ; i<N ; i++)
    {
        y[i] = a;
    }

    return;
}

/*---------------------------------------------------------------------*
 * set_zero()
 *
 * Set a vector vec[] of dimension lvec to zero
 *---------------------------------------------------------------------*/

void set_zero(
    float *vec, /* o  : input vector         */
    int lvec  /* i  : length of the vector */
)
{
    int i;

    for (i = 0; i < lvec; i++)
    {
        *vec++ = 0.0f;
    }

    return;
}


/*---------------------------------------------------------------------*
 * mvr2r()
 * mvs2s()
 * mvr2s()
 * mvs2r()
 * mvi2i()
 *
 * Transfer the contents of vector x[] to vector y[]
 *---------------------------------------------------------------------*/

void mvr2r(
    const float x[],  /* i  : input vector  */
    float y[],  /* o  : output vector */
    const short n     /* i  : vector size   */
)
{
    short i;

    if ( n <= 0 )
    {
        /* cannot transfer vectors with size 0 */
        return;
    }

    if (y < x)
    {
        for (i = 0; i < n; i++)
        {
            y[i] = x[i];
        }
    }
    else
    {
        for (i = n-1; i >= 0; i--)
        {
            y[i] = x[i];
        }
    }

    return;
}

void mvs2s(
    const short x[],  /* i  : input vector  */
    short y[],  /* o  : output vector */
    const short n     /* i  : vector size   */
)
{
    short i;

    if ( n <= 0 )
    {
        /* cannot transfer vectors with size 0 */
        return;
    }

    if (y < x)
    {
        for (i = 0; i < n; i++)
        {
            y[i] = x[i];
        }
    }
    else
    {
        for (i = n-1; i >= 0; i--)
        {
            y[i] = x[i];
        }
    }

    return;
}

unsigned int mvr2s(
    const float x[],  /* i  : input vector  */
    short y[],  /* o  : output vector */
    const short n     /* i  : vector size   */
)
{
    short i;
    float temp;
    unsigned int noClipping = 0;

    if ( n <= 0 )
    {
        /* cannot transfer vectors with size 0 */
        return 0;
    }

    if ((void*)y < (const void*)x)
    {
        for (i = 0; i < n; i++)
        {
            temp = x[i];
            temp = (float)floor(temp + 0.5f);

            if (temp >  32767.0f )
            {
                temp =  32767.0f;
                noClipping++;
            }
            else if (temp < -32768.0f )
            {
                temp = -32768.0f;
                noClipping++;
            }

            y[i] = (short)temp;
        }
    }
    else
    {
        for (i = n-1; i >= 0; i--)
        {
            temp = x[i];
            temp = (float)floor(temp + 0.5f);

            if (temp >  32767.0f )
            {
                temp =  32767.0f;
                noClipping++;
            }
            else if (temp < -32768.0f )
            {
                temp = -32768.0f;
                noClipping++;
            }

            y[i] = (short)temp;
        }
    }

    return noClipping;
}

void mvs2r(
    const short x[],  /* i  : input vector  */
    float y[],  /* o  : output vector */
    const short n     /* i  : vector size   */
)
{
    short i;

    if ( n <= 0 )
    {
        /* cannot transfer vectors with size 0 */
        return;
    }

    if ((void*)y < (const void*)x)
    {
        for (i = 0; i < n; i++)
        {
            y[i] = (float)x[i];
        }
    }
    else
    {
        for (i = n-1; i >= 0; i--)
        {
            y[i] = (float)x[i];
        }
    }

    return;
}


void mvi2i(
    const int x[],  /* i  : input vector  */
    int y[],  /* o  : output vector */
    const int n     /* i  : vector size   */
)
{
    int i;

    if ( n <= 0 )
    {
        /* no need to transfer vectors with size 0 */
        return;
    }

    if (y < x)
    {
        for (i = 0; i < n; i++)
        {
            y[i] = x[i];
        }
    }
    else
    {
        for (i = n-1; i >= 0; i--)
        {
            y[i] = x[i];
        }
    }

    return;
}

/*---------------------------------------------------------------------*
 * maximum()
 *
 * Find index and value of the maximum in a vector
 *---------------------------------------------------------------------*/

short maximum(         /* o  : index of the maximum value in the input vector */
    const float *vec,  /* i  : input vector                                   */
    const short lvec,  /* i  : length of input vector                         */
    float *max   /* o  : maximum value in the input vector              */
)
{
    short j, ind;
    float tmp;

    ind = 0;
    tmp = vec[0];

    for ( j=1; j<lvec; j++ )
    {
        if( vec[j] > tmp )
        {
            ind = j;
            tmp = vec[j];
        }
    }

    if ( max != 0 )
    {
        *max = tmp;
    }

    return ind;
}

/*---------------------------------------------------------------------*
 * minimum()
 *
 * Find index of a minimum in a vector
 *---------------------------------------------------------------------*/

short minimum(         /* o  : index of the minimum value in the input vector */
    const float *vec,  /* i  : input vector                                   */
    const short lvec,  /* i  : length of input vector                         */
    float *min   /* o  : minimum value in the input vector              */
)
{
    short j, ind;
    float tmp;

    ind = 0;
    tmp = vec[0];

    for ( j=1 ; j<lvec ; j++ )
    {
        if( vec[j] < tmp )
        {
            ind = j;
            tmp = vec[j];
        }
    }

    if ( min != 0 )
    {
        *min = tmp;
    }

    return ind;
}

/*---------------------------------------------------------------------*
 * emaximum()
 *
 * Find index of a maximum energy in a vector
 *---------------------------------------------------------------------*/

short emaximum(            /* o  : return index with max energy value in vector   */
    const float *vec,      /* i  : input vector                                   */
    const short lvec,      /* i  : length of input vector                         */
    float *ener_max  /* o  : maximum energy value                           */
)
{
    short j, ind;
    float temp;

    *ener_max = 0.0f;
    ind = 0;

    for( j=0; j<lvec; j++ )
    {
        temp = vec[j] * vec[j];

        if( temp > *ener_max )
        {
            ind = j;
            *ener_max = temp;
        }
    }

    return ind;
}


/*---------------------------------------------------------------------*
 * mean()
 *
 * Find the mean of the vector
 *---------------------------------------------------------------------*/

float mean(            /* o  : mean of vector                         */
    const float *vec,  /* i  : input vector                           */
    const short lvec   /* i  : length of input vector                 */
)
{
    float tmp;

    tmp = sum_f( vec, lvec ) / (float)lvec;

    return tmp;
}

/*---------------------------------------------------------------------*
 * dotp()
 *
 * Dot product of vector x[] and vector y[]
 *---------------------------------------------------------------------*/

float dotp(            /* o  : dot product of x[] and y[]    */
    const float  x[],  /* i  : vector x[]                    */
    const float  y[],  /* i  : vector y[]                    */
    const short  n     /* i  : vector length                 */
)
{
    short i;
    float suma;

    suma = x[0] * y[0];

    for(i=1; i<n; i++)
    {
        suma += x[i] * y[i];
    }

    return suma;
}



/*---------------------------------------------------------------------*
 * inv_sqrt()
 *
 * Find the inverse square root of the input value
 *---------------------------------------------------------------------*/

float inv_sqrt(    /* o  : inverse square root of input value */
    const float x  /* i  : input value                        */
)
{
    return (float)(1.0 / sqrt(x));
}

/*-------------------------------------------------------------------*
 * conv()
 *
 * Convolution between vectors x[] and h[] written to y[]
 * All vectors are of length L. Only the first L samples of the
 * convolution are considered.
 *-------------------------------------------------------------------*/

void conv(
    const float x[],   /* i  : input vector                              */
    const float h[],   /* i  : impulse response (or second input vector) */
    float y[],   /* o  : output vetor (result of convolution)      */
    const short L      /* i  : vector size                               */
)
{
    float temp;
    short i, n;
    for (n = 0; n < L; n++)
    {
        temp = x[0] * h[n];
        for (i = 1; i <= n; i++)
        {
            temp += x[i] * h[n-i];
        }
        y[n] = temp;
    }

    return;
}

/*-------------------------------------------------------------------*
 * fir()
 *
 * FIR filtering of vector x[] with filter having impulse response h[]
 * written to y[]
 * The input vector has length L and the FIR filter has an order of K, i.e.
 * K+1 coefficients. The memory of the input signal is provided in the vector mem[]
 * which has K values
 * The maximum length of the input signal is L_FRAME32k and the maximum order
 * of the FIR filter is L_FILT_MAX
 *-------------------------------------------------------------------*/

void fir(
    const float x[],   /* i  : input vector                              */
    const float h[],   /* i  : impulse response of the FIR filter        */
    float y[],   /* o  : output vector (result of filtering)       */
    float mem[], /* i/o: memory of the input signal (L samples)    */
    const short L,     /* i  : input vector size                         */
    const short K,     /* i  : order of the FIR filter (K+1 coefs.)      */
    const short upd    /* i  : 1 = update the memory, 0 = not            */
)
{
    float buf_in[L_FRAME48k+60], buf_out[L_FRAME48k], s;
    short i, j;

    /* prepare the input buffer (copy and update memory) */
    mvr2r( mem, buf_in, K );
    mvr2r( x, buf_in + K, L );

    if ( upd )
    {
        mvr2r( buf_in + L, mem, K );
    }

    /* do the filtering */
    for ( i = 0; i < L; i++ )
    {
        s = buf_in[K+i] * h[0];

        for (j = 1; j <= K; j++)
        {
            s += h[j] * buf_in[K+i-j];
        }

        buf_out[i] = s;
    }

    /* copy to the output buffer */
    mvr2r( buf_out, y, L );

    return;
}

/*-------------------------------------------------------------------*
 * v_add()
 *
 * Addition of two vectors sample by sample
 *-------------------------------------------------------------------*/

void v_add(
    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 short N       /* i  : Vector lenght                                    */
)
{
    short i ;

    for (i=0 ; i<N ; i++)
    {
        y[i] = x1[i] + x2[i] ;
    }

    return;
}

/*-------------------------------------------------------------------*
 * v_sub()
 *
 * Subtraction of two vectors sample by sample
 *-------------------------------------------------------------------*/

void v_sub(
    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 short N       /* i  : Vector lenght                                    */
)
{
    short i ;

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

    return;
}

/*-------------------------------------------------------------------*
 * v_mult()
 *
 * Multiplication of two vectors
 *-------------------------------------------------------------------*/

void v_mult(
    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 short N       /* i  : Vector lenght                                    */
)
{
    short i ;

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

    return;
}

/*-------------------------------------------------------------------*
 * v_multc()
 *
 * Multiplication of vector by constant
 *-------------------------------------------------------------------*/

void v_multc(
    const float x[],    /* i  : Input vector                                     */
    const float c,      /* i  : Constant                                         */
    float y[],    /* o  : Output vector that contains c*x                  */
    const short N       /* i  : Vector length                                    */
)
{
    short i ;

    for (i=0 ; i<N ; i++)
    {
        y[i] = c * x[i];
    }

    return;
}


/*-------------------------------------------------------------------*
 * squant()
 *
 * Scalar quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
 *
 * Searches a given codebook to find the nearest neighbour in Euclidean space.
 * Index of the winning codeword and the winning codeword itself are returned.
 *-------------------------------------------------------------------*/

int squant(             /* o: index of the winning codeword   */
    const float x,      /* i: scalar value to quantize        */
    float *xq,    /* o: quantized value                 */
    const float cb[],   /* i: codebook                        */
    const int   cbsize  /* i: codebook size                   */
)
{
    float dist, mindist, tmp;
    int   c, idx;
    idx = 0;
    mindist = 1e16f;

    for ( c = 0; c < cbsize; c++)
    {
        dist = 0.0f;
        tmp = x - cb[c];
        dist += tmp*tmp;
        if (dist < mindist)
        {
            mindist = dist;
            idx = c;
        }
    }

    *xq = cb[idx];

    return idx;
}


/*-------------------------------------------------------------------*
 * usquant()
 *
 * Uniform scalar quantizer according to MMSE criterion
 * (nearest neighbour in Euclidean space)
 *
 * Applies quantization based on scale and round operations.
 * Index of the winning codeword and the winning codeword itself are returned.
 *-------------------------------------------------------------------*/

short usquant(          /* o: index of the winning codeword   */
    const float x,      /* i: scalar value to quantize        */
    float *xq,    /* o: quantized value                 */
    const float qlow,   /* i: lowest codebook entry (index 0) */
    const float delta,  /* i: quantization step               */
    const short cbsize  /* i: codebook size                   */
)
{
    short idx;

    idx = (short) max(0.f, min(cbsize - 1, ( (x - qlow)/delta + 0.5f)));
    *xq = idx*delta + qlow;

    return idx;
}

/*-------------------------------------------------------------------*
 * usdequant()
 *
 * Uniform scalar de-quantizer routine
 *
 * Applies de-quantization based on scale and round operations.
 *-------------------------------------------------------------------*/

float usdequant(
    const int idx,      /* i: quantizer index                 */
    const float qlow,   /* i: lowest codebook entry (index 0) */
    const float delta   /* i: quantization step               */
)
{
    float g;

    g = idx * delta + qlow;

    return( g );
}


/*-------------------------------------------------------------------*
 * vquant()
 *
 * Vector quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
 *
 * Searches a given codebook to find the nearest neighbour in Euclidean space.
 * Index of the winning codevector and the winning vector itself are returned.
 *-------------------------------------------------------------------*/

int vquant(                   /* o: index of the winning codevector */
    float x[],          /* i: vector to quantize              */
    const float x_mean[],     /* i: vector mean to subtract (0 if none) */
    float xq[],         /* o: quantized vector                */
    const float cb[],         /* i: codebook                        */
    const int   dim,          /* i: dimension of codebook vectors   */
    const int   cbsize        /* i: codebook size                   */
)
{
    float dist, mindist, tmp;
    int   c, d, idx, j;

    idx = 0;
    mindist = 1e16f;

    if (x_mean != 0)
    {
        for( d = 0; d < dim; d++ )
        {
            x[d] -= x_mean[d];
        }
    }

    j = 0;
    for ( c = 0; c < cbsize; c++ )
    {
        dist = 0.0f;
        for( d = 0; d < dim; d++ )
        {
            tmp = x[d] - cb[j++];
            dist += tmp*tmp;
        }

        if ( dist < mindist )
        {
            mindist = dist;
            idx = c;
        }
    }

    if ( xq == 0 )
    {
        return idx;
    }

    j =  idx*dim;
    for ( d = 0; d < dim; d++)
    {
        xq[d] = cb[j++];
    }

    if (x_mean != 0)
    {
        for( d = 0; d < dim; d++)
        {
            xq[d] += x_mean[d];
        }
    }

    return idx;
}

/*-------------------------------------------------------------------*
 * w_vquant()
 *
 * Vector quantizer according to MMSE criterion (nearest neighbour in Euclidean space)
 *
 * Searches a given codebook to find the nearest neighbour in Euclidean space.
 * Weights are put on the error for each vector element.
 * Index of the winning codevector and the winning vector itself are returned.
 *-------------------------------------------------------------------*/

int w_vquant(                 /* o: index of the winning codevector */
    float x[],          /* i: vector to quantize              */
    const float x_mean[],     /* i: vector mean to subtract (0 if none) */
    const short weights[],    /* i: error weights                   */
    float xq[],         /* o: quantized vector                */
    const float cb[],         /* i: codebook                        */
    const int   dim,          /* i: dimension of codebook vectors   */
    const int   cbsize,       /* i: codebook size                   */
    const short rev_vect      /* i: reverse codebook vectors        */
)
{
    float dist, mindist, tmp;
    int   c, d, idx, j, k;

    idx = 0;
    mindist = 1e16f;

    if (x_mean != 0)
    {
        for( d = 0; d < dim; d++)
        {
            x[d] -= x_mean[d];
        }
    }

    j = 0;
    if (rev_vect)
    {
        k = dim-1;
        for ( c = 0; c < cbsize; c++)
        {
            dist = 0.0f;

            for( d = k; d >= 0; d--)
            {
                tmp = x[d] - cb[j++];
                dist += weights[d]*(tmp*tmp);
            }

            if (dist < mindist)
            {
                mindist = dist;
                idx = c;
            }
        }

        if (xq == 0)
        {
            return idx;
        }

        j =  idx*dim;
        for ( d = k; d >= 0; d--)
        {
            xq[d] = cb[j++];
        }
    }
    else
    {
        for ( c = 0; c < cbsize; c++)
        {
            dist = 0.0f;

            for( d = 0; d < dim; d++)
            {
                tmp = x[d] - cb[j++];
                dist += weights[d]*(tmp*tmp);
            }

            if (dist < mindist)
            {
                mindist = dist;
                idx = c;
            }
        }

        if (xq == 0)
        {
            return idx;
        }

        j =  idx*dim;
        for ( d = 0; d < dim; d++)
        {
            xq[d] = cb[j++];
        }
    }

    if (x_mean != 0)
    {
        for( d = 0; d < dim; d++)
        {
            xq[d] += x_mean[d];
        }
    }

    return idx;
}


/*----------------------------------------------------------------------------------*
 * v_sort()
 *
 * Sorting of vectors. This is very fast with almost ordered vectors.
 *----------------------------------------------------------------------------------*/

void v_sort(
    float *r,    /* i/o: Vector to be sorted in place */
    const short lo,    /* i  : Low limit of sorting range   */
    const short up     /* I  : High limit of sorting range  */
)
{
    short i, j;
    float tempr;

    for ( i=up-1; i>=lo; i-- )
    {
        tempr = r[i];
        for ( j=i+1; j<=up && (tempr>r[j]); j++ )
        {
            r[j-1] = r[j];
        }

        r[j-1] = tempr;
    }

    return;
}

/*---------------------------------------------------------------------*
 * var()
 *
 * Calculate the variance of a vector
 *---------------------------------------------------------------------*/

float var(                /* o: variance of vector                    */
    const float *x,        /* i: input vector                          */
    const int len          /* i: length of inputvector                 */
)
{
    float m;
    float v;
    short i;

    m = mean(x, (const short)len);

    v = 0.0f;
    for (i = 0; i < len; i++)
    {
        v += (x[i] - m)*(x[i] - m);
    }
    v /= len;

    return v;
}


/*---------------------------------------------------------------------*
 * std_dev()
 *
 * Calculate the standard deviation of a vector
 *---------------------------------------------------------------------*/

float std_dev(             /* o: standard deviation                    */
    const float *x,        /* i: input vector                          */
    const int len          /* i: length of the input vector            */
)
{
    short i;
    float std;

    std = 1e-16f;
    for( i = 0; i < len; i++)
    {
        std += x[i] * x[i];
    }

    std = (float)sqrt( std / len );

    return std;
}



/*---------------------------------------------------------------------*
 * dot_product_mat()
 *
 * Calculates dot product of type x'*A*x, where x is column vector of size m,
 * and A is square matrix of size m*m
 *---------------------------------------------------------------------*/

float dot_product_mat(    /* o  : the dot product x'*A*x        */
    const float  *x,      /* i  : vector x                      */
    const float  *A,      /* i  : matrix A                      */
    const short  m        /* i  : vector & matrix size          */
)
{
    short i,j;
    float suma, tmp_sum;
    const float *pt_x, *pt_A;

    pt_A = A;
    suma = 0;

    for(i=0; i<m; i++)
    {
        tmp_sum = 0;
        pt_x = x;
        for (j=0; j<m; j++)
        {
            tmp_sum += *pt_x++ **pt_A++;
        }

        suma += x[i] * tmp_sum;
    }

    return suma;
}

/*--------------------------------------------------------------------------------*
 * polezero_filter()
 *
 * Y(Z)=X(Z)(b[0]+b[1]z^(-1)+..+b[L]z^(-L))/(a[0]+a[1]z^(-1)+..+a[M]z^(-M))
 *          mem[n]=x[n]+cp[0]mem[n-1]+..+cp[M-1]mem[n-M], where cp[i]=-a[i+1]/a[0]
 *          y[n]=cz[0]mem[n]+cz[1]mem[n-1]+..+cz[L]mem[n-L], where cz[i]=b[i]/a[0]
 *          mem={mem[n-K] mem[n-K+1] . . . . mem[n-2] mem[n-1]}, where K=max(L,M)
 *
 * a[0] must be equal to 1.0f!
 *---------------------------------------------------------------------------------*/

void polezero_filter (
    const float *in,            /* i  : input vector                              */
    float *out,           /* o  : output vector                             */
    const short N,              /* i  : input vector size                         */
    const float *b,             /* i  : numerator coefficients                    */
    const float *a,             /* i  : denominator coefficients                  */
    const short order,          /* i  : filter order                              */
    const short numord,         /* i  : numerator order                           */
    const short denord,         /* i  : denominator order                         */
    float *mem            /* i/o: filter memory                             */
)
{
    short i, j, k;
    float *ptr, *ptr_start;


    ptr_start = mem + order - 1;

    if (denord == numord)
    {
        for (i=0; i<order; i++)
        {
            out[i] = in[i] * b[0];
            for (j = 0; j < i; j++)
            {
                out[i] += in[i-1-j] * b[j+1] - out[i-1-j] * a[j+1];
            }

            for (k = order-1; j < order; j++, k--)
            {
                out[i] += mem[k] * b[j+1] - mem[k+order] * a[j+1];
            }
        }

        for (; i < N; i++)
        {
            out[i] = in[i] * b[0];
            for (j = 0; j < order; j++)
            {
                out[i] += in[i-1-j] * b[j+1] - out[i-1-j] * a[j+1];
            }
        }

        for (i=0; i<order; i++)
        {
            mem[i] = in[N - order + i];
            mem[i+order] = out[N - order + i];
        }
    }
    else if (denord < numord)
    {
        for (i = 0; i < N; i++)
        {
            ptr = ptr_start;
            out[i] = *ptr + b[0] * in[i];
            for (j = 1; j <= denord; j++, ptr--)
            {
                *ptr = ptr[-1] + b[j] * in[i] - a[j] * out[i];
            }
            for (; j < numord; j++, ptr--)
            {
                *ptr = ptr[-1] + b[j] * in[i];
            }
            *ptr = b[j] * in[i];
        }
    }
    else
    {
        for (i = 0; i < N; i++)
        {
            ptr = ptr_start;
            out[i] = *ptr + b[0] * in[i];
            for (j = 1; j <= numord; j++, ptr--)
            {
                *ptr = ptr[-1] + b[j] * in[i] - a[j] * out[i];
            }
            for (; j < denord; j++, ptr--)
            {
                *ptr = ptr[-1] - a[j] * out[i];
            }
            *ptr = -a[j] * out[i];
        }
    }

    return;
}


static
float fleft_shift(float input, int shift)
{
    return (input * (float) pow(2.0, (double) shift));
}

static
float fright_shift(float input, int shift)
{
    return (input * (float) pow(0.5, (double) shift));
}



/*--------------------------------------------------------------------------------*
 * root_a()
 *
 * Implements a quadratic approximation to sqrt(a)
 * Firstly, a is normalized to lie between 0.25 & 1.0
 * by shifting the input left or right by an even number of
 * shifts. Even shifts represent powers of 4 which, after
 * the sqrt, can easily be converted to powers of 2 and are
 * easily dealt with.
 * At the heart of the algorithm is a quadratic
 * approximation of the curve sqrt(a) for 0.25 <= a <= 1.0.
 * Sqrt(a) approx = 0.27 + 1.0127 * a - 0.2864 * a^2
 *
 *---------------------------------------------------------------------------------*/

float root_a(float a)
{
    short int shift_a;
    float mod_a;
    float approx;

    if (a <= 0.0f)
    {
        return 0.0;
    }


    /* This next piece of code implements a "norm" function */
    /* and returns the shift needed to scale "a" to have a  */
    /* 1 in the (MSB-1) position. This is equivalent to     */
    /* giving a value between 0.5 & 1.0.                    */
    mod_a = a;

    shift_a = 0;
    while (mod_a > 1.0)
    {
        mod_a /= 2.0;
        shift_a--;
    }

    while (mod_a < 0.5)
    {
        mod_a *= 2.0;
        shift_a++;
    }


    shift_a &= 0xfffe;
    mod_a = fleft_shift(a, (int) shift_a);

    approx = 0.27f + 1.0127f * mod_a - 0.2864f * mod_a * mod_a;

    approx = fright_shift(approx, (int) (shift_a >> 1));

    return (approx);
}

/*--------------------------------------------------------------------------------*
 * root_a_over_b()
 *
 * Implements an approximation to sqrt(a/b)
 * Firstly a & b are normalized to lie between 0.25 & 1.0
 * by shifting the inputs left or right by an even number
 * of shifts.
 * Even shifts represent powers of 4 which, after the sqrt,
 * become powers of 2 and are easily dealt with.
 * At the heart of the algorithm is an approximation of the
 * curve sqrt(a/b) for 0.25 <= a <= 1.0 & 0.25 <= b <= 1.0.
 * Given the value of b, the 2nd order coefficients p0, p1
 * & p2 can be determined so that...
 * Sqrt(a/b) approx = p0 + p1 * a + p2 * a^2
 * where p0 approx =  0.7176 - 0.8815 * b + 0.4429 * b^2
 *       p1 approx =  2.6908 - 3.3056 * b + 1.6608 * b^2
 *       p2 approx = -0.7609 + 0.9346 * b - 0.4695 * b^2
 *
 *---------------------------------------------------------------------------------*/

float root_a_over_b(float a, float b)
{
    short int shift_a, shift_b, shift;
    float mod_a, mod_b;
    float p2 = -0.7609f;
    float p1 = 2.6908f;
    float p0 = 0.7176f;
    float b_sqr;
    float approx;

    if ((a <= 0.0f) || (b <= 0.0f))
    {
        return 0.0;
    }

    a += 0x00000001;
    b += 0x00000001;

    /* This next piece of code implements a "norm" function */
    /* and returns the shift needed to scale "a" to have a  */
    /* 1 in the (MSB-1) position. This is equivalent to     */
    /* giving a value between 0.5 & 1.0.                    */
    mod_a = a;

    shift_a = 0;
    while (mod_a > 1.0)
    {
        mod_a /= 2.0;
        shift_a--;
    }

    while (mod_a < 0.5)
    {
        mod_a *= 2.0;
        shift_a++;
    }

    shift_a &= 0xfffe;
    mod_a = fleft_shift(a, (int) shift_a);

    /* This next piece of code implements a "norm" function */
    /* and returns the shift needed to scale "b" to have a  */
    /* 1 in the (MSB-1) position. This is equivalent to     */
    /* giving a value between 0.5 & 1.0.                    */
    mod_b = b;

    shift_b = 0;
    while (mod_b > 1.0)
    {
        mod_b /= 2.0;
        shift_b--;
    }

    while (mod_b < 0.5)
    {
        mod_b *= 2.0;
        shift_b++;
    }

    shift_b &= 0xfffe;
    mod_b = fleft_shift(b, (int) shift_b);

    shift = (shift_b - shift_a) >> 1;

    b_sqr = mod_b * mod_b;

    p2 += 0.9346f * mod_b + -0.4695f * b_sqr;
    p1 += -3.3056f * mod_b + 1.6608f * b_sqr;
    p0 += -0.8815f * mod_b + 0.4429f * b_sqr;

    approx = p0 + p1 * mod_a + p2 * mod_a * mod_a;

    approx = fleft_shift(approx, (int) shift);

    return (approx);
}

/*--------------------------------------------------------------------------------*
 * rint_new()
 *
 * Round to the nearest integer with mid-point exception
 *---------------------------------------------------------------------------------*/

double rint_new(
    double x
)
{
    int a;

    /* middle value point test */
    if (ceil (x + 0.5) == floor (x + 0.5))
    {
        a = (int) ceil (x);

        if (a % 2 == 0)
        {
            return ceil (x);
        }
        else
        {
            return floor (x);
        }
    }
    else
    {
        return floor (x + 0.5);
    }
}


/*-------------------------------------------------------------------*
 * anint()
 *
 * Round to the nearest integer.
 *-------------------------------------------------------------------*/

double anint(
    double x
)
{
    return (x)>=0?(int)((x)+0.5):(int)((x)-0.5);
}

/*-------------------------------------------------------------------*
 * is_numeric_float()
 *
 * Returns 0 for all NaN and Inf values defined according to IEEE 754
 * floating point number's definition. Returns 1 for numeric values.
 *-------------------------------------------------------------------*/
short is_numeric_float(
    float x
)
{
    return ( ((*((int *)&x)) & 0x7f800000) != 0x7f800000 );
}