/*
 ** Copyright 2003-2010, VisualOn, Inc.
 **
 ** 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.
 */

/***********************************************************************
*  File: az_isp.c
*
*  Description:
*-----------------------------------------------------------------------*
* Compute the ISPs from  the LPC coefficients  (order=M)                *
*-----------------------------------------------------------------------*
*                                                                       *
* The ISPs are the roots of the two polynomials F1(z) and F2(z)         *
* defined as                                                            *
*               F1(z) = A(z) + z^-m A(z^-1)                             *
*  and          F2(z) = A(z) - z^-m A(z^-1)                             *
*                                                                       *
* For a even order m=2n, F1(z) has M/2 conjugate roots on the unit      *
* circle and F2(z) has M/2-1 conjugate roots on the unit circle in      *
* addition to two roots at 0 and pi.                                    *
*                                                                       *
* For a 16th order LP analysis, F1(z) and F2(z) can be written as       *
*                                                                       *
*   F1(z) = (1 + a[M])   PRODUCT  (1 - 2 cos(w_i) z^-1 + z^-2 )         *
*                        i=0,2,4,6,8,10,12,14                           *
*                                                                       *
*   F2(z) = (1 - a[M]) (1 - z^-2) PRODUCT (1 - 2 cos(w_i) z^-1 + z^-2 ) *
*                                 i=1,3,5,7,9,11,13                     *
*                                                                       *
* The ISPs are the M-1 frequencies w_i, i=0...M-2 plus the last         *
* predictor coefficient a[M].                                           *
*-----------------------------------------------------------------------*

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

#include "typedef.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "stdio.h"
#include "grid100.tab"

#define M   16
#define NC  (M/2)

/* local function */
static __inline Word16 Chebps2(Word16 x, Word16 f[], Word32 n);

void Az_isp(
        Word16 a[],                           /* (i) Q12 : predictor coefficients                 */
        Word16 isp[],                         /* (o) Q15 : Immittance spectral pairs              */
        Word16 old_isp[]                      /* (i)     : old isp[] (in case not found M roots)  */
       )
{
    Word32 i, j, nf, ip, order;
    Word16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
    Word16 x, y, sign, exp;
    Word16 *coef;
    Word16 f1[NC + 1], f2[NC];
    Word32 t0;
    /*-------------------------------------------------------------*
     * find the sum and diff polynomials F1(z) and F2(z)           *
     *      F1(z) = [A(z) + z^M A(z^-1)]                           *
     *      F2(z) = [A(z) - z^M A(z^-1)]/(1-z^-2)                  *
     *                                                             *
     * for (i=0; i<NC; i++)                                        *
     * {                                                           *
     *   f1[i] = a[i] + a[M-i];                                    *
     *   f2[i] = a[i] - a[M-i];                                    *
     * }                                                           *
     * f1[NC] = 2.0*a[NC];                                         *
     *                                                             *
     * for (i=2; i<NC; i++)            Divide by (1-z^-2)          *
     *   f2[i] += f2[i-2];                                         *
     *-------------------------------------------------------------*/
    for (i = 0; i < NC; i++)
    {
        t0 = a[i] << 15;
        f1[i] = vo_round(t0 + (a[M - i] << 15));        /* =(a[i]+a[M-i])/2 */
        f2[i] = vo_round(t0 - (a[M - i] << 15));        /* =(a[i]-a[M-i])/2 */
    }
    f1[NC] = a[NC];
    for (i = 2; i < NC; i++)               /* Divide by (1-z^-2) */
        f2[i] = add1(f2[i], f2[i - 2]);

    /*---------------------------------------------------------------------*
     * Find the ISPs (roots of F1(z) and F2(z) ) using the                 *
     * Chebyshev polynomial evaluation.                                    *
     * The roots of F1(z) and F2(z) are alternatively searched.            *
     * We start by finding the first root of F1(z) then we switch          *
     * to F2(z) then back to F1(z) and so on until all roots are found.    *
     *                                                                     *
     *  - Evaluate Chebyshev pol. at grid points and check for sign change.*
     *  - If sign change track the root by subdividing the interval        *
     *    2 times and ckecking sign change.                                *
     *---------------------------------------------------------------------*/
    nf = 0;                                  /* number of found frequencies */
    ip = 0;                                  /* indicator for f1 or f2      */
    coef = f1;
    order = NC;
    xlow = vogrid[0];
    ylow = Chebps2(xlow, coef, order);
    j = 0;
    while ((nf < M - 1) && (j < GRID_POINTS))
    {
        j ++;
        xhigh = xlow;
        yhigh = ylow;
        xlow = vogrid[j];
        ylow = Chebps2(xlow, coef, order);
        if ((ylow * yhigh) <= (Word32) 0)
        {
            /* divide 2 times the interval */
            for (i = 0; i < 2; i++)
            {
                xmid = (xlow >> 1) + (xhigh >> 1);        /* xmid = (xlow + xhigh)/2 */
                ymid = Chebps2(xmid, coef, order);
                if ((ylow * ymid) <= (Word32) 0)
                {
                    yhigh = ymid;
                    xhigh = xmid;
                } else
                {
                    ylow = ymid;
                    xlow = xmid;
                }
            }
            /*-------------------------------------------------------------*
             * Linear interpolation                                        *
             *    xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow);            *
             *-------------------------------------------------------------*/
            x = xhigh - xlow;
            y = yhigh - ylow;
            if (y == 0)
            {
                xint = xlow;
            } else
            {
                sign = y;
                y = abs_s(y);
                exp = norm_s(y);
                y = y << exp;
                y = div_s((Word16) 16383, y);
                t0 = x * y;
                t0 = (t0 >> (19 - exp));
                y = vo_extract_l(t0);         /* y= (xhigh-xlow)/(yhigh-ylow) in Q11 */
                if (sign < 0)
                    y = -y;
                t0 = ylow * y;      /* result in Q26 */
                t0 = (t0 >> 10);        /* result in Q15 */
                xint = vo_sub(xlow, vo_extract_l(t0));        /* xint = xlow - ylow*y */
            }
            isp[nf] = xint;
            xlow = xint;
            nf++;
            if (ip == 0)
            {
                ip = 1;
                coef = f2;
                order = NC - 1;
            } else
            {
                ip = 0;
                coef = f1;
                order = NC;
            }
            ylow = Chebps2(xlow, coef, order);
        }
    }
    /* Check if M-1 roots found */
    if(nf < M - 1)
    {
        for (i = 0; i < M; i++)
        {
            isp[i] = old_isp[i];
        }
    } else
    {
        isp[M - 1] = a[M] << 3;                      /* From Q12 to Q15 with saturation */
    }
    return;
}

/*--------------------------------------------------------------*
* function  Chebps2:                                           *
*           ~~~~~~~                                            *
*    Evaluates the Chebishev polynomial series                 *
*--------------------------------------------------------------*
*                                                              *
*  The polynomial order is                                     *
*     n = M/2   (M is the prediction order)                    *
*  The polynomial is given by                                  *
*    C(x) = f(0)T_n(x) + f(1)T_n-1(x) + ... +f(n-1)T_1(x) + f(n)/2 *
* Arguments:                                                   *
*  x:     input value of evaluation; x = cos(frequency) in Q15 *
*  f[]:   coefficients of the pol.                      in Q11 *
*  n:     order of the pol.                                    *
*                                                              *
* The value of C(x) is returned. (Satured to +-1.99 in Q14)    *
*                                                              *
*--------------------------------------------------------------*/

static __inline Word16 Chebps2(Word16 x, Word16 f[], Word32 n)
{
    Word32 i, cheb;
    Word16 b0_h, b0_l, b1_h, b1_l, b2_h, b2_l;
    Word32 t0;

    /* Note: All computation are done in Q24. */

    t0 = f[0] << 13;
    b2_h = t0 >> 16;
    b2_l = (t0 & 0xffff)>>1;

    t0 = ((b2_h * x)<<1) + (((b2_l * x)>>15)<<1);
    t0 <<= 1;
    t0 += (f[1] << 13);                     /* + f[1] in Q24        */

    b1_h = t0 >> 16;
    b1_l = (t0 & 0xffff) >> 1;

    for (i = 2; i < n; i++)
    {
        t0 = ((b1_h * x)<<1) + (((b1_l * x)>>15)<<1);

        t0 += (b2_h * (-16384))<<1;
        t0 += (f[i] << 12);
        t0 <<= 1;
        t0 -= (b2_l << 1);                  /* t0 = 2.0*x*b1 - b2 + f[i]; */

        b0_h = t0 >> 16;
        b0_l = (t0 & 0xffff) >> 1;

        b2_l = b1_l;                         /* b2 = b1; */
        b2_h = b1_h;
        b1_l = b0_l;                         /* b1 = b0; */
        b1_h = b0_h;
    }

    t0 = ((b1_h * x)<<1) + (((b1_l * x)>>15)<<1);
    t0 += (b2_h * (-32768))<<1;             /* t0 = x*b1 - b2          */
    t0 -= (b2_l << 1);
    t0 += (f[n] << 12);                     /* t0 = x*b1 - b2 + f[i]/2 */

    t0 = L_shl2(t0, 6);                     /* Q24 to Q30 with saturation */

    cheb = extract_h(t0);                  /* Result in Q14              */

    if (cheb == -32768)
    {
        cheb = -32767;                     /* to avoid saturation in Az_isp */
    }
    return (cheb);
}