#include "cgs.h"
#include "ofdm_system.h"
#include "fft.h"
//#include "fft_c.h"

/* function : ofdm frame de_package */
void	initial_matrix(row,column,matrix)
int	row;
int	column;
Complex *matrix;
{
	int	i,j;
	for (i = 0;i < row ; i++){
		for (j = 0;j < column ; j++){
			matrix[i*column+j].real = 0;
			matrix[i*column+j].imag = 0;
		}
	}
}

void	initial_vector(column, vector)
int column;
double *vector;
{
	int i;
	for (i = 0; i < column ; i++){
			vector[i] = 0;
	}
}
void ofdm_depak(ofdm_buffer, data_buffer, pilot_buffer)
Complex *ofdm_buffer;
Complex *data_buffer;
Complex *pilot_buffer;
{
	int i, d_idx, p_idx;
	double 	xreal[SUBCARRIERS], ximag[SUBCARRIERS], 
			yreal[SUBCARRIERS], yimag[SUBCARRIERS];
	
	/* remove cp */
	for (i = 0; i < SUBCARRIERS; i++) {
		xreal[i] = ofdm_buffer[CP_LENGTH+i].real;
		ximag[i] = ofdm_buffer[CP_LENGTH+i].imag;
	}
	
	/* perform fft */
	fft(SUBCARRIERS, xreal, ximag, yreal, yimag);
	
	/* seperate data and pilot */
	d_idx = 0;
	for (i = 0; i < SUBCARRIERS; i++) {
		if ((i-PILOT_PHASE) % PILOT_GAP ==  0) {
			p_idx = (int) (i-PILOT_PHASE) / PILOT_GAP;
			pilot_buffer[p_idx].real = yreal[i];
			pilot_buffer[p_idx].imag = yimag[i];
		}
		else {
			if (i >= FREQ_GUARD_LEN && i < (SUBCARRIERS-FREQ_GUARD_LEN)) {
				data_buffer[d_idx].real = yreal[i];
				data_buffer[d_idx].imag = yimag[i];
				d_idx++;
			}
		}
	}
	
	/* demodulate pilot */
	for (i = 0; i < PILOT_LENGTH; i++) {
		pilot_buffer[i] = c_mul(pilot_buffer[i], NormPilot[i]);
		//pilot_buffer[i] = c_dvd(pilot_buffer[i], cVpilot[i]);
	}
	
}



/* function : frequency response --> impulse response */
void freq2tau(pilot_buf)
Complex *pilot_buf;
{
	int i;
	double 	xreal[PILOT_LENGTH], ximag[PILOT_LENGTH], 
			yreal[PILOT_LENGTH], yimag[PILOT_LENGTH];
	
	for (i = 0; i < PILOT_LENGTH; i++) {
		xreal[i] = pilot_buf[i].real;
		ximag[i] = pilot_buf[i].imag;
	}
	ifft(PILOT_LENGTH, xreal, ximag, yreal, yimag);

	for (i = 0; i < PILOT_LENGTH; i++) {
		pilot_buf[i].real = yreal[i];
		pilot_buf[i].imag = yimag[i];
	}

}



/* function : MST algorithm */
void channel_mst(pilot_rev, pilot_proc)
Complex *pilot_rev;
Complex *pilot_proc;
{
	int i, j, max_idx;
	double max_value;
	int tap_idx[MST_TAP_LEN];
	double tap_power[PILOT_LENGTH];
	
	/* caculate each tap's power */
	for (i = 0; i < PILOT_LENGTH; i++) {
		tap_power[i] = pilot_rev[i].real * pilot_rev[i].real + pilot_rev[i].imag * pilot_rev[i].imag;
	}

	/* search for most significant taps */
	for (j = 0; j < MST_TAP_LEN; j++) {

		max_idx = 0;
		max_value = tap_power[max_idx];
		for (i = 1; i < PILOT_LENGTH; i++) { 
			if (tap_power[i] > max_value) {
				max_value = tap_power[i];
				max_idx = i;
			}
		}
		tap_idx[j] = max_idx;
		tap_power[max_idx] = 0;
	}
	
	/* zero force at none MST */
	for (i = 0; i < PILOT_LENGTH; i++) {
		pilot_proc[i].real = 0;
		pilot_proc[i].imag = 0;
	}
	for (j = 0; j < MST_TAP_LEN; j++) {
		pilot_proc[tap_idx[j]] = pilot_rev[tap_idx[j]];
	}

}



/* function : MST + fd_filter algorithm */
void channel_filt(pilot_rev, pilot_proc1, pilot_proc2, win_inc)
Complex *pilot_rev;
Complex *pilot_proc1;
Complex *pilot_proc2;
double win_inc;
{
	int i, j, k, max_idx, tap_count;
	int tap_idx[PILOT_LENGTH];
	double tap_power[PILOT_LENGTH];
	double tot_power, thresh_v, max_value;
	double enta_1, enta_2, enta_3;
	double pow_t, pow_s;

	int win_len, mst_method;
	double win_pow;
	double omiga_pow[1024];

	double 	xreal[1024], ximag[1024], 
			yreal[1024], yimag[1024];

	mst_method = 2;
	pow_t = 0;
	enta_1 = 1.0e-3;

	/* caculate each tap's avarage power */
	for (j = 0; j < PILOT_LENGTH; j++) {
	
		tap_power[j] = 0;
		for (i = 0; i < 1024; i++) {
			tap_power[j] = tap_power[j] + pilot_rev[j+i*PILOT_LENGTH].real * pilot_rev[j+i*PILOT_LENGTH].real + pilot_rev[j+i*PILOT_LENGTH].imag * pilot_rev[j+i*PILOT_LENGTH].imag;
		}
		tap_power[j] = tap_power[j] / 1024;
		pow_t = pow_t + tap_power[j];
	}
	
	if (mst_method == 1) {

		/* MST search according to avarage power */
		thresh_v = pow_t * enta_1;
		tap_count = 0;
		for (j = 0; j < PILOT_LENGTH; j++) {
			if (tap_power[j] >= thresh_v) {
	
				tap_idx[tap_count] = j;
				tap_count++;

				for (i = 0; i < 1024; i++) {
					pilot_proc1[j+i*PILOT_LENGTH] = pilot_rev[j+i*PILOT_LENGTH];
				}
			}
			else {
				for (i = 0; i < 1024; i++) {
					pilot_proc1[j+i*PILOT_LENGTH].real = 0;
					pilot_proc1[j+i*PILOT_LENGTH].imag = 0;
	
					pilot_proc2[j+i*PILOT_LENGTH].real = 0;
					pilot_proc2[j+i*PILOT_LENGTH].imag = 0;
				}
			}
		}
		
		/* search for max point */
		max_idx = tap_idx[0];
		max_value = tap_power[max_idx];
		for (j = 1; j < tap_count; j++) {
			if (tap_power[tap_idx[j]] > max_value) {
				max_idx = tap_idx[j];
				max_value = tap_power[max_idx];
			}
		}		
	}
	else {
		
		/* MST search according to tap number */
		tap_count = MST_TAP_LEN;
		
		/* search for most significant taps */
		pow_s = 0;
		for (j = 0; j < tap_count; j++) {
	
			max_idx = 0;
			max_value = tap_power[max_idx];
			for (i = 1; i < PILOT_LENGTH; i++) { 
				if (tap_power[i] > max_value) {
					max_value = tap_power[i];
					max_idx = i;
				}
			}
			pow_s = pow_s + tap_power[max_idx];		// signal power
			tap_idx[j] = max_idx;
			tap_power[max_idx] = 0;
		}
		max_idx = tap_idx[0];

		/* zero force at none MST */
		for (j = 0; j < PILOT_LENGTH; j++) {
			
			for (i = 0; i < 1024; i++) {
				pilot_proc1[j+i*PILOT_LENGTH].real = 0;
				pilot_proc1[j+i*PILOT_LENGTH].imag = 0;
				
				pilot_proc2[j+i*PILOT_LENGTH].real = 0;
				pilot_proc2[j+i*PILOT_LENGTH].imag = 0;
			}
		}
		
		for (j = 0; j < tap_count; j++) {
			
			for (i = 0; i < 1024; i++) {
				pilot_proc1[tap_idx[j]+i*PILOT_LENGTH] = pilot_rev[tap_idx[j]+i*PILOT_LENGTH];
			}
		}
	}

	/* fd spread domain process */
	/* step1: estimate fd spread */
	for (i = 0; i < 1024; i++) {
		xreal[i] = pilot_proc1[i*PILOT_LENGTH+max_idx].real;
		ximag[i] = pilot_proc1[i*PILOT_LENGTH+max_idx].imag;
	}

	fft(1024, xreal, ximag, yreal, yimag);
	tot_power = 0;
	for (i = 0; i < 1024; i++) {
		omiga_pow[i] = yreal[i] * yreal[i] + yimag[i] * yimag[i];
		tot_power = tot_power + omiga_pow[i];
	}

	/* estimate window length */
	enta_2 = FD_FILT_ENTA;
	win_len = 1;
	win_pow = omiga_pow[0];
	for (i = 1; i <= win_len; i++) {
		win_pow = win_pow + omiga_pow[i] + omiga_pow[1024-i];
	}

	while(win_pow < enta_2*tot_power) {
		win_len++;
		win_pow = win_pow + omiga_pow[win_len] + omiga_pow[1024-win_len];
	}
//	win_len = win_len + win_inc;

	/* estimate snr */
	enta_3 = 10*log10(pow_s/(pow_t - pow_s)) - 2;

	/* caculate window length */
	win_len = win_len + 10 + (int) floor(enta_3*win_inc);

	for (j = 0; j < tap_count; j++) { 
		
		k = tap_idx[j];
		for (i = 0; i < 1024; i++) {
			xreal[i] = pilot_proc1[i*PILOT_LENGTH+k].real;
			ximag[i] = pilot_proc1[i*PILOT_LENGTH+k].imag;
		}

		fft(1024, xreal, ximag, yreal, yimag);

		for (i = win_len+1; i <= 1024-win_len-1; i++) {
			yreal[i] = 0;
			yimag[i] = 0;
		}
		ifft(1024, yreal, yimag, xreal, ximag);

		for (i = 0; i < 1024; i++) {
			pilot_proc2[i*PILOT_LENGTH+k].real = xreal[i];
			pilot_proc2[i*PILOT_LENGTH+k].imag = ximag[i];
		}

	}

	/* log each tap's avarage power to file 
	{
		FILE *log_fp;
		
		log_fp = fopen(LOG_TAP_STP2, "a+");
		
		fprintf(log_fp, "%d\n", win_len);

		fclose(log_fp);
	}*/
}

/*function : channel transmission */
void	channel_trans(pilot_rev, pilot_proc)
Complex *pilot_rev; 
Complex *pilot_proc;
{
	int	i,j;
	int	tmp_ind;
	for (i = 0; i < 1024; i++){
		tmp_ind = i - fmod(i,inter_K);
		for (j = 0; j < PILOT_LENGTH; j++){
			pilot_proc[i*PILOT_LENGTH + j].real = pilot_rev[tmp_ind*PILOT_LENGTH + j].real;
			pilot_proc[i*PILOT_LENGTH + j].imag = pilot_rev[tmp_ind*PILOT_LENGTH + j].imag;
		}
	}
}
/*function : sub-block estimation algorithm*/
void	sub_esti_poly(pilot_rev, pilot_proc1, pilot_proc2)
Complex *pilot_rev;
Complex *pilot_proc1;
Complex *pilot_proc2;
{
	int i, j, k, max_idx, tap_count;
	int	tmp_i,tmp_j,tmp_k;
	double	tmp_real,tmp_imag;
	int tap_idx[PILOT_LENGTH];
	double tap_power[PILOT_LENGTH];
	double tot_power, thresh_v, max_value;
	double enta_1, enta_2, enta_3;
	double pow_t, pow_s;
	int win_len, mst_method;
	double win_pow;
	double omiga_pow[repeat_M];

	double 	xreal[repeat_M], ximag[repeat_M], 
			yreal[repeat_M], yimag[repeat_M];
	
	double	multi_matrix[poly_g+1][repeat_M];
	Complex	factor_matrix[poly_g+1][PILOT_LENGTH];
	double	recov_matrix[sub_clk_size][poly_g+1];
	double	tmp_v;
	int	tmp_clk_size = sub_clk_size;
	FILE	*fp;
	char	* filename;
	float	tmp_fv;
	mst_method = 2;
	pow_t = 0;
	enta_1 = 1.0e-3;

	/* caculate each tap's avarage power */
	for (j = 0; j < PILOT_LENGTH; j++) {
	
		tap_power[j] = 0;
		for (i = 0; i < repeat_M; i++) {
			tap_power[j] = tap_power[j] + pilot_rev[j+i*inter_K*PILOT_LENGTH].real * pilot_rev[j+i*inter_K*PILOT_LENGTH].real + pilot_rev[j+i*inter_K*PILOT_LENGTH].imag * pilot_rev[j+i*inter_K*PILOT_LENGTH].imag;
		}
		tap_power[j] = tap_power[j] / repeat_M;
		pow_t = pow_t + tap_power[j];
	}
	
	if (mst_method == 1) {

		for (j = 0; j < PILOT_LENGTH; j++) {
			
			for (i = 0; i < sub_clk_size; i++) {
				pilot_proc1[j+i*PILOT_LENGTH].real = 0;
				pilot_proc1[j+i*PILOT_LENGTH].imag = 0;
				
				pilot_proc2[j+i*PILOT_LENGTH].real = 0;
				pilot_proc2[j+i*PILOT_LENGTH].imag = 0;
			}
		}
		/* MST search according to avarage power */
		thresh_v = pow_t * enta_1;
		tap_count = 0;
		for (j = 0; j < PILOT_LENGTH; j++) {