/*
vox - a musical real-time vocoder. version 1.0
Copyright (C) 2000  Simon MORLAT (simon.morlat@free.fr)

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
*/

#include "tables.h"
#include "lpc.h"
#define THRES 0.06
#define Dmin 10.81e-3

/* Levinson Durbin algorithm for computing LPC coefficients using
autocorrelation fonction */
void lev_durb(double *corr,double *lpc_coef)
{
    double k[11],tab[11];
    double err,acc;
    int i,j;
    double *a=tab;
    double *prev_a=lpc_coef;
    double *exch;


    /*init vectors*/
    for (i=0; i<11; i++)
    {
        prev_a[i]=0;
        a[i]=0;
    };
    err=corr[0];
    for(i=1; i<11; i++)
    {
        prev_a[0]=1;
        acc=0;
        for(j=0; j<i; j++)
        {
            acc=acc+prev_a[j]*corr[i-j];
        };
        a[i]=k[i]=-acc/err;
        for(j=1; j<i; j++)
        {
            a[j]=prev_a[j]+k[i]*prev_a[i-j];
        };
        err=(1-k[i]*k[i])*err;
        exch=prev_a;
        prev_a=a;
        a=exch;
    };
}

void comp_lpc(double *buf_x,double *corr,double *lpc_coef,int n)
{
    double buffer[n*3];
    double acc,max=0;
    int i,j;
    /* computes LPC analysis for one subframe */
    /* hamming windowing*/
    acc=0;
    for(i=0; i<2*n; i++)
    {
        acc+=buf_x[i]*buf_x[i];
    };
    if (acc>THRES)
    {
        for(i=0; i<3*n; i++)
        {
            buffer[i]=buf_x[i-n]*HammingWindowTable[i];
        };
        /* autocorrelation computation*/
        for(i=0; i<11; i++)
        {
            acc=0;
            for(j=i; j<n*3; j++)
            {
                acc=acc+buffer[j]*buffer[j-i];
            };
            /* correction with binomial coeffs */
            corr[i]=acc;//*BinomialWindowTable[i];
        };
        corr[0]=corr[0]*(1.0+1.0/1024.0);
        lev_durb(corr,lpc_coef);
    }
    else
    {
        for(i=0; i<11; i++)
        {
            lpc_coef[i]=0;
        };
    }
}



/* LPC to LSP coefficients conversion */

/* evaluate  function C(x) (whose roots are  LSP coeffs)*/

double evalc(double cw,double *fonc)
{
    double b[7];
    double x,res;
    int k;

    x=cw;
    b[5]=1;
    b[6]=0;
    for(k=4; k>0; k--)
    {
        b[k]=2*x*b[k+1]-b[k+2]+fonc[5-k];
    };
    res=x*b[1]-b[2]+fonc[5]/2;
    return(res);
}



/* converts LPC vector into LSP frequency vector */
/* all LSP frenquencies are in [0;PI] but are normalized to be in [0;1] */
void lpc2lsp(double lpc_coef[],double *f1,double *f2,double lsp_coef[])
{

    int i,k=1;
    double *fonc,*prev_f,*f_exch;
    double prev_sign1,sign,prev_sign2;
    double *s, *prev_s,*s_exch;
    double lpc_exp[11];

    /* first computes an additional bandwidth expansion on LPC coeffs*/
    for(i=1; i<11; i++)
    {
        lpc_exp[i]=lpc_coef[i]*BandExpTable[i];
    };
    /* computes the F1 and F2 coeffs*/
    f1[0]=f2[0]=1;
    for(i=0; i<5; i++)
    {
        f1[i+1]=lpc_exp[i+1]+lpc_exp[10-i]-f1[i];
        f2[i+1]=lpc_exp[i+1]-lpc_exp[10-i]+f2[i];
    };

    /*find the roots of C(x) alternatively for F1 and F2*/
    fonc=f1;
    prev_f=f2;
    prev_sign1=evalc(1.0,f1);
    prev_sign2=evalc(1.0,f2);
    s=&prev_sign1;
    prev_s=&prev_sign2;
    for(i=1; i<256; i++)
    {
        sign=evalc(CosineTable[i],fonc);
        if ((sign)*(*s)<0)
        {
            /* interpolate to find root*/
            lsp_coef[k]=((double)i-(*s)/(sign-(*s)))/256.0;
            k++;
            /* chek if all roots are found */
            if (k==11) i=257;
            (*s)=sign;
            /* pointers exchange  */
            s_exch=s;
            s=prev_s;
            prev_s=s_exch;
            f_exch=fonc;
            fonc=prev_f;
            prev_f=f_exch;
        }
        else (*s)=sign;
    }
    /* if here all roots are not found , use  lspDC vector */
    if (k!=11)
    {
        for(i=1; i<11; i++)
        {
            lsp_coef[i]=LspDcTable[i];
        };
    };
}


/* converts lsp frequencies to lpc coeffs */

void lsp2lpc(double *lsp_coef,double *lpc_coef)
{
    int i,j=0,index,ok=1;
    double lspcos[11],delta,tmp,p_avg;
    double F1[12],F2[12]; /* begin at indice two*/

    F1[0]=0;
    F1[1]=1;
    F2[0]=0;
    F2[1]=1;
    /* stability check */
    while(ok && (j<11))
    {
        ok=0;
        for(i=1; i<10; i++)
        {
            if( (lsp_coef[i+1]-lsp_coef[i]) < Dmin)
            {
                ok=1;
                p_avg=(lsp_coef[i]+lsp_coef[i+1])/2.0;
                lsp_coef[i]=p_avg-Dmin/2.0;
                lsp_coef[i+1]=p_avg+Dmin/2.0;
            };
        };
        j++;
    }

    /* first converts lsp frequencies to lsp coefficients */
    for (i=1; i<11; i++)
    {
        /* interpolation */
        tmp=lsp_coef[i]*255.0;
        index=(int)tmp;
        delta=CosineTable[index+1]-CosineTable[index];
        lspcos[i]=CosineTable[index]+delta*(tmp-index);
    };

    for(i=2; i<7; i++)
    {
        F1[i]=-2*lspcos[2*i-3]*F1[i-1]+2*F1[i-2];
        F2[i]=-2*lspcos[2*i-2]*F2[i-1]+2*F2[i-2];
        for(j=i-1; j>1; j--)
        {
            F1[j]=F1[j]-2*lspcos[2*i-3]*F1[j-1]+F1[j-2];
            F2[j]=F2[j]-2*lspcos[2*i-2]*F2[j-1]+F2[j-2];
        };
    };
    for(i=6; i>1; i--)
    {
        F1[i]=F1[i]+F1[i-1];
        F2[i]=F2[i]-F2[i-1];
    };
    for(i=2; i<7; i++)
    {
        lpc_coef[i-1]=(F1[i]+F2[i])*0.5;
        lpc_coef[i+4]=(F1[8-i]-F2[8-i])*0.5;
    };
    lpc_coef[0]=1;
}