#include <stdio.h>
#include <math.h>
#include <complex.h>

#define PI 3.14159265358979
#define NUM_SAMPLES 100
#define TESTFREQ 800.0
#define SAMPLING_FREQ 8000.0


//zeromean_normalization
//calculates zero mean normalized signal
//INPUT: COMPLEX *x - array of signal values
//		 COMPLEX *mic_signal_no_dc - array to contain output
void zeromean_normalization(float complex *x, float complex *mic_signal_no_dc)
{
	//subtract mean from signal

	int n;
	float complex mean;
	float complex numel;

	mean = 0.0 + 0.0*I;
	numel = NUM_SAMPLES + 0.0*I;

	printf("subtracting mean...\n");
	for (n = 0; n < NUM_SAMPLES; n++)
	{
		mean += x[n];
	}
	mean = mean/numel;

	for (n = 0; n < NUM_SAMPLES; n++)
	{
		mic_signal_no_dc[n] = x[n] - mean;
	}
	printf("mean subtracted!\n");
	//normalize signal
	//find norm(euclidean length) of variable
	printf("normalizing...\n");
	float complex norm = 0.0 + 0.0*I;
	for (n = 0; n < NUM_SAMPLES; n++)
	{
		norm += pow(crealf(mic_signal_no_dc[n]),2)+0.0*I;
	}
	norm = csqrtf(norm);

	for (n = 0; n < NUM_SAMPLES; n++)
	{

			mic_signal_no_dc[n] = mic_signal_no_dc[n]/norm;
	}
	printf("normalized!\n");
	return;

}

void dft(float complex *x)
{
	float complex result[NUM_SAMPLES];
	int k,n;

	for (k=0; k<NUM_SAMPLES; k++)
	{
		result[k] = 0.0 + 0.0*I;

		for (n=0; n<NUM_SAMPLES; n++)
		{
			result[k] += crealf(x[n])*cos(2*PI*k*n/NUM_SAMPLES) + cimagf(x[n])*sin(2*PI*k*n/NUM_SAMPLES)
					+(cimagf(x[n])*cos(2*PI*k*n/NUM_SAMPLES) - crealf(x[n])*sin(2*PI*k*n/NUM_SAMPLES))*I;
		}
	}
	for (k=0;k<NUM_SAMPLES;k++)
	{
		x[k] = result[k];
	}
}
void idft(float complex *x)
{
	float complex result[NUM_SAMPLES];
	int k,n;

	for (k=0; k<NUM_SAMPLES; k++)
	{
		result[k] = 0.0 + 0.0*I;

		for (n=0; n<NUM_SAMPLES; n++)
		{
			result[k] += crealf(x[n])*cos(2*PI*k*n/NUM_SAMPLES) + cimagf(x[n])*sin(2*PI*k*n/NUM_SAMPLES)
					-(cimagf(x[n])*cos(2*PI*k*n/NUM_SAMPLES) - crealf(x[n])*sin(2*PI*k*n/NUM_SAMPLES))*I;
		}
	}
	for (k=0;k<NUM_SAMPLES;k++)
	{
		x[k] = result[k]/NUM_SAMPLES;
	}
}
/*
*GCC
 * computes generalized cross correlation on 2 signals to find the time delay
 * INPUT: both signals are in the fourier domain already
 * 			mic_signal2 is reference mic
 * OUPUT: returns time delay in samples
 */
int GCC(float complex *mic_signal1,float complex *mic_signal2)
{
	//find complex conjugate of reference mic
	int i;
	float complex correlation[NUM_SAMPLES];
	for(i = 0; i < NUM_SAMPLES; i++)
	{
		mic_signal2[i] = conjf(mic_signal2[i]);
	}

	//calculate GCC
	for(i=0;i<NUM_SAMPLES;i++)
	{
		correlation[i] = mic_signal1[i]*mic_signal2[i];
	}
	//switch to PHAT
	//calculate denominator, max(abs(GCC))
	float denom = 1*powf(10,-16);
	for(i=0;i<NUM_SAMPLES;i++)
	{
		if(cabsf(correlation[i])>denom)
		{
			denom = cabsf(correlation[i]);
		}
	}
	//PHAT calculation
	for(i = 0; i<NUM_SAMPLES; i++)
	{
		correlation[i] = correlation[i]/denom;
	}

	//ifft, only keep real part
	float delay[NUM_SAMPLES];
	idft(correlation);
	for(i=0; i<NUM_SAMPLES; i++)
	{
		delay[i] = crealf(correlation[i]);
	}
	return 0;
}

int main(void) {

	//initialize starting curve
	int n;
	int delay;
	float complex samples[NUM_SAMPLES];
	float complex samples2[NUM_SAMPLES];
	int delay1, delay2;
	float complex signal_norm[NUM_SAMPLES];
	float complex signal_norm2[NUM_SAMPLES];

	printf("creating samples...\n");
	delay1 = 75;
	delay2 = 50;
	for(n = 0; n <NUM_SAMPLES; n++)
	{
		samples[n] = 0.0+0.0*I;
		samples2[n] = 0.0+0.0*I;
		if(n == delay1)
			samples[n] = 1.0+0.0*I;
		if(n == delay2)
			samples2[n] = 1.0+0.0*I;
	}


	printf("samples created!\n");

	//data preprocessing

	zeromean_normalization(samples,signal_norm);
	zeromean_normalization(samples2,signal_norm2);
	
	//find fft
	dft(signal_norm);
	dft(signal_norm2);
	printf("fft calculated!\n");

	//calculate GCC
	printf("calculating GCC");

	delay = GCC(signal_norm,signal_norm2);
	printf("GCC calculated");

	return 0;
}