
/*

This software module was originally developed by

    Masayuki Nishiguchi and Kazuyuki Iijima (Sony Corporation)

    and edited by

    Yuji Maeda and 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 <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <values.h>

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

#include "hvxcCbAm4k.h"

#define	ENC	1
#define DEC	0

extern int	ipc_encMode;
extern int	ipc_decMode;

static float f_coef[(JISU-1)*OVER_R+1];

vqscheme_lpc_dec ipc_cvq_lpc_dec;

#define MAX_HARM       76

static vqschm4k schm4k;

void IPC_set_const_lpcVM_dec(void)
{
	int i;

	ipc_cvq_lpc_dec.numpulse = 44;

	ipc_cvq_lpc_dec.vqdim_lpc[0]=44;
	ipc_cvq_lpc_dec.vqdim_lpc[1]=44;

	ipc_cvq_lpc_dec.vqsize_lpc[0]=16;
	ipc_cvq_lpc_dec.vqsize_lpc[1]=16;
	ipc_cvq_lpc_dec.gsize_lpc[0]=32;

	for( i=1; i<= 44; i++)
		ipc_cvq_lpc_dec.leak[i]=0.0;  

        schm4k.num_vq = 4;
        schm4k.dim_tot = 14;

        schm4k.vqdim[0]=2;
        schm4k.vqdim[1]=4;
        schm4k.vqdim[2]=4;
        schm4k.vqdim[3]=4;
        schm4k.vqdim[4]=4;

        schm4k.vqsize[0]=128;
        schm4k.vqsize[1]=1024;
        schm4k.vqsize[2]=512;
        schm4k.vqsize[3]=64;
        schm4k.vqsize[4]=2;
}


static void over_s_r(float *re, float *rel, int send1, float w0f, float w0) 
{
	int i,j,p,ii;

	for(i=0;i<=(send1+1)*OVER_R;i++)
		rel[i]= 0.;  
	ii=0;
	for(i=0; i<=send1;i++){ 	
		for(p=0;p<OVER_R;p++){	
			if((i*OVER_R+p+1)*w0f > w0*ii){ 
				rel[i*OVER_R+p]=rel[i*OVER_R+p+1]=0.; 
				for(j=1;j<JISU;j++){
					if(i+j-(JISU-1)/2 >= 0){
						rel[i*OVER_R+p] += f_coef[j * OVER_R - p] * re[i+j-(JISU-1)/2];
						rel[i*OVER_R+p+1] += f_coef[j * OVER_R - (p+1)] * re[i+j-(JISU-1)/2];
						}	
					}
				ii++;
			}   
		}
	}

}



static void IpAmFIR_Deci_r(float *am1, int send1, float w01, float *am1ip, float *am2deci, float w0, int send)
{
	int i,j,lb,ub = 0,bw,idx,ii;
	float w0f;
	float re[SAMPLE];
	float rel[SAMPLE*R];
	float ap[SAMPLE*R/2];
	
	for(i=0;i<=send1;i++)
		re[i]=am1[i];
	for(i=send1+1; i<=send1+(JISU-1)/2; i++)
		re[i]=re[send1];
	w0f=w01/(float)R;
	over_s_r(re,rel,send1,w0f,w0);  
	for(i=0;i<= (send1+1) * OVER_R ;i++) 
		ap[i] = rel[i] ;		
	ii=0;
	for(i=0;i<=(send1+1)*R-1;i++){ 
		if(i==0) lb=0;
		else 	 lb=ub;
		ub=IPC_inint((float)(i+1) * w0f);
		if(ub >= SAMPLE*R/2 )
			ub=SAMPLE*R/2;	
		bw=ub-lb;
		if(bw <=0) {printf("SOMETHING WRONG!!"); am2deci[ii]=ap[i]; ii++; }
		else{
		idx=0;
		for(j=lb;j<ub;j++){
			if(j == IPC_inint((float)ii * w0)){ 
				am1ip[j]=ap[i]*(1.-(float)idx/(float)bw)+ap[i+1]*((float)idx/(float)bw);
				am2deci[ii]=am1ip[j];
				ii++;
				}
			idx++;
			if(ii > send ) break;
			}
			}
		if(ii > send ) break;
		}
}


static void dec_2st_sgvq( int vqdim0, int vqdim1, float (*cb0)[44], float (*cb1)[44], float *g0, float *qedvec, IdAm *idAm)   
{
	int l,idx_s0,idx_s1,idx_g;

	idx_s0 = idAm->idS0;
	idx_s1 = idAm->idS1;
	idx_g = idAm->idG;

	for(l=0;l<vqdim0;l++)
		qedvec[l]=g0[idx_g]*(cb0[idx_s0][l]+cb1[idx_s1][l]);

}

static void quand_lpc_dec(float *da, int sendmax, vqscheme_lpc_dec *cvq_lp, cbook_lpc_dec *cba_lp, IdAm *idAm, int voiced)
{
	float qedvec[MAXDIM3];
	int j;

	da[0]=0.; 

if(voiced){
	dec_2st_sgvq(cvq_lp->vqdim_lpc[0],cvq_lp->vqdim_lpc[1],cba_lp->cb1lpc,cba_lp->cb2lpc,cba_lp->g0lpc,qedvec,idAm); 
	}

else{
	for(j=0;j<sendmax;j++)
		qedvec[j]=0.;
	}

	for(j=1;j<=sendmax;j++)
		da[j]=qedvec[j-1];
}

static float dis(
float cb[],
float in_vec[],
float weight[],
int vqdim)
{
	int i;
	float disout;

	disout=0.;
	for(i=0;i<vqdim;i++)
		disout += (cb[i]-in_vec[i])*(cb[i]-in_vec[i])*weight[i]*weight[i];

	return(disout);

}

static void StVq(
float		in_vec[],
float		weight[],
float		qed_vec[],
cbook4k		*cb4k,
int		nBnd,
vqschm4k 	*schm4k,
int		*id,
int		ende)
{
    int j,l,idx;
    float dist,mindist = 0.0;
    /* float dis();  */
    
    if(ende == ENC)
    {
	idx = 0;
	switch(nBnd)
	{
	case 0:
	    for(j = 0; j < schm4k->vqsize[0]; j++)
	    {
		dist = dis(cb4k->cb0[j], in_vec, weight, schm4k->vqdim[0]);
		if(dist < mindist || j == 0)
		{
		    mindist = dist;
		    idx = j;
		}
	    }
	    break;
	case 1:
	    for(j = 0; j < schm4k->vqsize[1]; j++)
	    {
		dist = dis(cb4k->cb1[j], in_vec, weight, schm4k->vqdim[1]);
		if(dist < mindist || j == 0)
		{
		    mindist = dist;
		    idx = j;
		}
	    }
	    break;
	case 2:
	    for(j = 0; j < schm4k->vqsize[2]; j++)
	    {
		dist = dis(cb4k->cb2[j], in_vec, weight, schm4k->vqdim[2]);
		if(dist < mindist || j == 0)
		{
		    mindist = dist;
		    idx = j;
		}
	    }
	    break;
	case 3:
	    for(j = 0; j < schm4k->vqsize[3]; j++)
	    {
		dist = dis(cb4k->cb3[j], in_vec, weight, schm4k->vqdim[3]);
		if(dist < mindist || j == 0)
		{
		    mindist = dist;
		    idx = j;
		}
	    }
	    break;
	case 4:
	    for(j = 0; j < schm4k->vqsize[4]; j++)
	    {
		dist = dis(cb4k->cb4[j], in_vec, weight, schm4k->vqdim[4]);
		if(dist < mindist || j == 0)
		{
		    mindist = dist;
		    idx = j;
		}
	    }
	    break;
	}
	*id = idx;
	
	switch(nBnd)
	{
	case 0:
	    for(l = 0; l < schm4k->vqdim[0]; l++)
		qed_vec[l] = cb4k->cb0[idx][l];
	    break;
	case 1:
	    for(l = 0; l < schm4k->vqdim[1]; l++)
		qed_vec[l] = cb4k->cb1[idx][l];
	    break;
	case 2:
	    for(l = 0; l < schm4k->vqdim[2]; l++)
		qed_vec[l] = cb4k->cb2[idx][l];
	    break;
	case 3:
	    for(l = 0; l < schm4k->vqdim[3]; l++)
		qed_vec[l] = cb4k->cb3[idx][l];
	    break;
	case 4:
	    for(l = 0; l < schm4k->vqdim[4]; l++)
		qed_vec[l] = cb4k->cb4[idx][l];
	    break;
	}
    }
    else
    {
	idx = *id;
	
	switch(nBnd)
	{
	case 0:
	    for(l = 0; l < schm4k->vqdim[0]; l++)
		qed_vec[l] = cb4k->cb0[idx][l];
	    break;
	case 1:
	    for(l = 0; l < schm4k->vqdim[1]; l++)
		qed_vec[l] = cb4k->cb1[idx][l];
	    break;
	case 2:
	    for(l = 0; l < schm4k->vqdim[2]; l++)
		qed_vec[l] = cb4k->cb2[idx][l];
	    break;
	case 3:
	    for(l = 0; l < schm4k->vqdim[3]; l++)
		qed_vec[l] = cb4k->cb3[idx][l];
	    break;
	case 4:
	    for(l = 0; l < schm4k->vqdim[4]; l++)
		qed_vec[l] = cb4k->cb4[idx][l];
	    break;
	}
    }
}

void Quan4kDec(  
float		target[],
int		send2,   		
vqschm4k	*schm4k,
cbook4k		*cb4k,
int		id4k[])
{
    float in_vec[MAXDIM1];
    float weight[MAXDIM1];
    float qed_vec[MAXDIM1];
    int ende,k,i,j;

    ende=DEC;
    target[0]=0.;

    k=1;
    for(j=0;j<schm4k->num_vq;j++)
    {
	StVq(in_vec, weight, qed_vec, cb4k, j, schm4k, &id4k[j], ende);

	for(i=0;i<schm4k->vqdim[j];i++)
	{
	    target[k]=qed_vec[i];
	    k++;
	}
    }

    if (ipc_decMode == DEC3K) {
	k = schm4k->dim_tot - schm4k->vqdim[3];
	if (k < send2){
	    for(i= k+1 ; i<=send2; i++)
		target[i]=0.;
        }
    } else {
        if (schm4k->dim_tot < send2){
	    for(i= (schm4k-> dim_tot) +1 ; i<=send2; i++)
	        target[i]=0.;
        }
    }
}


static void st_vq(
float in_vec[],
float weight[],
float qed_vec[],
float cb[][MAXDIM1],
int vqdim,
int vqsize,
int *id,
int ende)
{
	int j,l,idx;
	float dist,mindist = 0.0;
	/* float dis(); */

 	if(ende == ENC){
		idx=0;
		for(j=0;j<vqsize;j++){
			dist=dis(cb[j],in_vec,weight,vqdim);
			if(dist < mindist || j == 0){
				mindist=dist;
				idx=j;
			}
		}
		*id=idx;
		for(l=0;l<vqdim;l++)
			qed_vec[l]=cb[idx][l];

	}
	else{
		idx=*id;
		for(l=0;l<vqdim;l++)
			qed_vec[l]=cb[idx][l];
	
	}

}



void quan_4k_dec(  
float target[],
int send2,   		
vqschm4k *schm4k,
cbook4k *cb4k,
float *cb4kpt[],
int id4k[])
{
	float in_vec[MAXDIM1];
	float weight[MAXDIM1];
	float qed_vec[MAXDIM1];
	int ende,k,i,j;

	ende=DEC;
	target[0]=0.;
	k=1;
	for(j=0;j<schm4k->num_vq;j++){
		st_vq(in_vec,weight,qed_vec,(float (*)[MAXDIM1])cb4kpt[j],schm4k->vqdim[j], schm4k->vqsize[j],&id4k[j], ende);
		for(i=0;i<schm4k->vqdim[j];i++){
			target[k]=qed_vec[i];
			k++;
		}
	}

	if (schm4k->dim_tot < send2){
		for(i= (schm4k-> dim_tot) +1 ; i<=send2; i++)
			target[i]=0.;
	}
	
}



void harm_sew_dec(float (*am)[3], float *pch, float *dumLSF, int normMode,
		  int vuv, int *idxS, int bitnum, int idxG, int vqp,
		  float pchmod, int *id4k) 
{
	int i;
	float w02,w0max;
	int send2,sendmax;
	float qam2[SAMPLE/2];
	float localip_f[SAMPLE*R/2];
	static float prevqed[(20+NUMPITCH/2)/2 + 1];
	float prevqed2[(20+NUMPITCH/2)/2 + 1];
	float da[(20+NUMPITCH/2)/2 + 1];
	float feneq;
	static float feneqold;
	float dpcm_ctl;
	int ende;
	static int old_vuv=0;
	IdAm  idAmInt;
	float target[MAX_HARM];
	int mute;  /* 98.1.16 */

        mute=0;    /* 98.1.16 */

	if(normMode != 0){
		printf("Hamonic quantization: Invalid Normalization Mode !!");
		exit(10);
	}
	if(bitnum != 10){
		printf("Hamonic quantization: Invalid Bit number !!");
		exit(10);
	}

	sendmax = ipc_cvq_lpc_dec.numpulse; 

	w0max = (float)(SAMPLE*R/2 * 0.95)/(float)sendmax ;  

	if( vuv ==0 || old_vuv ==0 ) 
		dpcm_ctl=0.;
	else
		dpcm_ctl=1.;
	idAmInt.idS0=idxS[0];
	idAmInt.idS1=idxS[1];
	idAmInt.idG= idxG;
	ende = 0;
	quand_lpc_dec(da,sendmax,&ipc_cvq_lpc_dec,&cba_lpc_dec,&idAmInt,vuv);  


	if( vqp ==0){
		for(i=1;i<=sendmax;i++){
			prevqed[i]=da[i]+prevqed[i] * LEAKFAC * (ipc_cvq_lpc_dec.leak[i]) * dpcm_ctl; 
			}
	}

	prevqed[0]= 0.;  
	
	for(i=1;i<=sendmax;i++){
		if(prevqed[i] <= 0){
	        	prevqed[i]=0.;
		}
	}

	feneq=0.;
	for(i=1;i<=sendmax;i++)
		feneq += prevqed[i]*prevqed[i];
	feneq = sqrt(feneq/(float)sendmax);
	if(feneq < 1.0){ 
		for(i=0;i<=sendmax;i++)
			prevqed[i]=0.;
   		mute=1;
		}
	else if(0.5*(feneqold + feneq) < 1.4){
		for(i=0;i<=sendmax;i++)
			prevqed[i]=0.;
		mute=1;
		}
	feneqold=feneq;


     *pch = *pch * pchmod;                        

     if(*pch > 147)
	*pch = 147.;
     if(*pch < 20)
	*pch = 20.;

      w02 = (float)(SAMPLE*R)/( *pch );      

      send2 = (int)(  0.95 *  ( *pch )/2.);   

        for(i=0; i<=44; i++)
               prevqed2[i] = prevqed[i]; 
	IpAmFIR_Deci_r(prevqed2,sendmax,w0max,localip_f,qam2,w02,send2);   

    if(mute==0 && (ipc_decMode == DEC4K || ipc_decMode == DEC3K)) /* 98.1.16 */
    {
	Quan4kDec(target, send2, &schm4k, &cb4k, id4k);

	for(i=0;i<=send2;i++)
	    qam2[i]=qam2[i] + target[i];
    }
	


	for(i=0; i<=send2; i++)
		am[i][2]=qam2[i]; 
	for(i=send2+1; i<SAMPLE/2; i++){
		am[i][2]=0.;
		}
	am[0][2]=0.;
	old_vuv=vuv;

}


void IPC_make_f_coef_dec(void)
{
	int i;
	int numc;
	
	numc=(JISU-1)*OVER_R; 
	for(i=1; i<=numc/2 ;i++)
		f_coef[i+numc/2 ]= f_coef[numc/2 - i ] = sin((float)i * M_PI/8.)/((float)i*M_PI/8.);
	f_coef[numc/2]=1.;
	for(i=0; i<=numc ;i++)
		f_coef[i]=f_coef[i]* 0.5 * (1. - cos(2. * M_PI * i/numc));

}

