
/************************************************************
**	HRLE.c
**
**  Private :
**
**  History :
**     2013.05.13 M.Takigahira Build有無指定に対応
**     2013.05.07 M.Takigahira 処理精査、インデント修正、不要なloop内ifを排除、
**                             #defineで固定となっていたパラメータの変数化
**     2012.12.09 kama@SIF     新規作成(V1.0)
**
**  Description
**  -----------
**  定常ノイズレベル推定(入出力、パワースペクトル） Histogram Reursive Level Estimation
**  STATUS	HRLE({float[id], *float[id])	IDの数分用意。入力信号と推定ノイズ。
**  入力1　：パワースペクトル
**  出力1　：パワースペクトル
**
**  	処理の内容
**
**  	1) レベル変換				YL(t) = 10log10Yp(t)
**  	2) インデックスの計算			Iy(t) = {(YL(t) - Lmin) / Lstep} ... {a}はa以下で最も近い整数
**  	3) ヒストグラムの更新			N(t,l) = α・N(t-1,l) + (1-α)・δ・(l-Iy(t))
**  	4) 累積ヒストグラムの計算		S(t,l) = ΣN(t,k)
**  	5) Lxインデックスの計算		Ix(t) = argmin[S(t,Imax)x/100 - S(t,I)]
**  	6) レベルの計算			Lx(t) = Lmin + Lstep ・ Ix(t)
**  	7) パワーへの変換			Np(t) = 10^Lx(t)/10
**
*/

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

///////////////////////////////////////////////////////////////
//  Define Macros
#define LOCAL_DEFAULT_BIN_MAX              (2048) // Ensure the array allocation, it is necessary to ensure a size larger than hrle_info.num_bin in advance.
#define LOCAL_DEFAULT_UPDATE_TABLE_SIZE 	(32768)
#define LOCAL_DEFAULT_UPDATE_TABLE_SBIT 	(16)
#define LOCAL_DEFAULT_UPDATE_TABLE_STEP 	((long)1 << LOCAL_DEFAULT_UPDATE_TABLE_SBIT)

#if !defined BUILD_MODULE_HRLE
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
int HASetParamHRLE(Param_HRLE_t *param){ return 0; }
void ClearHRLE(void){ }
int	PF_initialize(){ return 0; }
int	PF_execute(
		HA_Float X1[ARY_NUM_FFT],
		HA_Float Y[ARY_NUM_FFT]
		){ return 0; }
int HRLE(
		int num_src,
		HA_Float inputSpec[ARY_NUM_SRC][ARY_NUM_FFT],
		HA_Float outputSpec[ARY_NUM_SRC][ARY_NUM_FFT]
		){ return 0; }
#else // #if defined BUILD_MODULE_HRLE
///////////////////////////////////////////////////////////////
//  Parameter
/*
static Param_HRLE_t hrle_param ={
	ARY_NUM_FFT,  // FFT長
	0.500000f,    // 正規化累積頻度
	0.999937f,    // 時定数
	-200.000000f, // ヒストグラムの最少レベル
	0.500000f,    // ヒストグラムのビンの幅
	1000          // ヒストグラムのビン数
};
*/
static Param_HRLE_t hrle_param ={
	ARY_NUM_FFT,  // FFT長
	0.400000f,    // 正規化累積頻度
	0.999937f,    // 時定数
	-160.000000f, // ヒストグラムの最少レベル
	0.400000f,    // ヒストグラムのビンの幅
	800           // ヒストグラムのビン数
};

//static int debug_hrle = 1; // #define ORG_PARAM is 1, #undef ORG_PARAM is 2.

///////////////////////////////////////////////////////////////
//  Local Buffer
HA_Float	X[ARY_NUM_SRC][ARY_NUM_FFT];
HA_Float	X1[ARY_NUM_FFT];
HA_Float	Y[ARY_NUM_FFT];
//HA_Float	YHistA[PF_N][ARY_NUM_FFT];
HA_Float	YHistA[ARY_NUM_FFT][LOCAL_DEFAULT_BIN_MAX];
HA_Float	PowV[LOCAL_DEFAULT_BIN_MAX];
HA_Float	ctmp[LOCAL_DEFAULT_BIN_MAX];
short		UPDATE_TABLE[LOCAL_DEFAULT_UPDATE_TABLE_SIZE+1024];	// Make Table for update
int			LxI[ARY_NUM_FFT];

HA_Float	tr = (1. - 1./(16000.*60)); 			// for debugｌ setting;
HA_Float	tr_inv = 1/(1. - 1./(16000.*60));
HA_Float	tr_pow = 1.0e-20;

int	HPostv4_Initialized = FALSE;		// 最初の一回だけ初期化するためのフラグ

///////////////////////////////////////////////////////////////
//  Clear
void ClearHRLE(void) /* 2013.02.15 初期化処理を追加 */
{
	HPostv4_Initialized = FALSE;

	/* バッファのクリア */
	bzero(X, sizeof(X));
	bzero(X1, sizeof(X1));
	bzero(Y, sizeof(Y));
	bzero(YHistA, sizeof(YHistA));
	bzero(PowV, sizeof(PowV));
	bzero(ctmp, sizeof(ctmp));
	bzero(UPDATE_TABLE, sizeof(UPDATE_TABLE));
	bzero(LxI, sizeof(LxI));

	/* 係数の初期化 */
	tr = (1. - 1./(16000.*60));
	tr_inv = 1/(1. - 1./(16000.*60));
	tr_pow = 1.0e-20;

	/* 2013.03.05 K.Takagi 追加 */ /* 2013.05.07 M.Takigahira パラメータセット（暫定）追加 */
#if 0
	Param_HRLE_t dummy = {
		ARY_NUM_FFT,  // FFT長
		0.400000f,    // 正規化累積頻度
		0.999937f,    // 時定数
		-160.000000f, // ヒストグラムの最少レベル
		0.400000f,    // ヒストグラムのビンの幅
		800           // ヒストグラムのビン数
	};
#endif

#if 0 // 2013.08.07 K.Takagi
	// 以下はWavファイルテスト用です K.Takagi
	Param_HRLE_t dummy = {
		ARY_NUM_FFT,  // FFT長
		0.500000f,    // 正規化累積頻度
		0.999937f,    // 時定数
		-200.000000f, // ヒストグラムの最少レベル
		0.500000f,    // ヒストグラムのビンの幅
		2000          // ヒストグラムのビン数
	};

	// パラメータ設定はinit_GHDSSに移動
	HASetParamHRLE(&dummy);
#endif

	PF_initialize();
	HPostv4_Initialized = TRUE;
}

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

///////////////////////////////////////////////////////////////
//  SetParam
int HASetParamHRLE(Param_HRLE_t *param)
{
	if((param->lx < 0.0f) || (param->lx > 1.0f)){
		return -1;
	}
	if((param->tr < 0.0f) || (param->tr > 1.0f)){
		return -1;
	}
	if((param->min_lv < -2000.0f) || (param->min_lv > 2000.0)){ // -2000~2000dB
		return -1;
	}
	if(param->step_lv <= 0.0f){
		return -1;
	}
	if((param->num_bin < 0) || (param->num_bin > LOCAL_DEFAULT_BIN_MAX)){
		return -1;
	}
	hrle_param = *param;
	return 0;
}

int HAGetParamHRLE(Param_HRLE_t *param)
{
	*param = hrle_param;
	return 0;
}

int	PF_initialize()
{
	int		b,f,n;
	float	bf, *tpflt;
	long	m;

	const int num_fft = hrle_param.fft_length;
	const int max_bin = hrle_param.num_bin;
	const float hist_min_lv = hrle_param.min_lv;
	const float hist_step_lv = hrle_param.step_lv;
	//YHistA.zeros(N, NfreqNch);	// N=2000, NfreqNch = 257
	//for(b=0; b<PF_N; b++){
	//for(b=0; b<max_bin; b++){
		//for(f=0; f<num_fft; f++){
	for(f=0; f<num_fft; f++){
		for(b=0; b<max_bin; b++){ YHistA[f][b] = 0.0f; } // Elements order was swapped. old) YHistA[b][f] = 0.0;
	}

	//PowV.colon(Min, Step, Min+(N-1)*Step);	//  Min = -200.000000, Step = 0.500000, etc = 799.500000
	//PowV = PowV.dB2Pow();

	//bf = PF_Min;
	//for(b=0; b<PF_N; b++){
	for(b=0, bf=hist_min_lv; b<max_bin; b++, bf+=hist_step_lv){
		{
			union {
				double d_val;
				int i_val[2];
			} u;
			u.i_val[1] = (int)(bf*346867.3+1072632447); // dB変換も同時に行う
			u.i_val[0] = 0;
			PowV[b] = (float) u.d_val;
		} // TAKIGAHIRA : This code same as PowV[b] = pow(10.0, bf * 0.1);
		//bf += PF_Step;
		//LOGI("PowV=%e\n", PowV[b]);
	}

	//for(n=0; n<1; n++){
	for(n=0, m=0; n<LOCAL_DEFAULT_UPDATE_TABLE_SIZE; n++, m+=LOCAL_DEFAULT_UPDATE_TABLE_STEP){
		//vp 		= reinterpret_cast<void*> (&m); 	// for erasing warning
		//tpflt 	= reinterpret_cast<float*> (vp); 	// tpflt = (float*)&m;
		tpflt = (float *)&m;

		if((~m & 0x78000000) == 0){ break; } 	// avoid Inf or Non

		////ctmp = PowV - *tpflt;
		////for(b=0; b<PF_N; b++){
		//OLD-1//for(b=0; b<max_bin; b++){
		//OLD-1//	ctmp[b] = PowV[b] - *tpflt;
		//OLD-1//	//LOGI("%d : ctmp = %e\n", b, ctmp[b]);
		//OLD-1//}
		////mind = ctmp.ind_firstplus();
		////UPDATE_TABLE[n] = mind.r(0);
		//OLD-1//UPDATE_TABLE[n] = 0;
		//OLD-1////for(b=0; b<PF_N; b++){
		//OLD-1//for(b=0; b<max_bin; b++){
		//OLD-1//	if(ctmp[b]>0){
		//OLD-1//		UPDATE_TABLE[n] = b;
		//OLD-1//		break;
		//OLD-1//	}
		//OLD-1//}

		UPDATE_TABLE[n] = 0;
		for(b=0; b<max_bin; b++){
			ctmp[b] = PowV[b] - *tpflt;
			//LOGI("%d : ctmp = %e\n", b, ctmp[b]);
			if(ctmp[b]>0){
				UPDATE_TABLE[n] = b;
				break;
			}
		}
		//LOGI("n=%d : UPDATE_TABLE=%d\n", n, UPDATE_TABLE[n]);
	}

	// Initialize LxI
	//LxI.zeros(1, YHistA.size2());
	for(f=0; f<num_fft; f++){ LxI[f] = 0.0f; }

	return(OK);
}

///////////////////////////////////////////////////////////////
//  MAIN
int	PF_execute(
	HA_Float	X1[ARY_NUM_FFT],
	HA_Float	Y[ARY_NUM_FFT]
	)
{
	//// TAKIGAHIRA : for 64bit build support
	////long	*ptemp;
	////long 	ltemp;
	////long 	n1, n2, n12, N2, N12;
	int		*ptemp;
	int		ltemp;
	int 	n1, n2, n12, N2, N12;

	float 	*rn2, *rn12;
	float 	ftmp;
	float 	xr;
	int		f;
	
	const int num_fft = hrle_param.fft_length;
	const int max_bin = hrle_param.num_bin;
	if(HPostv4_Initialized == FALSE){
		//LOGE("HPostv4 Initialized...\n");
		PF_initialize();
		//LOGE("Initialize finished\n");
		HPostv4_Initialized = TRUE;
	}

	//YHistA.TBL_addset_tr(YP.reshape(1), UPDATE_TABLE, UPDATE_TABLE_SBIT, tr_pow);
	for(f=0; f<num_fft; f++){
		//// TAKIGAHIRA : for 64bit build support
		////ptemp = (long *)&X1[f];
		ptemp = (int *)&X1[f];
		ltemp = *ptemp;
		//LOGI("f=%d : ltemp = %d\n", f, ltemp); // KAMA

		ltemp >>= LOCAL_DEFAULT_UPDATE_TABLE_SBIT;
		//// TAKIGAHIRA : for 64bit build support
		////n1 = (long)UPDATE_TABLE[ltemp];
		n1 = (int)UPDATE_TABLE[ltemp];
		//n12 = N2 + n1;
		//N12 = N2 + PF_N;
		//for(rn12=&YHistA[n12]; n12<N12; n12++, rn12+=2){
		for(n12=n1; n12<max_bin; n12++){ // change, PF_N => hrle_info.num_bin
			YHistA[f][n12] += tr_pow; // Elements order was swapped. old) YHistA[n12][f] += tr_pow;
		}
	}

	//for(f=0; f<5; f++){ // TAKIGAHIRA modified loop
	//    LOGI("YHistA[%d][0] = %e\n", f, YHistA[i][0]); // KAMA
	//}

	tr_pow *= tr_inv;

	if(tr_pow>1.0e+20){
		tr_pow *= 1.0e-40; // YHistA *= 1.0e-40;
		LOGI("WARN : tr_pow > 1.0e+20 \n");
	}

	//YHistA.nearest_inc((YHistA.mid1(YHistA.end1()) * Lx), LxI); 	// nearest index, end1=1999
	//High speed spec.
	//void Compmat::nearest_inc(const Compmat& x, Compmat& retval) const
	for(n2=0; n2<num_fft; n2++){
		//xr = YHistA[n2][PF_N-1] * PF_Lx; // Elements order was swapped. old) xr = YHistA[PF_N-1][n2] * PF_Lx;
		xr = YHistA[n2][max_bin-1] * hrle_param.lx;
		n1 = LxI[n2];
		//LOGI("n2=%d : xr=%e, n1=%d\n", n2, xr, n1);

		if(xr < YHistA[n2][n1]){ // backward search // Elements order was swapped. old) if(xr < YHistA[n1][n2]){ ... }
			for(n1--; n1>=0; n1--){
				//LOGI("n2=%d : Y=%f\n", n2, YHistA[n1][n2]*pow(10,20));
				if(xr >= YHistA[n2][n1]){ break; } // Elements order was swapped. old) if(xr >= YHistA[n1][n2]){ break; }
			}
			LxI[n2] = n1 + 1;
			if(n1<0){ continue; }

			ftmp = YHistA[n2][n1] + YHistA[n2][n1+1]; // Elements order was swapped. old) ftmp = YHistA[n1][n2] + YHistA[n1+1][n2];
			//LOGI("n2=%d : ftmp=%f\n", n2, ftmp*pow(10,20));
			ftmp -= xr;
			ftmp -= xr;
			if(ftmp>0){ LxI[n2] -= 1; }
		}
		else{  // forward search
			//for(n1++; n1<PF_N; n1++){
			for(n1++; n1<max_bin; n1++){
				if(xr < YHistA[n2][n1]){ break; } // Elements order was swapped. old) if(xr < YHistA[n1][n2]){ break; }
			}
			LxI[n2] = n1 - 1;
			if(n1==max_bin){ continue; } // change, PF_N => hrle_info.num_bin

			ftmp = YHistA[n2][n1] + YHistA[n2][n1-1]; // Elements order was swapped. old) ftmp = YHistA[n1][n2] + YHistA[n1-1][n2];
			ftmp -= xr;
			ftmp -= xr;
			if(ftmp<0){ LxI[n2] += 1; }
		}
		//LOGI("n2=%d : LxI=%d\n", n2, LxI[n2]);
		Y[n2] = PowV[LxI[n2]];
	}
	//LxP = PowV(LxI.reshape());	<- Yにかえす
	return(OK);
}

///////////////////////////////////////////////////////////////
//  MAIN
int HRLE(
	int	num_src,
	HA_Float	inputSpec[ARY_NUM_SRC][ARY_NUM_FFT],
	HA_Float	outputSpec[ARY_NUM_SRC][ARY_NUM_FFT]
	)
{
	int	s, f;
	const int num_fft = hrle_param.fft_length;

	if(num_src>0){
		/* 2013.03.05 K.Takagi X=X1のためバッファコピー省略
		//for(s=0; s<num_src; s++){
		//	for(f=0; f<num_fft; f++){
		//		X[s][f] = inputSpec[s][f];
		//		//if(f<10){ LOGI("f=%d : X=%f\n", f, X[s][f]); }
		//	}
		//}
		*/
		// postfilterの実行（各音源について、内部推定雑音ヒストグラムは共通に利用）
		// ＋出力用変数mNoiseSpecvalue( map<int, vector<float> >型) の各要素にデータを詰める

		for(s=0; s<num_src; s++){ // 音源のループ
			//X1 = X.mid1(nsrc);	// <- 単にsrcの位置のデータを抜き出すだけ。作業用。
			for(f=0; f<num_fft; f++){
				X1[f] = inputSpec[s][f]; //X1[f] = X[s][f];
			}

			//Y = PF.execute(X1); // Yには各音源でのノイズ推定値が入る
			PF_execute(X1, Y);

			for(f=0; f<num_fft; f++){
				outputSpec[s][f] = Y[f];	// vector<float>につめる
				//if(s==2){ LOGI("f=%d : outputSpec=%f\n", f, outputSpec[s][f]); }
				//if(f<10){ LOGI("f=%d : outputSpec=%f\n", f, outputSpec[s][f]); }
			}
		}
		//exit(0);
	}
	return(OK);
}
#endif // BUILD_MODULE_HRLE

// EOF
