
/************************************************************
**	PreEmphasis.c
**
**  Private :
**
**  History :
**     2013.05.14 M.Takigahira Build有無指定に対応
**     2013.05.09 M.Takigahira 処理精査、インデント修正、不要なloop内ifを排除、
**                             固定値#defineをパラメータ化、コメント追記、
**                             スコープバグを修正（WAVだと未定義変数でクラッシュする、SPECTRUMはあまり顕在化しない）、
**                             スペクトルフィルタの初期化関数を分離、
**                             "HA_log.h"をインクルード、printf=>LOGIへ変更
**                             ////負値に対する安全対策でnum_src!=0からnum_src>0へ変更
**                             num_fft, num_waveを引数から得るように変更、配列はARY_NUM_xxxマクロで宣言するように変更
**                             CalcSpecAdderPowerと添字を合わせてi,kからs(SRC),f(FFT)に変更、w(WAVE)は追加
**     2013.01.10 kama@SIF     新規作成(V1.0)
**
**  Description
**  -----------
**  高域を強調する
**  STATUS	PreEmphasis
**  入力1　：WAVEデータ、またはスペクトル
**  出力1　：高域強調されたWAVEデータ、またはスペクトル
**
*/

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

///////////////////////////////////////////////////////////////
//  Define Macros

#if !defined BUILD_MODULE_PRE_EMPHASIS
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
void ClearPreEmphasis(void){ }
void HAInitPreEmphasis(void){ }
int	PreEmphasis(
		int num_src,
		int count,
		HA_Complex inputSpec[ARY_NUM_SRC][ARY_NUM_FFT],
		HA_Complex outputSpec[ARY_NUM_SRC][ARY_NUM_FFT]
		){ return 0; }
#else // #if defined BUILD_MODULE_PRE_EMPHASIS
///////////////////////////////////////////////////////////////
//  Parameter
#define LOCAL_DEFAULT_PREEMCUTOFF 7900
#define LOCAL_DEFAULT_PREEMCOEF   0.97f

static int   input_type	  = SPECTRUM; // default mode SPECTRUM

static Param_PreEmphasis_t preem_param = {
	ARY_NUM_WAVE,
	ARY_NUM_FFT,
	DEFAULT_SAMPLING_RATE,
	LOCAL_DEFAULT_PREEMCUTOFF,
	LOCAL_DEFAULT_PREEMCOEF
};

static int   n            = ARY_NUM_WAVE; //512; // length of PreEmphasis parameter
static int   n_2          = (ARY_NUM_WAVE / 2);
static int   n_4          = (ARY_NUM_WAVE / 4);
static int   pslength     = (ARY_NUM_WAVE / 2 + 1);
static int   separ_length = (ARY_NUM_WAVE * 7900 / SAMPLING_RATE);
static float preemcoef    = -LOCAL_DEFAULT_PREEMCOEF;
static float rad_coef     = (M_PI * 2.0f / ARY_NUM_WAVE);

///////////////////////////////////////////////////////////////
//  Local Buffer
HA_Complex	preempha[ARY_NUM_FFT];

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

///////////////////////////////////////////////////////////////
//  Initialize
void HAInitPreEmphasis(void)
{
	int f, revf;
	float re, im, re2, im2;

	HA_Complex	a[ARY_NUM_FFT];
	HA_Complex	ma[ARY_NUM_FFT];
	HA_Complex	ma2[ARY_NUM_FFT];
	HA_Complex	y;

	const int num_fft  = preem_param.fft_length;
	const int num_wave = preem_param.length;

	switch(input_type){
	case WAV:
		preemcoef    = -preem_param.preem_coef;
		break;
	case SPECTRUM:
		n            = num_wave;
		n_2          = n / 2;
		n_4          = n / 4;
		pslength     = n_2 + 1;
		separ_length = n * preem_param.cutoff / SAMPLING_RATE;
		preemcoef    = -preem_param.preem_coef;       // INFO : float
		rad_coef     = M_PI * 2.0f / n;  // INFO : float
		// making a spectral filter
		a[0].re = 1.0f;
		a[0].im = preemcoef;
		for(f=1; f<num_fft; f++){
			a[f].re = 0.0f;
			a[f].im = 0.0f;
		}
		//for(f=0; f<num_fft; f++){ LOGI("f=%d : a[f](%f, %f)\n", f, a[f].re, a[f].im); }

		//fftf.analyze(); //<=PreEmphasis.cc
		//m_fft.analyze(); //<=fft.hpp <=PreEmphasis.cc::rfftf.analyze();
		//==== fft.hpp rfft.analyze() start ====// // 変数を減らすためfft.hppで使っていた添字のkからfへ変更
		fft(a, ma);
		//for(f=0; f<num_fft; f++){ LOGI("f=%d : ma[f](%f, %f)\n", f, ma[f].re, ma[f].im); }

		for(f=1, revf=n_2-1; f<n_4; f++, revf--){
			//std::complex<T> y = (T)0.5
			//                  * std::complex<T>(1.0 - w[k].imag(), w[k].real())
			//                  * (m_fft.A[k] - conj(m_fft.A[n_2 - k]));
			re = 1.0f + sin(rad_coef * f);
			im = cos(rad_coef * f);
			re2 = ma[f].re - ma[revf].re;
			im2 = ma[f].im + ma[revf].im;
			y.re = 0.5f * (re * re2 + im * im2);
			y.im = 0.5f * (re * im2 + im * re2);
			//LOGI("f=%d: y(%f, %f)\n", f, y.re, y.im);

            //A[k] = m_fft.A[k] - y;
			ma2[f].re = ma[f].re - y.re;
			ma2[f].im = ma[f].im - y.im;

            //A[n_2 - k] = conj(conj(m_fft.A[n_2 - k]) + y);
			ma2[revf].re =  ma[revf].re + y.re;
			ma2[revf].im = -(-ma[revf].im + y.im);
		}

		//A[0] = m_fft.A[0].real() + m_fft.A[0].imag();   // 後でma2[0]は0.0f埋めを行うので処理しない
		//A[n_2] = m_fft.A[0].real() - m_fft.A[0].imag(); // 後でma2[n_2]は0.0f埋めを行うので処理しない
		//A[n_4] = conj(m_fft.A[n_4]);
		ma2[n_4].re = ma[n_4].re;
		ma2[n_4].im = -ma[n_4].im;
		//==== fft.hpp rfft.analyze() end ====//

		//for(f=0; f<pslength; f++){
		//	if(f<4 || f>separ_length){
	  	//		ma2[f].re = 0.0f;
	  	//		ma2[f].im = 0.0f;
		//	}
		//}
		for(f=(4>pslength ? pslength-1 : 4-1); f>=0; f--){ // if(f>=0 && f<4 && f<pslength)
  			ma2[f].re = 0.0f;
  			ma2[f].im = 0.0f;
		}
		for(f=separ_length+1; f<pslength; f++){ // if(f>separ_length && f<pslength)
  			ma2[f].re = 0.0f;
  			ma2[f].im = 0.0f;
		}

		for(f=0, revf=pslength-1; f<pslength; f++, revf--){
			preempha[f] = ma2[revf];
			//LOGI("f=%d : preempha[f](%f, %f)\n", f, preempha[f].re, preempha[f].im);
   	   	}
		break;
	default:
		break;
	}
}

///////////////////////////////////////////////////////////////
//  SetParam
/******************************************************************************
**  History :
**    2013.05.31 M.Takigahira 新規作成
*/
// Public : パラメータ設定　初期化の前に実行すること
int HASetParamPreEmphasis(Param_PreEmphasis_t *param)
{
    if((param->length <= 0) || (param->length > ARY_NUM_WAVE)){
        return -1;
    }
    if((param->fft_length <= 0) || (param->fft_length > ARY_NUM_FFT)){
        return -1;
    }
	if((param->cutoff < 0) || (param->cutoff > param->sampling_rate)){
		return -1;
	}
    if((param->preem_coef<0.0f)/* || (param->preem_coef>1.0f)*/){
    	return -1;
    }
    preem_param = *param;
    return 0;
}


///////////////////////////////////////////////////////////////
//  MAIN
int	PreEmphasis(
	int	num_src,
	int	count,
	HA_Complex	inputSpec[ARY_NUM_SRC][ARY_NUM_FFT],
	HA_Complex	outputSpec[ARY_NUM_SRC][ARY_NUM_FFT]
	//HA_Float	inputWave[ARY_NUM_SRC][ARY_NUM_WAVE],
	//HA_Float	outputWave[ARY_NUM_SRC][ARY_NUM_WAVE]
	)
{
	int s, f/*, w*/;
	const int num_fft  = preem_param.fft_length;
	const int num_wave = preem_param.length;
	HA_Complex tmp_out, tmp_in, tmp_preem;

	// preempha初期化
	////if(count==0){ HAInitPreEmphasis(); } // move to Initialize function

    // processing
	switch(input_type){
	case WAV: // INFO : preemcoef is already *(-1) in HAInitPreEmphasis()
#ifdef NEVER // because inputWave isn't defined yet.
		for(s=0; s<num_src; s++){
			outputWave[s][0] = inputWave[s][0] * (1.0f + preemcoef); // w==0 step
			for(w=1; w<num_wave; w++){
				outputWave[s][w] = inputWave[s][w] + inputWave[s][w-1] * preemcoef;
			}
      	}
#endif
		break;
	case SPECTRUM:
		for(s=0; s<num_src; s++){
			for(f=0; f<num_fft; f++){
				tmp_in = inputSpec[s][f];
				tmp_preem = preempha[f];
				tmp_out.re  = tmp_in.re * tmp_preem.re;
				tmp_out.re += tmp_in.im * tmp_preem.im;
				tmp_out.im  = tmp_in.im * tmp_preem.re;
				tmp_out.im += tmp_in.re * tmp_preem.im;
				outputSpec[s][f] = tmp_out;
			}
      	}
		break;
	default:
    	//cout << "input_type?"<<endl;
		break;
	}

#ifdef NEVER // original code.
    if(input_type=="WAV"){
      	for(; it != in_map.end(); ++it){
			for(int i = wav.size() -1; i >= 1; i--){
	  			(*x)[i] = wav[i]-wav[i - 1] * preemcoef;
			}
			(*x)[0] = wav[0]*(1.0f -preemcoef);
			(*output)[it->first] = x;
      	}
      	out[count]=output;
    }
	else if(input_type=="SPECTRUM"){
      	for(; it != in_map.end(); ++it){
			for(int i=0; i<spec.size(); i++){
	  			(*X)[i] = spec[i]*preempha[i];
			}
			(*output)[it->first] = X;
      	}
      	out[count]=output;
    }
    else{
      	cout << "input_type?"<<endl;
    }
#endif
	return(OK);
}
#endif // BUILD_MODULE_PRE_EMPHASIS

// EOF
