
/************************************************************
**	EstimateLeak.c
**
**  Private :
**
**  History :
**     2013.05.20 M.Takigahira "HA_log.h"をインクルード、printf=>LOGIへ変更
**                             負値に対する安全対策でnum_src!=0からnum_src>0へ変更
**                             num_fftをパラメータに変更、配列はARY_NUM_xxxマクロで宣言するように変更
**     2013.05.15 M.Takigahira FFTとSRCの処理順序交換、配列削減、配列次元数縮小
**     2013.05.13 M.Takigahira Build有無指定に対応
**     2013.04.26 M.Takigahira 処理精査、インデント修正、処理順序の最適化
**     2012.12.10 kama@SIF     新規作成(V1.0)
**
**  Description
**  -----------
**  漏れノイズ推定
**  STATUS	EstimateLeak({float[id], *float[id])	IDの数分用意。
**  入力1　：フィルタバンク
**  出力1　：動的特徴量を追加した、フィルタバンク
**
**  EstLeak OUT real = 0.500000  k = 0
**  EstLeak OUT real = 1.042076  k = 1
**  EstLeak OUT real = 1.243199  k = 2
**  EstLeak OUT real = 0.908173  k = 3
**  EstLeak OUT real = 3.486143  k = 4
**
**  	処理の内容
**
**  	1) pha_fS		vector float[257]を３個。IDごとにコピー
**  	2) pha_fS_sum	vector float[257]を１個分。pha_fSを周波数ごとに加算。
**  	3) pha_fS_leak	vector float[257]を３個分。	pha_fS_sum - pha_fS
**
**  	4) α、βの計算
**  		beta = -alp_leak / (1 - alp_leak^2 + alp_leak*(1 - alp_leak)*(M - 2) );
**  		alpha = 1 - (M - 1) * alp_leak * beta;
**
**  		alp_leakは、変数=LEAK_FACTOR=0.25
**  		Mは音源の数
**
**  	5) Zの計算
**  		Z = alpha * S + beta * S_leak; <- Z = alpha * pha_fS + beta * pha_fS_leak
**  		Z(Z < 1) = 1; % Clipping
**
**  	6) Z_sumの計算
**  		Z_sum += Z
**
**  	7) λleakの計算
**  		λleak = alp_leak * (Z_sum - Z)		.. 合算した分離音から自分のを引く
**
**  	**M.Takigahira 処理順序の変更
**  	「4)」のα、β計算は依存関係がないので最初に計算するように変更
**  	「1)-7)」のループはNUM_FFT(j)間の依存関係がないので同一変数への再アクセス性を高めコンパイラにレジスタで最適化が行われるように
**  	単一ループ内で各NUM_SRC(i)について計算されるように変更した
**
*/

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

///////////////////////////////////////////////////////////////
//  Define Macros
#define	LOCAL_DEFAULT_LEAK_FACTOR  (0.25) // fAlp_Leak

#if !defined BUILD_MODULE_ESTIMATE_LEAK
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
void ClearEstimateLeak(void){ }
void HAInitEstimateLeak(void){ }
int HASetParamEstimateLeak(Param_EstimateLeak_t *param){ return 0; }
int	EstimateLeak(
		int num_src,
		int num_fft,
		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_ESTIMATE_LEAK
///////////////////////////////////////////////////////////////
//  Parameter
static Param_EstimateLeak_t estleak_param = {
	ARY_NUM_FFT,
	LOCAL_DEFAULT_LEAK_FACTOR
};

///////////////////////////////////////////////////////////////
//  Local Buffer
////HA_Float	pha_fS_sum[ARY_NUM_FFT];               // TAKIGAHIRA
////HA_Float	pha_fS_leak[ARY_NUM_SRC][ARY_NUM_FFT]; // TAKIGAHIRA
////HA_Float	pfsZ[ARY_NUM_SRC][ARY_NUM_FFT];        // TAKIGAHIRA : I reduced the number of dimensions of the array.
HA_Float pfsZ[ARY_NUM_SRC];
////HA_Float	pha_fZ_sum[ARY_NUM_FFT];               // TAKIGAHIRA
////HA_Float	pha_fLamda_leak[ARY_NUM_SRC][ARY_NUM_FFT]; // TAKIGAHIRA : I know not to use this array.
HA_Float calcAlpha[ARY_NUM_SRC+1]; // TAKIGAHIRA : Do not use calcAlpha[0] for to fast reference.
HA_Float calcBeta[ARY_NUM_SRC+1];  // TAKIGAHIRA : Do not use calcBeta[0] for to fast reference.

///////////////////////////////////////////////////////////////
//  Clear
/******************************************************************************
**  History :
**    2013.03.31 M.Takigahira 初期化対象のバッファを追加、不要なバッファを削除
**    2013.02.22 ------------ 新規作成
*/
// Public : バッファクリア
void ClearEstimateLeak(void)
{
	////bzero(pha_fS_sum, sizeof(pha_fS_sum));   // TAKIGAHIRA
	////bzero(pha_fS_leak, sizeof(pha_fS_leak)); // TAKIGAHIRA
	bzero(pfsZ, sizeof(pfsZ));
	////bzero(pha_fZ_sum, sizeof(pha_fZ_sum));   // TAKIGAHIRA
	////bzero(pha_fLamda_leak, sizeof(pha_fLamda_leak));  // TAKIGAHIRA
	bzero(calcAlpha, sizeof(calcAlpha)); // TAKIGAHIRA
	bzero(calcBeta, sizeof(calcBeta)); // TAKIGAHIRA
}

///////////////////////////////////////////////////////////////
//  Initialize
/******************************************************************************
**  History :
**    2013.05.31 M.Takigahira 新規作成
*/
// Public : 初期化
void HAInitEstimateLeak(void)
{
	int i;
	const HA_Float fAlp_leak = estleak_param.leak_factor;
	// TAKIGAHIRA : Performs the alpha and beta calculations in advance for convert to table.
	for(i=ARY_NUM_SRC; i>0; i--){
		calcBeta[i]  = (HA_Float)(-1.0f * fAlp_leak / (1.0f - fAlp_leak * fAlp_leak + fAlp_leak * (1.0f - fAlp_leak) * ((HA_Float)(i - 2))));
		calcAlpha[i] = (HA_Float)(1.0f - (HA_Float)(i - 1) * fAlp_leak * calcBeta[i]);
		//LOGI("num_src=%d, fBeta=%f, fAlpha=%f\n", i, calcBeta[i], calcAlpha[i]); // debug
	}
	calcBeta[i]  = 0.0f;
	calcAlpha[i] = 0.0f;
}

///////////////////////////////////////////////////////////////
//  SetParam
/******************************************************************************
**  History :
**    2013.05.20 M.Takigahira 新規作成
*/
// Public : パラメータ設定　初期化の前に実行すること
int HASetParamEstimateLeak(Param_EstimateLeak_t *param)
{
    if((param->fft_length <= 0) || (param->fft_length > ARY_NUM_FFT)){
        return -1;
    }
    if((param->leak_factor<0.0f) || (param->leak_factor>1.0f)){
    	return -1;
    }
    estleak_param = *param;
    return 0;
}

///////////////////////////////////////////////////////////////
//  MAIN
int	EstimateLeak(
	int	num_src,
	HA_Float	inputSpec[ARY_NUM_SRC][ARY_NUM_FFT],
	HA_Float	outputSpec[ARY_NUM_SRC][ARY_NUM_FFT]
	)
{
	// InputされるMapをFloatSpecArrayに変換し，Leak計算を行う
	// 計算結果をMapに変換し出力する

//	int i, k, iCount;
//	int iNCh, iNFreq;
//	HA_Float	*pfSpecTemp, *pfSpecTempEnd;

//	MICARY_FloatSpecArray	*pfsS_leak, *pfsZ;
//	HA_Float	**pha_fS, **pha_fS_leak, **pha_fZ, **pha_fLamda_leak;
//	HA_Float	*pha_fS_sum, *pha_fZ_sum;

//	int	iFreq, iCh;
//	int	iArrayLen;

	int	s, f;
	HA_Float ha_fAlpha, ha_fBeta;
	HA_Float pha_fS_sum, pha_fZ_sum;

	if(num_src>0){
		const int num_fft          = estleak_param.fft_length;
		const HA_Float fAlp_leak   = estleak_param.leak_factor;
		const HA_Float calcedBeta  = calcBeta[num_src];
		const HA_Float calcedAlpha = calcAlpha[num_src];
		HA_Float in_mirror[num_fft][num_src];
		// Regacy codes
		// /* alpha, betaの計算 */
		//ha_fBeta = (HA_Float)(-1.0f * fAlp_leak / (1.0f - fAlp_leak * fAlp_leak + fAlp_leak * (1.0f - fAlp_leak) * ((HA_Float)(num_src - 2))));
		//ha_fAlpha = (HA_Float)(1.0f - (HA_Float)(num_src - 1) * fAlp_leak * ha_fBeta);
		//LOGI("fBeta=%f, fAlpha=%f\n", ha_fBeta, ha_fAlpha); // debug
		//
		//for(f=0; f<num_fft; f++){
		//	/* S_sumの計算 */
		//	pha_fS_sum[f] = 0; // S_sumの初期化
		//	for(s=0; s<num_src; s++){
		//		pha_fS_sum[f] += inputSpec[s][f];
		//		//if(f<10){ LOGI("s=%d, f=%d : sum = %f\n", s, f, pha_fS_sum[f]); } // debug
		//	}
		//	/* S_leakの計算 */
		//	for(s=0; s<num_src; s++){
		//		pha_fS_leak[s][f] = pha_fS_sum[f] - inputSpec[s][f];
		//		//if(f<10){ LOGI("s=%d, f=%d : leak = %f\n", s, f, pha_fS_leak[s][f]); } // debug
		//	}
		//	/* Zの計算 */
		//	for(s=0; s<num_src; s++){
		//		pfsZ[s][f] = ha_fAlpha * inputSpec[s][f] + ha_fBeta * pha_fS_leak[s][f];
		//		if(pfsZ[s][f] < 1.0){ pfsZ[s][f] = 1.0; }
		//		//if(f<10){ LOGI("s=%d, f=%d : pfsZ = %f, %f, %f\n", s, f, pfsZ[s][f], inputSpec[s][f], pha_fS_leak[s][f]); } // debug
		//	}
		//	/* Z_sumの計算 */
		//	pha_fZ_sum[f] = 0; // Z_sumの初期化
		//	for(s=0; s<num_src; s++){
		//		pha_fZ_sum[f] += pfsZ[s][f];
		//	}
		//	/* lamda_leakの計算 */
		//	for(s=0; s<num_src; s++){
		//		//pha_fLamda_leak[s][f] = LOCAL_DEFAULT_LEAK_FACTOR * (pha_fZ_sum[f] - pfsZ[s][f]);
		//		outputSpec[s][f] = LOCAL_DEFAULT_LEAK_FACTOR * (pha_fZ_sum[f] - pfsZ[s][f]);
		//		//if(f<10){ LOGI("Lamda_leak = %f\n", outputSpec[s][f]); } // debug
		//	}
		//}
		for(f=0; f<num_fft; f++){
			for(s=0; s<num_src; s++){
				in_mirror[f][s] = inputSpec[s][f];
			}
		}
		for(f=0; f<num_fft; f++){
			/* S_sumの計算 */
			pha_fS_sum = 0.0f; // S_sumの初期化
			for(s=0; s<num_src; s++){
				//pha_fS_sum += inputSpec[s][f];
				pha_fS_sum += in_mirror[f][s];
				//if(f<10){ LOGI("s=%d, f=%d : sum = %f\n", s, f, pha_fS_sum); } // debug
			}
			/* S_leakの計算, Zの計算, Z_sumの計算を全て一括処理 */
			pha_fZ_sum = 0.0f; // Z_sumの初期化
			for(s=0; s<num_src; s++){
				//pfsZ[s]  = calcedAlpha * inputSpec[s][f];
				//pfsZ[s] += calcedBeta * (pha_fS_sum - inputSpec[s][f]);
				pfsZ[s]  = calcedAlpha * in_mirror[f][s];
				pfsZ[s] += calcedBeta * (pha_fS_sum - in_mirror[f][s]);
				if(pfsZ[s] < 1.0f){ pfsZ[s] = 1.0f; }
				//if(f<10){ LOGI("s=%d, f=%d : pfsZ=%f, input=%f, leak=%f\n", s, f, pfsZ[s], inputSpec[s][f], (pha_fS_sum - inputSpec[s][f])); } // debug
				pha_fZ_sum += pfsZ[s];
			}
			/* lamda_leakの計算 */
			for(s=0; s<num_src; s++){
				outputSpec[s][f] = fAlp_leak * (pha_fZ_sum - pfsZ[s]);
				//if(f<10){ LOGI("Lamda_leak=%f\n", outputSpec[s][f]); } // debug
			}
		}
	}
	return(OK);
}
#endif // BUILD_MODULE_ESTIMATE_LEAK

// EOF
