 
/*

This software module was originally developed by

    Kazuyuki Iijima, Masayuki Nishiguchi, and Shiro Omori (Sony Corporation)

    and edited by

    Akira Inoue (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 <values.h>
#include <stdio.h>

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

#define AA	0.551543
#define BB	0.152100
#define CC	0.89
#define DD	0.198025
#define GG	1.226

extern int	ipc_encMode;
extern int	ipc_decMode;


static void hpf_dec(float *bufsf)
{
    int i;
    float x3;
    static float x1old=0.,x2old=0.,x3old=0.,x4old=0.;
    
    for(i=0; i<FRM; i++){
	x3=bufsf[i]+x1old*AA + x2old*BB - ( x3old*CC + x4old*DD );
	x2old=x1old;
	x1old=bufsf[i];
	x4old=x3old;
	x3old=x3;
	bufsf[i]=x3*GG;
    }
}


typedef struct
{
    double	gainAdj;
    double	a[4], b[4];
}
CoefFltr4;

static CoefFltr4 cf4 = 
{
     1.0,

    {
        -1.998066423746901,
         1.000000000000000,
        -1.962822436245804,
         0.9684991816600951,
    },
     
   {
        -1.999633313803449,
         0.9999999999999999,
        -1.858097918647416,
        0.8654599838007603,
    }
};

typedef struct
{
    double	a[4], b[4];
}
StateFltr4;

static void hpf4ji(
float	*data,
int	size,
StateFltr4	*state,
CoefFltr4	*coef)
{
    int i;
    float x3;

    for(i = 0; i < size; i++)
    {
        x3 = data[i] + state->a[0] * coef->a[0] + state->a[1] * coef->a[1]
            - (state->a[2] * coef->a[2] + state->a[3] * coef->a[3]);
        state->a[1] = state->a[0];
        state->a[0] = data[i];
        state->a[3] = state->a[2];
        state->a[2] = x3;
        data[i] = x3;
    }
    for(i = 0; i < size; i++)
    {
        x3 = data[i] + state->b[0] * coef->b[0] + state->b[1] * coef->b[1]
            - (state->b[2] * coef->b[2] + state->b[3] * coef->b[3]);
        state->b[1] = state->b[0];
        state->b[0] = data[i];
        state->b[3] = state->b[2];
        state->b[2] = x3;
        data[i] = x3 / coef->gainAdj;
    }
}



#define AL      -2. * 1. * cos((4.0/4.0)*M_PI)
#define BBL     1.
#define CL      -2. * 0.78 * cos((3.55/4.0)*M_PI)
#define DL      0.78*0.78
#define GL      0.768


static void lpf3400(float *bufsf)
{
    int i;
    float x3;
    static float x1old=0.,x2old=0.,x3old=0.,x4old=0.;
    
    for(i=0; i<SAMPLE-OVERLAP; i++){
	x3=bufsf[i]+x1old*AL + x2old*BBL - ( x3old*CL + x4old*DL );
	x2old=x1old;
	x1old=bufsf[i];
	x4old=x3old;
	x3old=x3;
	bufsf[i]=x3*GL;
    }
}

void IPC_bpf(float *bufsf)
{
    static StateFltr4	state;

    hpf_dec(bufsf);
    hpf4ji(bufsf, FRM, &state, &cf4);
    lpf3400(bufsf);
}


/* informative */
void IPC_posfil_v(
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[IP][P+1];
    float alphaipIIR[IP][P+1];
    
    float resi[160];
    float pos[160];
    float nokori[160];
    float synhp[20];
    static float hpm=0.;
    float gain;
    static float oldgain;
    static float alphaipFIRold[P+1];
    static float alphaipIIRold[P+1];
    float window[160];
    float sp,op;
    float spn,opn;
    float fac05[11],fac08[11];

    float emph;

    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<160;i++)
	window[i]=1.;
    

    for(j=0;j<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<IP;ii++)
    {
	emph = 0.15 * alphaip[ii][1];
	if(-0.5 > emph)
	{
	    emph = -0.5;
	}
	else if(emph > 0)
	{
	    emph = 0.0;
	}
	
	for(i=0;i<20;i++)
	{
	    synhp[i] = syn[ii*20+i] + emph * hpm;
	    hpm=syn[ii*20+i];
	}

	for(i=0;i<=P;i++)
	    mem1old[i]=mem1[i];
	
	for(i=0;i<20;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*20+i]= out;
	    nokori[ii*20+i] = outold;
	}
	
	for(i=0;i<=P;i++)
	    mem2old[i]=mem2[i];
	
	for(i=0;i<20;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*20+i] - out2;
	    out2old = nokori[ii*20+i] - out2old;
	    mem2[1]= out2;
	    mem2old[1]= out2old;
	    pos[ii*20+i]= out2;
	    nokori[ii*20+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<160;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<160;i++)
	syn[i] = gain * pos[i] * window[i] + oldgain * nokori[i] * (1.0-window[i]) ;  
    
    oldgain=gain;
}


#define PU_IP  2

void IPC_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;
}


void IPC_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;
}
