
/*

This software module was originally developed by

    Masayuki Nishiguchi and Kazuyuki Iijima (Sony Corporation)

    and edited by

    Yuji Maeda (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];

	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.;
		}
	else if(0.5*(feneqold + feneq) < 1.4){
		for(i=0;i<=sendmax;i++)
			prevqed[i]=0.;
		}
	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(ipc_decMode == DEC4K || ipc_decMode == DEC3K)
    {
	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));

}

