
/*

This software module was originally developed by

    Masayuki Nishiguchi and Kazuyuki Iijima (Sony Corporation)

    in the course of development of the MPEG-4 Audio standard (ISO/IEC 14496-3).
    This software module is an implementation of a part of one or more
    MPEG-4 Audio (ISO/IEC 14496-3) tools as specified by the MPEG-4 Audio
    standard (ISO/IEC 14496-3).
    ISO/IEC gives users of the MPEG-4 Audio standards (ISO/IEC 14496-3)
    free license to this software module or modifications thereof for use
    in hardware or software products claiming conformance to the MPEG-4
    Audio standards (ISO/IEC 14496-3).
    Those intending to use this software module in hardware or software
    products are advised that this use may infringe existing patents.
    The original developer of this software module and his/her company,
    the subsequent editors and their companies, and ISO/IEC have no
    liability for use of this software module or modifications thereof in
    an implementation.
    Copyright is not released for non MPEG-4 Audio (ISO/IEC 14496-3)
    conforming products. The original developer retains full right to use
    the code for his/her own purpose, assign or donate the code to a third
    party and to inhibit third party from using the code for non MPEG-4
    Audio (ISO/IEC 14496-3) conforming products.
    This copyright notice must be included in all copies or derivative works.

    Copyright (c)1996.

*/

#include <math.h>
#include <stdio.h>

#include "hvxc.h"
#include "hvxcDec.h"
#include "hvxcCommon.h"

#define PU_IP  2 

extern int	ipc_encDelayMode;
extern int	ipc_decDelayMode;

static void calc_syn_cont2u_LD(
float   *res,
float   (*alphaip)[11],
float   *syn)
{
    int i, j;
	

    float out2;
    static float mem2[P+1]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
		    
    float pos[FRM];
   
    for(i = 0; i < FRM; i++)    pos[i] = 0.0;


       for(i = 0; i < FRM/2 - LD_LEN; i++)
       {
   	   out2 = 0.;
	   for(j=P;j>0;j--)
	   {
		   out2 += mem2[j] * alphaip[0][j];
		   mem2[j]=mem2[j-1];
	    }

	    out2 = res[i] - out2;
            mem2[1]= out2;
            pos[i]= out2;
       }


       for(i = FRM/2 - LD_LEN ; i < FRM; i++)
       {
	   out2 = 0.;
	   for(j=P;j>0;j--)
	   {
	       out2 += mem2[j] * alphaip[1][j];
	       mem2[j]=mem2[j-1];
	   }

	   out2 = res[i] - out2;
	   mem2[1]= out2;
	   pos[i]= out2;
       }

       for(i=0;i<FRM;i++)
       {
           syn[i] =  pos[i] ;
       }
}

static void calc_syn_cont2u(
float	*res,
float	(*alphaip)[11],
float	*syn)
{
    int i, j, ii;
    
    float out2,out2old;
    static float mem2[P+1]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
    float mem2old[P+1];
    
    float pos[FRM];
    float nokori[80];
    static float alphaipold[P+1];
    float window[80];
    
    for(i=0;i<40;i++)
	window[i]=(float)(i)/40.;
    for(i=40;i<80;i++)
	window[i]=1.;
    
    for(i = 0; i < P + 1; i++)	mem2old[i] = 0.0;
    for(i = 0; i < FRM; i++)	pos[i] = 0.0;
    for(i = 0; i < 80; i++)	nokori[i] = 0.0;
    
    
    for(ii=0;ii<2;ii++)
    {
	for(i=0;i<=P;i++)
	    mem2old[i]=mem2[i];
	
	for(i = 0; i < 80; i++)
	{
	    out2 = 0.;
	    out2old = 0.;
	    for(j=P;j>0;j--)
	    {
		out2 += mem2[j] * alphaip[ii][j];
		mem2[j]=mem2[j-1];
		out2old += mem2old[j]*alphaipold[j];
		mem2old[j]=mem2old[j-1];
	    }
	    
	    out2 = res[ii*80+i] - out2;
	    mem2[1]= out2;
	    pos[ii*80+i]= out2;
	    out2old = res[ii*80+i] - out2old;
	    mem2old[1]= out2old;
	    nokori[i]= out2old;
	}
	
	for(i=0;i<80;i++)
	{
	    syn[ii*80+i] =  pos[ii*80+i] ;
	}
	
	for(i=0;i<=P;i++)
	    alphaipold[i]=alphaip[ii][i];
    }
}

static void IpLsp2Lpc(
float	(*qLsp)[10],
float	(*alphaip)[11])     	                  
{
    int		i, j;
    float	ipLsp0[P + 1], ipLsp1[P + 1];
    
    ipLsp0[0] = ipLsp1[0] = 0.0;
    for(i = 0; i < P; i++)
    {
	ipLsp0[i + 1] = qLsp[0][i];
	ipLsp1[i + 1] = qLsp[1][i];
    }
    
    IPC_lsp_lpc(ipLsp0, alphaip[0]);
    IPC_lsp_lpc(ipLsp1, alphaip[1]);
    
    for(j = 0; j < 2; j++)
    {
	alphaip[j][0] = 1.0;
	for(i = 1; i < P + 1; i++)
	{
	    alphaip[j][i] = -alphaip[j][i];
	}
    }
    return;
}    


static void posfil_u_LD(
float	*syn,
float	(*alphaip)[11])
{
    int i,j,ii;
    float out,outold;
    static float mem1[P+1]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
    float mem1old[P+1];
    
    float out2,out2old;
    static float mem2[P+1]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
    float mem2old[P+1];
    
    float alphaipFIR[PU_IP][P+1];
    float alphaipIIR[PU_IP][P+1];
    
    float resi[FRM];
    float pos[FRM];
    float nokori[FRM];
    float synhp[FRM];
    static float hpm=0.;
    float gain;
    static float oldgain;
    static float alphaipFIRold[P+1];
    static float alphaipIIRold[P+1];
    float window[FRM];
    float sp,op;
    float spn,opn;
    float fac05[11],fac08[11];
    int  subf_len[2], slen;

    fac05[0]=1.0;
    fac05[1]=0.5;
    fac05[2]=0.25;
    fac05[3]=0.125;
    fac05[4]=0.0625;
    fac05[5]=0.03125;
    fac05[6]=0.015625;
    fac05[7]=0.0078125;
    fac05[8]=0.00390625;
    fac05[9]=0.001953125;
   fac05[10]=0.0009765625;

    fac08[0]=1.0;
    fac08[1]=0.8;
    fac08[2]=0.64;
    fac08[3]=0.512;
    fac08[4]=0.4096;
    fac08[5]=0.32768;
    fac08[6]=0.262144;
    fac08[7]=0.2097152;
    fac08[8]=0.16777216;
    fac08[9]=0.134217728;
   fac08[10]=0.107374182;


    subf_len[0]=FRM/2 - LD_LEN;
    subf_len[1]=FRM/2 + LD_LEN;

    for(i = 0; i < 20; i++)
	window[i] = (float) i / 20.;
    for(i = 20 ; i < FRM; i++)
	window[i]=1.;

    for(j=0;j<PU_IP;j++)
    {
	for(i=0;i<=P;i++)
	{
	    alphaipFIR[j][i]=alphaip[j][i]*fac05[i];
	    alphaipIIR[j][i]=alphaip[j][i]*fac08[i];
	}
    }
    
    slen=0;
    for(ii=0;ii<PU_IP;ii++){

	for(i=0;i<subf_len[ii];i++){
	    synhp[i]=syn[slen+i]- 0.1*hpm;
	    hpm=syn[slen+i];
	}
	for(i=0;i<=P;i++)
	    mem1old[i]=mem1[i];
	
	for(i=0;i<subf_len[ii];i++){  
	    mem1[0]=synhp[i];
	    mem1old[0]=synhp[i];
	    out = 0.;
	    outold = 0.;
	    for(j=P;j>0;j--){
		out += mem1[j]*alphaipFIR[ii][j];
		mem1[j]=mem1[j-1];
		outold += mem1old[j]*alphaipFIRold[j];
		mem1old[j]=mem1old[j-1];
	    }
	    out += mem1[0];
	    outold += mem1old[0];
	    resi[slen+i]= out;
	    nokori[slen+i] = outold;
	    
	}
	
	for(i=0;i<=P;i++)
	    mem2old[i]=mem2[i];
	
	for(i=0;i<subf_len[ii];i++){  
	    out2 = 0.;
	    out2old = 0.;
	    for(j=P;j>0;j--){
		out2 += mem2[j]*alphaipIIR[ii][j];
		out2old += mem2old[j]*alphaipIIRold[j];
		mem2[j]=mem2[j-1];
		mem2old[j]=mem2old[j-1];
	    }
	    out2 = resi[slen+i] - out2;
	    out2old = nokori[slen+i] - out2old;
	    mem2[1]= out2;
	    mem2old[1]= out2old;
	    pos[slen+i]= out2;
	    nokori[slen+i]= out2old;
	}
	
	for(i=0;i<=P;i++){
	    alphaipFIRold[i]=alphaipFIR[ii][i];
	    alphaipIIRold[i]=alphaipIIR[ii][i];
	}
	
    slen = slen + subf_len[ii];
    }
    
    sp = op = spn = opn = 0.;
    for(i=0;i<FRM;i++){
	sp += syn[i]*syn[i] ;
	op += pos[i]*pos[i] ;
	
    }
    
    if(op >= 2560)
	gain = sqrt(sp/op);
    else
	gain =0.;
    
    for(i=0;i<FRM;i++)
	syn[i] = gain * pos[i] * window[i] + oldgain * nokori[i] * (1.0-window[i]) ;  
    
    oldgain=gain;
}


static void posfil_u(
float	*syn,
float	(*alphaip)[11])
{
    int i,j,ii;
    float out,outold;
    static float mem1[P+1]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
    float mem1old[P+1];
    
    float out2,out2old;
    static float mem2[P+1]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
    float mem2old[P+1];
    
    float alphaipFIR[PU_IP][P+1];
    float alphaipIIR[PU_IP][P+1];
    
    float resi[FRM];
    float pos[FRM];
    float nokori[FRM];
    float synhp[FRM / PU_IP];
    static float hpm=0.;
    float gain;
    static float oldgain;
    static float alphaipFIRold[P+1];
    static float alphaipIIRold[P+1];
    float window[FRM];
    float sp,op;
    float spn,opn;
    float fac05[11],fac08[11];
    
    fac05[0]=1.0;
    fac05[1]=0.5;
    fac05[2]=0.25;
    fac05[3]=0.125;
    fac05[4]=0.0625;
    fac05[5]=0.03125;
    fac05[6]=0.015625;
    fac05[7]=0.0078125;
    fac05[8]=0.00390625;
    fac05[9]=0.001953125;
   fac05[10]=0.0009765625;

    fac08[0]=1.0;
    fac08[1]=0.8;
    fac08[2]=0.64;
    fac08[3]=0.512;
    fac08[4]=0.4096;
    fac08[5]=0.32768;
    fac08[6]=0.262144;
    fac08[7]=0.2097152;
    fac08[8]=0.16777216;
    fac08[9]=0.134217728;
   fac08[10]=0.107374182;

    for(i = 0; i < 20; i++)
	window[i] = (float) i / 20.;
    for(i = 20 ; i < FRM; i++)
	window[i]=1.;

    for(j=0;j<PU_IP;j++)
    {
	for(i=0;i<=P;i++)
	{
	    alphaipFIR[j][i]=alphaip[j][i]*fac05[i];
	    alphaipIIR[j][i]=alphaip[j][i]*fac08[i];
	}
    }
    
    for(ii=0;ii<PU_IP;ii++){
	for(i=0;i<FRM/PU_IP;i++){
	    synhp[i]=syn[ii*FRM/PU_IP+i]- 0.1*hpm;
	    hpm=syn[ii*FRM/PU_IP+i];
	}
	for(i=0;i<=P;i++)
	    mem1old[i]=mem1[i];
	
	for(i=0;i<FRM/PU_IP;i++){  
	    mem1[0]=synhp[i];
	    mem1old[0]=synhp[i];
	    out = 0.;
	    outold = 0.;
	    for(j=P;j>0;j--){
		out += mem1[j]*alphaipFIR[ii][j];
		mem1[j]=mem1[j-1];
		outold += mem1old[j]*alphaipFIRold[j];
		mem1old[j]=mem1old[j-1];
	    }
	    out += mem1[0];
	    outold += mem1old[0];
	    resi[ii*FRM/PU_IP+i]= out;
	    nokori[ii*FRM/PU_IP+i] = outold;
	    
	}
	
	for(i=0;i<=P;i++)
	    mem2old[i]=mem2[i];
	
	for(i=0;i<FRM/PU_IP;i++){  
	    out2 = 0.;
	    out2old = 0.;
	    for(j=P;j>0;j--){
		out2 += mem2[j]*alphaipIIR[ii][j];
		out2old += mem2old[j]*alphaipIIRold[j];
		mem2[j]=mem2[j-1];
		mem2old[j]=mem2old[j-1];
	    }
	    out2 = resi[ii*FRM/PU_IP+i] - out2;
	    out2old = nokori[ii*FRM/PU_IP+i] - out2old;
	    mem2[1]= out2;
	    mem2old[1]= out2old;
	    pos[ii*FRM/PU_IP+i]= out2;
	    nokori[ii*FRM/PU_IP+i]= out2old;
	}
	
	for(i=0;i<=P;i++){
	    alphaipFIRold[i]=alphaipFIR[ii][i];
	    alphaipIIRold[i]=alphaipIIR[ii][i];
	}
	
    }
    
    sp = op = spn = opn = 0.;
    for(i=0;i<FRM;i++){
	sp += syn[i]*syn[i] ;
	op += pos[i]*pos[i] ;
	
    }
    
    if(op >= 2560)
	gain = sqrt(sp/op);
    else
	gain =0.;
    
    for(i=0;i<FRM;i++)
	syn[i] = gain * pos[i] * window[i] + oldgain * nokori[i] * (1.0-window[i]) ;  
    
    oldgain=gain;
}



static void lp_synU(int *vuv, float *synoutu, float *resinu,
		     float (*qLsp)[10])
{
    int i;
    float lsp[2][P];
    float alphaip[2][P+1];
    float res[FRM];
    float syn[FRM];

    for(i=0;i<10;i++){
	lsp[0][i]=qLsp[0][i];
	lsp[1][i]=qLsp[1][i];
    }
    
    IpLsp2Lpc(lsp, alphaip);
    
    for(i = 0; i < 160; i++)
	res[i] = resinu[i];
    
    if(ipc_decDelayMode == DM_SHORT)
    {
	calc_syn_cont2u_LD(res, alphaip, syn);
	posfil_u_LD(syn,alphaip);
    }
    else
    {
	calc_syn_cont2u(res, alphaip, syn);
	posfil_u(syn,alphaip);
    }	
    
    for(i = 0; i < 160; i++)
	synoutu[i] = syn[i];
}

void td_synt(
float	*qRes,
int	*VUVs,
float	(*qLsp)[10],
float	*synoutu)
{
	float suv[FRM];

	IPC_uvExt(VUVs,suv,qRes);
	lp_synU(VUVs, synoutu, suv, qLsp);
}
