
/************************************************************
**	Synthesize.c
**
**  Private :
**
**  History :
**     2013.05.14 M.Takigahira Build有無指定に対応
**     2013.05.07 M.Takigahira 処理精査、インデント修正、不要なloop内ifを排除、
**                             #defineで固定となっていたパラメータの変数化
**     2012.12.17 kama@SIF     新規作成(V1.0)
**
**  Description
**  -----------
**  周波数領域の信号を時間領域の波形に変換する
**
*/

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

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

#if !defined BUILD_MODULE_SYNTHESIZE
///////////////////////////////////////////////////////////////
//  Dummy Functions for build empty methods.
void CalcConjWindow(float *w, int length);
int HASetParamSynthesize(Param_Synthesize_t *param){ return 0; }
int HAGetParamSynthesize(Param_Synthesize_t *param){ return 0; }
void makeConjWindow(void){ }
void fft(HA_Complex spec[ARY_NUM_FFT], HA_Complex complexWave[ARY_NUM_FFT]){ }
void ifft(HA_Complex A[ARY_NUM_FFT], HA_Float wave[ARY_NUM_WAVE]){ }
int Synthesize(int num_src, int count, HA_Complex inputSpec[ARY_NUM_SRC][ARY_NUM_FFT], HA_Float outputWave[ARY_NUM_SRC][ARY_NUM_WAVE]){ return 0; }
#else // #if defined BUILD_MODULE_SYNTHESIZE
///////////////////////////////////////////////////////////////
//  Parameter

//#define	min_freq_ind	(4)
//#define	max_freq_ind	(252)
//#define	m_tlength		(256)
//#define output_gain     (0.00005)
#define LOCAL_DEFAULT_SYNTHESIZE_MIN_FREQ_IND  4
#define LOCAL_DEFAULT_SYNTHESIZE_MAX_FREQ_IND  252
#define LOCAL_DEFAULT_SYNTHESIZE_TLENGTH       256
#define LOCAL_DEFAULT_SYNTHESIZE_OUTPUT_GAIN   1.0f // 0.00005f
/*
static int min_freq_ind = 4;
static int max_freq_ind = 252;
static int m_tlength    = 256;
static float synthesize_output_gain = 1.0f; // 0.00005;
*/
static Param_Synthesize_t synthesize_param = {
	ARY_NUM_WAVE,
	ARY_NUM_FFT,
	LOCAL_DEFAULT_SYNTHESIZE_MIN_FREQ_IND,
	LOCAL_DEFAULT_SYNTHESIZE_MAX_FREQ_IND,
	LOCAL_DEFAULT_SYNTHESIZE_TLENGTH,
	LOCAL_DEFAULT_SYNTHESIZE_OUTPUT_GAIN
};

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

static float _synthesize_window[ARY_NUM_WAVE];

void CalcConjWindow(float *w, int length);

///////////////////////////////////////////////////////////////
//	MAIN

int HASetParamSynthesize(Param_Synthesize_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->min_freq < 0) || (param->max_freq >= ARY_NUM_FFT) || (param->max_freq < param->min_freq)){
		return -1;
	}
	if(param->tlength < 1){
		return -1;
	}
	if(param->output_gain <= 0.0f){
		return -1;
	}
	synthesize_param = *param;
	return 0;
}
int HAGetParamSynthesize(Param_Synthesize_t *param)
{
	*param = synthesize_param;
	return 0;
}

void makeConjWindow(void){
	float *window = _synthesize_window;
	CalcConjWindow(window, ARY_NUM_WAVE);
}

void fft
	(
	HA_Complex	spec[ARY_NUM_FFT],
	HA_Complex	complexWave[ARY_NUM_FFT]
	)
{
	int m, mh, i, j, k, irev, f; // remove variable , prev_irev;
    const int n = synthesize_param.tlength;		// <- tlength
    const float rad_coef = M_PI * 2.0 / n;
    const int num_fft = synthesize_param.fft_length;
	HA_Complex	y; // This variable use as a temporary variable in the scrambling process.
	HA_Complex	w;
	HA_Complex	a[ARY_NUM_FFT];

	for(f=0; f<num_fft; f++){
		a[f].re = spec[f].re;
		a[f].im = spec[f].im;
	}

	// FFT main
    i = 0;
	for(j=1; j<n-1; j++){
		//LOGI("j=%d : a[j](%f, %f)\n", j, a[j].re, a[j].im);
        for(k=n>>1; k>(i^=k); k>>=1);
        if(j<i){
        	//std::swap(a[i], a[j]);
			//LOGI("%d : %d, %f, %f\n",i, j, spec[i-1].re, spec[j-1].re);
			y.re = a[i].re;
			y.im = a[i].im;
			a[i].re = a[j].re;
			a[i].im = a[j].im;
			a[j].re = y.re;
			a[j].im = y.im;
			//LOGI("swap i=%d, j=%d : a[i](%f, %f), a[j](%f, %f)\n", i, j, a[i].re, a[i].im, a[j].re, a[j].im);
		}
		//LOGI("j=%d : a[j](%f, %f)\n", j, a[j].re, a[j].im);
    }

    for(mh=1; (m=mh<<1)<=n; mh=m){
        irev = 0;
        for(i=0; i<n; i+=m){
            ////prev_irev = irev;
            w.re = cos(rad_coef * irev);
            w.im = sin(rad_coef * irev);
//			LOGI("i=%d : cos_coef=%f, sin_coef=%f\n", i, cos_coef, sin_coef);

            for(k=n>>2; k>(irev^=k); k>>=1);
            for(j=i; j<mh+i; j++){
				k = j + mh;

				y.re = a[j].re - a[k].re;
				y.im = a[j].im - a[k].im;
				a[j].re += a[k].re;
				a[j].im += a[k].im;

//				a[k]  = std::conj(w[prev_irev]) * y;
				////a[k].re = cos(M_PI*2.0 * prev_irev/n) * y.re - sin(M_PI*2.0 * prev_irev/n) * y.im;
				////a[k].im = cos(M_PI*2.0 * prev_irev/n) * y.im + sin(M_PI*2.0 * prev_irev/n) * y.re;
				a[k].re  = w.re * y.re;
				a[k].re -= w.im * y.im;
				a[k].im  = w.re * y.im;
				a[k].im += w.im * y.re;
//				LOGI("k=%d : y(%f, %f), a[k](%f, %f)\n", k, y.re, y.im, a[k].re, a[k].im);
			}
		}
	}

	for(f=0; f<num_fft; f++){
		complexWave[f].re = a[f].re;
		complexWave[f].im = a[f].im;
	}
}

void ifft
	(
	HA_Complex	A[ARY_NUM_FFT],
	HA_Float	wave[ARY_NUM_WAVE]
	)
{
	int i, j; // remove variable , k;
	HA_Complex	x, y, z;
	HA_Complex	mA[ARY_NUM_FFT];
	HA_Complex	mA2[ARY_NUM_FFT];

	const int n = synthesize_param.length;
	const int n_2 = n / 2;
	const int n_4 = n / 4;
	const float rad_coef = M_PI * 2.0 / n;

//	m_fft.A[0] = (T)0.5 * std::complex<T>((A[0].real() + A[n_2].real()), (A[0].real() - A[n_2].real()));
//	m_fft.A[n_4] = conj(A[n_4]);

//	for(i=1; i<n_2; i++){
//        LOGI("i=%d : A[i](%f, %f)\n", i, A[i].re, A[i].im);
//	}

	mA[0].re = 0.5 * A[0].re + A[n_2].re;
	mA[0].im = 0.5 * A[0].re - A[n_2].re;

	mA[n_4].re =  A[n_4].re;
	mA[n_4].im = -A[n_4].im;

	for(i=1, j=n_2-1; i<n_4; i++, j--){ // [j] same as [n_2-i]
//		std::complex<T> y = (T)0.5 * std::complex<T>(1.0 + w[i].imag(), w[i].real()) * (A[i] - conj(A[n_2 - i]));
//		m_fft.A[i] = conj(A[n_2 - i]) + y;
//		m_fft.A[n_2 - i] = conj(A[i] - y);

		////x.re = 1.0 - sin(M_PI*2.0 * i/ARY_NUM_WAVE);
		////x.im = cos(M_PI*2.0 * i/ARY_NUM_WAVE);
		x.re = 1.0f - sin(rad_coef * i);
		x.im = cos(rad_coef * i);

		z.re = A[i].re - A[j].re;
		z.im = A[i].im + A[j].im;

		//y.re = 0.5 * (x.re * z.re - x.im * z.im);
		//y.im = 0.5 * (x.re * z.im + x.im * z.re);
		y.re  = (x.re * z.re);
		y.re -= (x.im * z.im);
		y.re *= 0.5f;
		y.im  = (x.re * z.im);
		y.im += (x.im * z.re);
		y.im *= 0.5f;
//		LOGI("i=%d : x(%f, %f), z(%f, %f), y(%f, %f)\n", i, x.re, x.im, z.re, z.im, y.re, y.im);

		mA[i].re = A[j].re + y.re;
		mA[i].im = -A[j].im + y.im;

		mA[j].re = A[i].re - y.re;
		mA[j].im = -(A[i].im - y.im);
//		LOGI("i=%d : mA[i](%f, %f), y(%f, %f), mA[n/2-i](%f, %f)\n", i, mA[i].re, mA[i].im, y.re, y.im, mA[j].re, mA[j].im);
	}

	fft(mA, mA2);

	for(i=0, j=0; i<n_2; i++, j+=2){ // [j] same as [i*2]
		wave[j]   = mA2[i].re * 2;
		wave[j+1] = mA2[i].im * 2;
//		LOGI("i=%d : wave(%f, %f)\n", i, wave[j], wave[j+1]);
	}
}

int Synthesize
	(
	int num_src,
	int count,
	HA_Complex	inputSpec[ARY_NUM_SRC][ARY_NUM_FFT],
	HA_Float	outputWave[ARY_NUM_SRC][ARY_NUM_WAVE]
	)
{
	int s, f, w; // SRC=s, FFT=f, WAVE=w
	float coef = 1.0f / ARY_NUM_WAVE;
	HA_Complex	spec[ARY_NUM_FFT];
	HA_Float	buf[ARY_NUM_WAVE];
	FILE	*fp;
	const int num_wave = synthesize_param.length;
	const int num_fft = synthesize_param.fft_length;
	const int min_freq_ind = synthesize_param.min_freq;
	const int max_freq_ind = synthesize_param.max_freq;
#ifdef NEVER
	float	tmp;

	// ファイルからリード(REAL)
	if(NULL == (fp = fopen("real.txt","r"))){
		LOGI("ERROR : FFT file cannot open.\n");
		return(ERROR);
	}
	for(f=0; f<255; f++){
		fscanf(fp,"%f",&tmp);
		spec[f].re = tmp;
	}
	fclose(fp);

	// ファイルからリード(IMAG)
	if(NULL == (fp = fopen("imag.txt","r"))){
		LOGI("ERROR : FFT file cannot open.\n");
		return(ERROR);
	}
	for(f=0; f<255; f++){
		fscanf(fp,"%f",&tmp);
		spec[f].im = tmp;
	}
	fclose(fp);
#endif

	// Synthesize MAIN
	for(s=0; s<num_src; s++){
		//for(f=0; f<257; f++){
		//	spec[f].re = inputSpec[n][f].re;
		//	spec[f].im = inputSpec[n][f].im;
		//	LOGI("f=%d : spec(%f, %f)\n", f, spec[f].re, spec[f].im);
		//}
		//for(f=0; f<num_fft; f++){
   	    //	if(f<min_freq_ind || f>max_freq_ind){
		//		spec[f].re = 0.0f;
		//		spec[f].im = 0.0f;
   	    //	}
   		//}
		f=0;
		for(; f<min_freq_ind; f++){ // RANGE : 0 <= f < min_freq_ind
			spec[f].re = 0.0f;
			spec[f].im = 0.0f;
   		}
		for(; f<=max_freq_ind; f++){ // RANGE : min_freq_ind <= f <= max_freq_ind
			spec[f].re = inputSpec[s][f].re;
			spec[f].im = inputSpec[s][f].im;
		}
		for(; f<num_fft; f++){ // RANGE : max_freq_ind < f < ARY_NUM_FFT
			spec[f].re = 0.0f;
			spec[f].im = 0.0f;
   		}
	    ifft(spec, buf);
		float *window = _synthesize_window;
		//CalcConjWindow(window, ARY_NUM_WAVE);

		for(w=0; w<num_wave; w++){
			//(*wave)[w] = rfft1.a[w] * (1.0f / rfft1.tlength()) * window[w];
			//void Hamming(boost::numeric::ublas::vector<T>& w){
			//	float c = 2.0 * M_PI / (w.size() - 1);
			//	for(int n=0; n<w.size(); n++){ w[n] = 0.54 - 0.46 * cos(c * n); }
			//}

			//float c = 2.0 * M_PI / (num_wave-1);
			//float window = 0.54 - 0.46 * cos(c * w);	// Hamming window

			//outputWave[s][w] = buf[w] * (1.0f/num_wave) * window[w] * output_gain;
			outputWave[s][w] = buf[w] * coef * window[w];

			//LOGI("%f\n",outputWave[s][w]);
			//if(i<10){ LOGI("%f\n",buf[w]); }
		}

#ifdef NEVER
		// ファイルへライト
		if(NULL == (fp = fopen("wav.raw","wb"))){
			LOGI("ERROR : FFT file cannot open.\n");
			return(ERROR);
		}
		for(w=0; w<num_wave; w++){
			short d = buf[w] * 32767;
			fwrite(&d, 2, 1, fp);
		}
		fclose(fp);
#endif
	}

#ifdef NEVER
    RCPtr<Map<int, ObjectRef> > output(new Map<int, ObjectRef>);
    Buffer& outputBuffer = *(outputs[outputID].buffer);
    outputBuffer[count] = output;

    // Perform overlap add for 0 time frame.
    if(count == 0){
      	for(int i=0; i<overlap_frame; i++){ 	// <- ovrelap=3
        	SynthesizeFrame(i);

        	const Map<int, ObjectRef>& wave_map = object_cast<Map<int, ObjectRef> >((*wave_buffer)[i]);

			Map<int, ObjectRef>::const_iterator it = wave_map.begin();

        	int begin = 0;
        	int end = length - advance * (i + 1);
        	int lf_begin = advance * i;
        	int lf_end = length - advance;
                
        	for (; it != wave_map.end(); ++it){
          		const Vector<float>& w = object_cast<Vector<float> >(it->second);
	          	vector<float>& lf = last_frame[it->first];

          		lf.resize(length, 0.0f);

          		for (int i = 0; i < end; i++){
            		lf[lf_begin + i] += w[i];
          		}
        	}
      	}
    }

    SynthesizeFrame(count + overlap_frame);

	// Perform overlap add.
    for(int frame=0; frame<=overlap_frame; frame++){ 	// <- overlap frame = 3
      	const Map<int, ObjectRef>& wave_map = object_cast<Map<int, ObjectRef> >((*wave_buffer)[count+frame]);
     	Map<int, ObjectRef>::const_iterator it = wave_map.begin();
            
      	int begin = max(length - advance * (frame + 1), 0);
      	int end = length - advance * frame;
      	int lf_begin = length - (end - begin);

      	for(; it != wave_map.end(); ++it){
        	const Vector<float>& w = object_cast<Vector<float> >(it->second);
        	vector<float>& lf = last_frame[it->first];
        	lf.resize(length, 0.0f);

        	for (int i = 0; i < end - begin; i++){
          		lf[lf_begin + i] += w[begin + i];
        	}
      	}
    }

    // Remove sound sources which have gone from the last_frame map.
    const Map<int, ObjectRef>& wave_map = object_cast<Map<int, ObjectRef> >((*wave_buffer)[count]);
    map<int, vector<float> >::iterator it = last_frame.begin();

    while (it != last_frame.end()){
    	if (wave_map.find(it->first) == wave_map.end()){
        	last_frame.erase(it++);
      	}
    	else{
        	++it;
      	}
    }
    
    for(map<int, vector<float> >::iterator it_lf = last_frame.begin(); it_lf != last_frame.end(); ++it_lf){
      	RCPtr<Vector<float> > x(Vector<float>::alloc(it_lf->second.size()));
      	copy(it_lf->second.begin(), it_lf->second.end(), x->begin());

      	for(int i=0; i < x->size(); i++){
        	(*x)[i] *= output_gain;
      	}

		(*output)[it_lf->first] = x;
            
      	for (int i = 0; i < length - advance; i++){
        	it_lf->second[i] = it_lf->second[i + advance];
      	}

      	for (int i = length - advance; i < length; i++){
        	it_lf->second[i] = 0.0f;
      	}
    }
#endif
	return(OK);
}
#endif // BUILD_MODULE_SYNTHESIZE

// EOF
