
/************************************************************
**	MSLSExtraction.c
**
**  Private :
**
**  History :
**     2013.05.16 M.Takigahira
**     2013.05.14 M.Takigahira Build有無指定に対応
**     2013.05.09 M.Takigahira 処理精査、インデント修正、全てのloop内ifを排除
**                             "HA_log.h"をインクルード、printf=>LOGIへ変更
**                             num_fftを引数から得るように変更、配列はARY_NUM_xxxマクロで宣言するように変更
**                             CalcSpecAdderPowerと添字を合わせてi,kからs(SRC),f(FFT)に変更
**     2013.01.10 kama@SIF     新規作成(V1.0)
**
**  Description
**  -----------
**  静的特徴量から動的特徴量を求める
**  STATUS	MSLSExtraction
**  入力1　：フィルタバンク
**  入力2　：入力スペクトル
**  出力1　：音響特徴量ベクトル
**
*/

///////////////////////////////////////////////////////////////
//  Include Files
#include "build_modules.h"
//////////#include "header.h"
#include "HA_log.h"
#include "android_hark.h"

///////////////////////////////////////////////////////////////
//  Define Macros
#define	CEPSTRAL_MODE		(1)
#define	SPECTRAL_MODE		(2)

#if !defined BUILD_MODULE_MSLS_EXTRACTION
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
void idct(float *x, float *y, int num_fbank){ }
void dct(float* x, float* y, int num_fbank){ }
void LifterOld(int num_fbank, HA_Float fbank[ARY_NUM_FBANK]){ }
void SubtractMean(int num_fbank, HA_Float fbank[ARY_NUM_FBANK]){ }
void EmphasizeSpectralPeak(int num_fbank, HA_Float vec[ARY_NUM_FBANK]){ }
int	MSLSExtraction(
		int num_src,
		int num_fft,
		int num_fbank,
		HA_Complex spec[ARY_NUM_SRC][ARY_NUM_FFT],
		HA_Float input_fbank[ARY_NUM_SRC][ARY_NUM_FBANK*2],
		HA_Float output_fbank[ARY_NUM_SRC][ARY_NUM_FBANK_POWER*2]
		){ return 0; }
#else // #if defined BUILD_MODULE_MSLS_EXTRACTION
///////////////////////////////////////////////////////////////
//  Parameter
static int use_power          = TRUE;
static int normalization_mode = CEPSTRAL_MODE;

static int num_fft                  = ARY_NUM_FFT;
static int num_fbank                = ARY_NUM_FBANK;               // 13 = 13
static int num_fbank_delta_power    = ARY_NUM_FBANK_DELTA_POWER;   // 28 = 13 + 1 + 13 + 1

///////////////////////////////////////////////////////////////
//  Local Buffer

///////////////////////////////////////////////////////////////
//  Clear

///////////////////////////////////////////////////////////////
//  Initialize

///////////////////////////////////////////////////////////////
//  SetParam
int HASetParamMSLSExtraction(Param_MSLSExtraction_t *param)
{
    if(param->fbank <= 0){
        return -1;
    }
    else{
    	num_fbank             = param->fbank;
    	num_fbank_delta_power = (param->fbank+1)*2;
    }
    if((param->power != TRUE)
    && (param->power != FALSE)){
    	return -1;
    }
    else{
    	use_power = param->power;
    }
    if((param->normalization != CEPSTRAL_MODE)
    && (param->normalization != SPECTRAL_MODE)){
    	return -1;
    }
    else{
    	normalization_mode = param->normalization;
    }
    if(param->fft_length <= 0){
    	return -1;
    }
    else{
    	num_fft = param->fft_length;
    }
    return 0;
}

///////////////////////////////////////////////////////////////
//  MAIN
void idct(float *x, float *y)
{
	int i, j;
    float a, c, tmp, coef;
    a = 1.0f / num_fbank;
    c = (float) M_PI / num_fbank;
    for(i=0; i<num_fbank; i++){
    	tmp = x[0] * 0.5f;
    	coef = (i + 0.5f) * c;
      	for(j=1; j<num_fbank; j++){
      		tmp += x[j] * cos(j * coef);
      	}
      	tmp *= a;
      	y[i] = tmp;
    }
}

void dct(float* x, float* y)
{
	int i, j;
	float c, tmp, coef;
	c = (float) M_PI / num_fbank;
    for(i=0; i<num_fbank; i++){
    	tmp = 0.0f;
    	coef = i * c;
      	for(j=0; j<num_fbank; j++){
      		tmp += x[j] * cos((j + 0.5f) * coef); // TAKIGAHIRA : a=M_PI*i/num_fbank;
      	}
      	tmp *= 2.0f;
      	y[i] = tmp;
    }
}

void LifterOld(HA_Float fbank[ARY_NUM_FBANK])
{
	int i;
	HA_Float	cepstrum[ARY_NUM_FBANK];

	dct(&fbank[0], &cepstrum[0]);
    cepstrum[0] = 0.0f;
    for(i=13; i<num_fbank; i++){ cepstrum[i] = 0.0f; }
    idct(&cepstrum[0], &fbank[0]);
}

void SubtractMean(HA_Float fbank[ARY_NUM_FBANK])
{
	int i;
    float mean = 0.0f;
    for(i=0; i<num_fbank; i++){
      	mean += fbank[i];
    }
    mean /= num_fbank;
    for(i=0; i<num_fbank; i++){
      	fbank[i] -= mean;
    }
}

void EmphasizeSpectralPeak(HA_Float vec[ARY_NUM_FBANK])
{
	int i;
    for(i=num_fbank-1; i>0; i--){
      	vec[i] -= 0.9f * vec[i - 1]; // TAKIGAHIRA : p=0.9f; is constant value.
    }
}

int	MSLSExtraction
	(
	int	num_src,
	HA_Complex	spec[ARY_NUM_SRC][ARY_NUM_FFT],			// 入力スペクトル
	HA_Float	input_fbank[ARY_NUM_SRC][ARY_NUM_FBANK*2],
	HA_Float	output_fbank[ARY_NUM_SRC][ARY_NUM_FBANK_DELTA_POWER]
	)
{
	float power;
	int	s, i, f;
	HA_Float	fbank[ARY_NUM_FBANK];
	HA_Complex	tmp;

	for(s=0; s<num_src; s++){
		for(i=0; i<num_fbank_delta_power; i++){
			output_fbank[s][i] = 0.0f;
		}
	}

	for(s=0; s<num_src; s++){
		for(i=0; i<num_fbank; i++){
        	fbank[i] = logf(input_fbank[s][i] < 1.0f ? 1.0f : input_fbank[s][i]);
      	}

		switch(normalization_mode){
		case CEPSTRAL_MODE:		// <- とりあえずこっちだけ実装
			LifterOld(fbank);
			break;
		case SPECTRAL_MODE:
			SubtractMean(fbank);
			EmphasizeSpectralPeak(fbank);
			break;
		default:
			break;
		}

		for(i=0; i<num_fbank; i++){
        	output_fbank[s][i] = fbank[i];
      	}

      	if(use_power){
#if 0 /* 2013.03.22 K.Takagi 修正 */
         	power = 0.0f;
          	for(f=0; f<num_fft-1; f++){ 	// <- あとでデバック 1.#QNON0の件
				if(f==0 || f==num_fft-1){ // TAKIGAHIRA memo : loopがf<num_fft-1条件のため、f==num_fft-1にならない不具合
          			power += ((spec[s][f].re * spec[s][f].re) + (spec[s][f].im * spec[s][f].im));
				}
				else{
          			power += ((spec[s][f].re * spec[s][f].re) + (spec[s][f].im * spec[s][f].im)) *2;
				}
          	}
#endif
        	//power = ((spec[s][f].re * spec[s][f].re) + (spec[s][f].im * spec[s][f].im)); // f==0
         	//f = 0;
          	//for(f=1; f<num_fft-1; f++){
          	//	power += ((spec[s][f].re * spec[s][f].re) + (spec[s][f].im * spec[s][f].im)) * 2;
          	//}
        	//power += ((spec[s][f].re * spec[s][f].re) + (spec[s][f].im * spec[s][f].im)); // f==num_fft-1
          	//power /= (num_fft-1)*2;
        	//output_fbank[s][i] = (power >= 1e-40 ? log(power) : 0.0f); // output_fbank[s][num_fbank+1-1] ==> output_fbank[s][i]

          	f = 0;
          	tmp = spec[s][f];
        	power = ((tmp.re * tmp.re) + (tmp.im * tmp.im)) * 0.5f; // f==0
          	for(; f<num_fft-1; f++){
              	tmp = spec[s][f];
          		power += (tmp.re * tmp.re);
          		power += (tmp.im * tmp.im);
          	}
          	tmp = spec[s][f];
        	power += ((tmp.re * tmp.re) + (tmp.im * tmp.im)) * 0.5f; // f==num_fft-1
          	power /= (num_fft-1);
        	output_fbank[s][i] = (power >= 1e-40 ? logf(power) : 0.0f); // output_fbank[s][num_fbank+1-1] ==> output_fbank[s][i]
		}
    }
	return(OK);
}
#endif // BUILD_MODULE_MSLS_EXTRACTION

// EOF
