
/************************************************************
**	MelFilterBank.c
**
**  Private :
**
**  History :
**     2013.05.13 M.Takigahira Build有無指定に対応
**     2013.05.08 M.Takigahira 処理精査、インデント修正、全てのloop内ifを排除
**     2013.01.10 kama@SIF     新規作成(V1.0)
**
**  Description
**  -----------
**  メルフィルタバンク処理を行う
**  STATUS	MelFilterBank
**  入力1　：入力スペクトル
**  出力1　：フィルタバンク
**
*/

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

///////////////////////////////////////////////////////////////
//  Define Macros
////#define	LO_FREQ 	(MIN_FREQUENCY * 512 / SAMPLING_RATE)
////#define	HI_FREQ 	(MAX_FREQUENCY * 512 / SAMPLING_RATE)

#if !defined BUILD_MODULE_MEL_FILTER_BANK
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
void ClearMelFilterBank(void){ }
void InitMelFilterBank(void){ }
int HASetParamMelFilterBank(Param_MelFilterBank_t *param){ return 0; }
float Mel(int f){ return 0.0f; }
float Mel2(int k, int fs, int n){ return 0.0f; }
void PrepareForFilter(void){ }
void Filter(HA_Float spec[ARY_NUM_FFT], HA_Float fbank[ARY_NUM_FBANK*2]){ }
int	MelFilterBank(int num_src, int num_fft, int num_wave, int count, HA_Complex inputSpec[ARY_NUM_SRC][ARY_NUM_FFT], HA_Float outputFbank[ARY_NUM_SRC][ARY_NUM_FBANK*2]){ return 0; }
#else // #if !defined BUILD_MODULE_MEL_FILTER_BANK
///////////////////////////////////////////////////////////////
//  Parameter
#define LOCAL_DEFAULT_CUTOFF         8000
#define LOCAL_DEFAULT_MIN_FREQUENCY  63
#define LOCAL_DEFAULT_MAX_FREQUENCY  8000

static Param_MelFilterBank_t melfilter_param = {
	ARY_NUM_FBANK,               // 13    [bank]
	ARY_NUM_WAVE,                // 512
	ARY_NUM_FFT,                 // 256+1
	DEFAULT_SAMPLING_RATE,       // 16000 [sample/sec]
	LOCAL_DEFAULT_CUTOFF,        // 8000  [Hz]
	LOCAL_DEFAULT_MIN_FREQUENCY, // 63    [Hz]
	LOCAL_DEFAULT_MAX_FREQUENCY  // 8000  [Hz]
};
static int lo_freq = (LOCAL_DEFAULT_MIN_FREQUENCY * ARY_NUM_WAVE / DEFAULT_SAMPLING_RATE);
static int hi_freq = (LOCAL_DEFAULT_MAX_FREQUENCY * ARY_NUM_WAVE / DEFAULT_SAMPLING_RATE);

///////////////////////////////////////////////////////////////
//  Local Buffer
short		lo_index[ARY_NUM_FFT];
HA_Float	lo_weight[ARY_NUM_FFT];
HA_Float	spectrum[ARY_NUM_FFT];
HA_Float	fbank_tmp[ARY_NUM_FBANK*2];

///////////////////////////////////////////////////////////////
//  Clear
void ClearMelFilterBank(void) /* 2013.02.22 初期化処理を追加 */
{
	/* バッファのクリア */
	bzero(lo_index, sizeof(lo_index));
	bzero(lo_weight, sizeof(lo_weight));
	bzero(spectrum, sizeof(spectrum));
	bzero(fbank_tmp, sizeof(fbank_tmp));
}

///////////////////////////////////////////////////////////////
//  Initialize
// f : frequency [Hz]
float Mel(int f)
{
	return (1127 * log(1 + f * (1 / 700.0f)));
}

// k : frequency bin ID, fs : sampling frequency [Hz], n : analysis frame length
float Mel2(int k, int fs, int n)
{
	return (1127 * log(1 + (k - 1) * fs * (1 / 700.0f) / n));
}

void PrepareForFilter(void)
{
	int c, f;

	const int num_fbank = melfilter_param.fbank;
	const int num_wave = melfilter_param.length;
    const int sampling_rate = melfilter_param.sampling_rate;

	const int slength = num_wave / 2 + 1;
	const int mel_center_size = num_fbank + 2;

	const float mel_hi = Mel(melfilter_param.cutoff);
	const float mel_lo = 0.0f;

	float mel_f;
	HA_Float	mel_center[mel_center_size];

	////float mel_s 	= mel_hi - mel_lo;
    const float mel_coef = (float) ((mel_hi - mel_lo) / (num_fbank + 1));

	////int	sampling_rate = SAMPLING_RATE;
	////int	length = num_wave;

	mel_center[0] = mel_lo; // TAKIGAHIRA : This modified to ensure the safety in the case of c==0, modified to for fast calculation of lo_weight[k]
	for(c=1; c<mel_center_size; c++){
      	//mel_center[c] = ((float)c / (num_fbank + 1)) * mel_s + mel_lo;
      	mel_center[c] = ((float)c * mel_coef) + mel_lo;
    }
	for(f=1, c=1; f<slength; f++){
     	mel_f = Mel2(f, sampling_rate, num_wave);
      	while(mel_center[c]<mel_f && c<mel_center_size){ c++; }
      	lo_index[f] = c - 1; // TAKIGAHIRA : Remark, if(mel_center[1]>mel_f) => c==0.
      	//LOGI("%d: %d\n", f, lo_index[f]);
    }
	for(f=1; f<=hi_freq; f++){ // TAKIGAHIRA : modified to for fast calculation lo_weight[f]
     	mel_f = Mel2(f, sampling_rate, num_wave);
		c = lo_index[f];
		//if(c > 0){ lo_weight[f] = (mel_center[c + 1] - Mel2(f, sampling_rate, num_wave)) / (mel_center[c + 1] - mel_center[c]); }
		//else{      lo_weight[f] = (mel_center[    1] - Mel2(f, sampling_rate, num_wave)) / (mel_center[    1] - mel_lo       ); }
		lo_weight[f] = (mel_center[c + 1] - mel_f) / (mel_center[c + 1] - mel_center[c]);
		//LOGI("f=%d : lo_weight[f](%f)\n", f, lo_weight[f]);
	}
}

void HAInitMelFilterBank(void)
{
	PrepareForFilter();
}

///////////////////////////////////////////////////////////////
//  SetParam
int HASetParamMelFilterBank(Param_MelFilterBank_t *param)
{
	if(param->fbank < 0){
		return -1;
	}
	if(param->length < 1){
		return -1;
	}
	if(param->sampling_rate < 0.0f){
		return -1;
	}
	if((param->cutoff < 0) || (param->cutoff > param->sampling_rate)){
		return -1;
	}
	if((param->min_frequency < 0)
	|| (param->max_frequency > param->sampling_rate)
	|| (param->max_frequency < param->min_frequency)){
		return -1;
	}
	lo_freq = (int) (param->min_frequency * param->length / param->sampling_rate);
	hi_freq = (int) (param->max_frequency * param->length / param->sampling_rate);
	melfilter_param = *param;

	return 0;
}

///////////////////////////////////////////////////////////////
//  MAIN
// TAKIGAHIRA : fbankのサイズが実際に使う[fbank_count]の倍もあり無駄だがオリジナルの仕様に基づき変更しない
//              オリジナルHARKでは前段で処理されたBank数と異なるBank数でMelFilterを掛ける可能性があるためと考えられる
void Filter
	(
	HA_Float	spec[ARY_NUM_FFT],
	HA_Float	fbank[ARY_NUM_FBANK*2]
	)
{
	int i, f;
	const int num_fbank = melfilter_param.fbank;

	for(i=0; i<ARY_NUM_FBANK*2; i++){
      	fbank[i] = 0.0f;
    }
	for(f=lo_freq; f<=hi_freq; f++){
      	float 	a = spec[f];
      	int 	bin = lo_index[f];
      	float 	weighted_a = lo_weight[f] * a;

     	//if(bin>0 && bin<=num_fbank){ fbank[bin - 1] += weighted_a; }
     	//if(bin<num_fbank){           fbank[bin] += a - weighted_a; }
     	if(bin>0){
     		if(bin<num_fbank){
    	       	fbank[bin - 1] += weighted_a;
    	       	fbank[bin] += a - weighted_a;
        	}
     		else if(bin==num_fbank){ // TAKIGAHIRA : this code is important, because bin's max value is num_fbank+1.
    	       	fbank[bin - 1] += weighted_a;
     		}
  	 	}
     	else{ // TAKIGAHIRA : case of bin==0, that can skip if(bin<num_fbank).
	       	fbank[bin] += a - weighted_a;
     	}
     	//LOGI("bin=%d : %f, %f, %f\n", bin, fbank[bin-1], fbank[bin - 1], weighted_a );
   	}
	//for(i=0; i<num_fbank*2; i++){
	//	LOGI("i=%d : fbank[i](%f)\n", i, fbank[i]);
	//}
}

int	MelFilterBank
	(
	int	num_src,
	int	count,
	HA_Complex	inputSpec[ARY_NUM_SRC][ARY_NUM_FFT],	// 入力はComplexのみ
	HA_Float	outputFbank[ARY_NUM_SRC][ARY_NUM_FBANK*2]
	)
{
	int s, i, f;
	const int num_fft = melfilter_param.fft_length;

	////if(count==0){ PrepareForFilter(); } // TAKIGAHIRA : move to Initialize function
	for(s=0; s<num_src; s++){
		// 入力はComplexのみ
		for(f=0; f<num_fft; f++){
          	spectrum[f] = sqrtf((inputSpec[s][f].re*inputSpec[s][f].re) + (inputSpec[s][f].im*inputSpec[s][f].im)); // TAKIGAHIRA powを展開
          	//if(f<10){ LOGI("f=%d : spectrum[f](%f)\n", f, spectrum[f]); }
        }
       	Filter(spectrum, fbank_tmp);
		for(i=0; i<ARY_NUM_FBANK*2; i++){ // 未使用領域も初期化したいので返す
			outputFbank[s][i] =	fbank_tmp[i];
			//LOGI("%d: %f\n", i, fbank_tmp[i]);
    	}
	}
	return(OK);
}
#endif // BUILD_MODULE_MEL_FILTER_BANK

// EOF
