#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>

#include "dirtyd.h"
#include "bytes.h"

#undef GOLD

#define DATA_RATE_HZ (50.0)
#define CHIP_RATE_HZ (1023000.0)
#define IF_HZ (10.7e6)
#define FS_HZ (10.0*IF_HZ)
#define GPS_SAT_ID (10)
#define OFFSETS (10)
#define SEQ_LEN (1023)

WORD parity(WORD x)
{
	x ^= x>>1;
	x ^= x>>2;
	x ^= x>>4;
	x ^= x>>8;
	return (x & 1);
}

typedef struct GPS_CA_CODE SAT;

struct GPS_CA_CODE
{
	int satellite_ID;
	int offset;
	WORD size;
	WORD since_epoch;
	WORD g1_state, g2_state;
	WORD phase_mask;
	WORD g1_poly, g2_poly;
	WORD load;
	WORD chip;
	WORD phase;
	int *sequence;
};

int SAT_b1[] = {10,
                 2, 3, 4, 5, 1, 2, 1, 2,
				 3, 2, 3, 5, 6, 7, 8, 9,
				 1, 2, 3, 4, 5, 6, 1, 4,
				 5, 6, 7, 8, 1, 2, 3, 4,
				 5, 4, 1, 2, 4,
                 1, 6, 1, 4, 7, 2, 3, 3, 1};
   
int SAT_b2[] = {10,
                 6, 7, 8, 9, 9,10, 8, 9,
				10, 3, 4, 6, 7, 8, 9,10,
				 4, 5, 6, 7, 8, 9, 3, 6,
				 7, 8, 9,10, 6, 7, 8, 9,
				10,10, 7, 8,10,
				 5,10, 2, 5,10, 4, 5, 9, 10};

int SAT_EpochCheck(SAT *p)
{
	if (p)
		return (p->phase == p->size);
	return -1;
}

int SAT_Chip(SAT *p, int index)
{
	if (p)
	{
		if (index < 0)
			return ((p->chip)? 1 : -1);
		else
			return (p->sequence[index%1023]);
	}
	return 0;
}

int SAT_Advance(SAT *p)
{
	WORD phase;

	if (p)
	{
		// Next State
		p->g1_state += parity((WORD)(p->g1_state&p->g1_poly)) << p->size;
		p->g1_state >>= 1;
		p->g2_state += parity((WORD)(p->g2_state&p->g2_poly)) << p->size;
		p->g2_state >>= 1;

		// G2 Phase Selector Output
		phase = parity((WORD)(p->g2_state&p->phase_mask));

		// Determine Output
		p->chip = (p->g1_state&(WORD)1)^(phase);
		
		// Update Phase Detector
		if (phase)
			p->phase++;
		else
			p->phase = 0;
	}
	return SAT_Chip(p, -1);
}

SAT *SAT_Initialize(SAT *p, int satellite_ID)
{
	WORD phase;
	int i;
	int k;

	if (p)
	{
		p->sequence = NULL;

		satellite_ID %= 47; // Force ID into range of defined PRNs
		p->size = 10;
		p->load = 0x03FF;
		p->g1_state = p->load;
		p->g2_state = p->load;
		p->g1_poly = 0x0481 & p->load;
		p->g2_poly = 0x0597 & p->load;
		p->phase_mask = ((WORD)1 << (p->size-SAT_b1[satellite_ID])) | ((WORD)1 << (p->size-SAT_b2[satellite_ID]));

		// G2 Phase Selector Output
		phase = parity((WORD)(p->g2_state&p->phase_mask));

		// Determine Output
		p->chip = (p->g1_state&(WORD)1)^(phase);

		p->sequence = (int *) malloc(1023*sizeof(int));

		if (p->sequence)
		{
			#ifdef GOLD
			// Generate and store Gold Code sequence
			for (i = 0; i < 1023; i++)
			{
				p->sequence[i] = SAT_Chip(p, -1);
				SAT_Advance(p);
			}
			#else
			// Generate and store "random" sequence
			for (i = 0, k = 0; i < 1023; i++)
			{
				p->sequence[i] = (rand()>(RAND_MAX/2))? 1 : -1;
				k += p->sequence[i]; 
			}
			while (k > 0)
			{
				while (1 != p->sequence[i = rand()%1023])
					;
				p->sequence[i] = -1;
				k -= 2;
			}
			while (k < 0)
			{
				while (-1 != p->sequence[i = rand()%1023])
					;
				p->sequence[i] = 1;
				k += 2;
			}
		#endif
		}
		else
		{
			free (p);
			p = NULL;
		}
	}
	return p;
}

void SAT_GenerateTable(void)
{
	SAT *sat;

	int chips;
	int delay, ten;
	int id;

	sat = (SAT *) malloc(sizeof(SAT));
	if (NULL == sat)
		exit (EXIT_FAILURE);

	// Construct the Cold Code table
	for (id = 0; id <= 46; id++)
	{
		SAT_Initialize(sat, id);
		ten = 0;
		delay = -1;
		chips = 0;
		do
		{
			// Gold Code output
			if (chips < 10)
			{
				ten <<= 1;
				ten += (SAT_Chip(sat, -1)>0);
			}

			// Next State
			SAT_Advance(sat);
			chips++;

			if (SAT_EpochCheck(sat))
				delay = (chips - 9) % 1023;

		} while ((delay<0)||(chips < 10));

		printf("%2i (%2i,%2i) : delay = %4i  first 10 (oct) = %4o\n", id, SAT_b1[id], SAT_b2[id], delay, ten);
	}

	free(sat);
}

int SAT_CrossCorrelation(void)
{
	FILE *fp;	
	int id;
	SAT *sat1, *sat2;

	double index;
	double offset;
	double correlation, c1, c2;

	fp = fopen("gold_cc.txt", "wt");
	if (!fp)
		exit (EXIT_FAILURE);

	sat1 = (SAT *) malloc(sizeof(SAT));
	if (NULL == sat1)
	{
		fclose(fp);
		exit (EXIT_FAILURE);
	}

	sat2 = (SAT *) malloc(sizeof(SAT));
	if (NULL == sat2)
	{
		fclose(fp);
		free(sat1);
		exit (EXIT_FAILURE);
	}

	SAT_Initialize(sat1, 16);

	fprintf(fp, "%+5.0f ", 9999.0);
	for (id = 0; id < 47; id++)
	{
		fprintf(fp, "%10i ", id);
	}
	fprintf(fp, "\n");

	for (offset = -512.0; offset < 512.0; offset += 1.0)
	{
		fprintf(fp, "%+5.0f ", offset);
		for (id = 0; id < 47; id++)
		{
			SAT_Initialize(sat2, id);
			correlation = 0.0;
			for (index = 0; index <1023; index += 1)
			{
				c1 = SAT_Chip(sat1, (int) (1023.0+index));
				c2 = SAT_Chip(sat2, (int) (1023.0+index+offset));
				correlation += c1*c2;
			}
			correlation /= 1023.0;
			fprintf(fp, "%+10.5f ", correlation);
		}
		fprintf(fp, "\n");
	}

	fclose(fp);
	free(sat1);
	free(sat2);

	return EXIT_SUCCESS;
}

typedef struct GPSTX GPSTX;

struct GPSTX
{
	SAT *sat;
	double freq_Hz, gain, dist_m;
	double cps_Hz;
	int offsets;
	double phase;
	double mod_depth;
	int *offset;
};

GPSTX *GPSTX_New(int id, int offsets, double cps_Hz, double freq_Hz, double gain, double dist_m, double phase)
{
	GPSTX *p;
	int offset;

	p = NULL;

	if (offsets < 1)
		return p;

	p = (GPSTX *) malloc(sizeof(GPSTX));
	if (NULL == p)
		return p;

	p->sat = (SAT *) malloc(sizeof(SAT));
	if (NULL == p->sat)
	{
		free(p);
		p = NULL;
		return p;
	}

	p->offset = (int *) malloc(offsets*sizeof(int));
	if (NULL == p->offset)
	{
		free(p->sat);
		free(p);
		p = NULL;
		return p;
	}

	SAT_Initialize(p->sat, id);
	p->freq_Hz = freq_Hz;
	p->gain = gain;
	p->dist_m = dist_m;
	p->offsets = offsets;
	p->cps_Hz = cps_Hz;
	p->mod_depth = PI/2.0;
	p->phase = phase;

	for (offset = 0; offset < p->offsets; offset++)
	{
		p->offset[offset] = rand()%1023;
	}

	return p;
}

void GPSTX_Delete(GPSTX *p)
{
	if (p)
	{
		if (p->sat)
		{
			free(p->sat);
			p->sat = NULL;
		}
		
		if (p->offset)
		{
			free(p->offset);
			p->offset = NULL;
		}

		free(p);
		p = NULL;
	}
}

#define LIGHTSPEED_mps (2.99e8)

double GPSTX_Chip(GPSTX *p, double t_rx)
{
	double t;
	double index;
	double chip;
	int i;

	if (NULL == p)
		return 0;

	t = t_rx - p->dist_m/LIGHTSPEED_mps;
	chip = 0.0;
	for (i = 0; i < p->offsets; i++)
	{
		index = p->offset[i] + t * p->cps_Hz;
		while (index < 0.0)
			index += 1023.0;
		chip += SAT_Chip(p->sat, (int) (index));
	}
	chip /= p->offsets;

	return chip;
}

double GPSTX_Chip2(GPSTX *p, double t_rx, int offset)
{
	double t;
	double index;
	double chip;
	int i;

	if (NULL == p)
		return 0;

	t = t_rx - p->dist_m/LIGHTSPEED_mps;
	chip = 0.0;
	for (i = 0; i < p->offsets; i++)
	{
		index = offset + p->offset[i] + t * p->cps_Hz;
		while (index < 0.0)
			index += 1023.0;
		chip += SAT_Chip(p->sat, (int) (index));
	}
	chip /= p->offsets;

	return chip;
}

double GPSTX_Signal(GPSTX *p, double t_rx)
{
	double t;
	double sig;
	double chip;

	if (NULL == p)
		return 0.0;

	t = t_rx - p->dist_m/LIGHTSPEED_mps;

	sig = 0.0;
	chip = GPSTX_Chip(p, t_rx);
	chip *= p->gain;
	if (chip >  1.0) chip =  1.0;
	if (chip < -1.0) chip = -1.0;
	sig = sin(2.0*PI*p->freq_Hz*t + p->mod_depth*chip + p->phase);

	return sig;
}

double GPSTX_Signal2(GPSTX *p, double t_rx, int offset)
{
	double t;
	double sig;
	double chip;

	if (NULL == p)
		return 0.0;

	t = t_rx - p->dist_m/LIGHTSPEED_mps;

	sig = 0.0;
	chip = GPSTX_Chip(p, t_rx + (offset / p->cps_Hz));
	chip *= p->gain;
	if (chip >  1.0) chip =  1.0;
	if (chip < -1.0) chip = -1.0;
	sig = sin(2.0*PI*p->freq_Hz*t + p->mod_depth*chip + p->phase);

	return sig;
}

double Gaussian(void)
{
	double q,u,v,x,y;
	do
	{
		u = rand_norm();
		v = 1.7156*(rand_norm()-0.5);
		x = u - 0.449871;
		y = fabs(v) + 0.386595;
		q = x*x + y*(0.19600*y - 0.25472*x);
	} while (q > 0.27597 && ((q > 0.27846) || ((v*v) > (-4.0*log(u)*u*u))));
	return (v/u);
}

double NormalPDF(double sigmas)
{
	return exp(-0.5*sigmas*sigmas)/sqrt(2.0*PI);
}

#define NORMALIZED_AWG_POWER (sqrt(2.0/PI))

double Noise_AWG_raw(void)
{
	double noise;

	// The following will produce AWG noise with rms value of sqrt(2/PI)
	noise = Gaussian();
	if (noise < 0.0)
		return -sqrt(-noise);
	
	return sqrt(noise);
}

double Noise_AWG(double rms_power)
{
	double noise;

	// The following will produce AWG noise with rms value of sqrt(2/PI)
	noise = Noise_AWG_raw();

	// The following will produce AWG noise with unity rms value
	noise /= sqrt(NORMALIZED_AWG_POWER);

	// The following will produce AWG noise with rms value of rms_power
	noise *= sqrt(rms_power);

	return noise;
}

double Test_Noise_AWG(double target)
{
	double t, dt, t_final;
	double x, energy, power;

	dt = 0.001;
	t_final = 100.0;
	energy = 0.0;

	for (t = 0.0; t < t_final; t+= dt)
	{
		x = Noise_AWG(target);
		energy += x*x*dt;
	}
	power = energy / t_final;
	printf("Power = %g\n", power);
	return ((power - target)/target);
}

int Gaussian_test(void)
{
	FILE *fp;
	double x;
	int trial, trials;
	int bin, bins;
	double range, binsize;
	double *dist;
	
	fp = fopen("norm_data.txt", "wt");
	if (!fp)
		exit (EXIT_FAILURE);

	bins = 201;
	range = 5.0;
	trials = 1000000;

	binsize = (2.0*range)/(double)(bins-1);

	dist = (double *) malloc(bins*sizeof(double));
	if (NULL == dist)
		exit (EXIT_FAILURE);

	for (bin = 0; bin < bins; bin++)
	{
		dist[bin] = 0.0;
	}

	for (trial = 0; trial < trials; trial++)
	{
		x = Gaussian();
		bin = (int)floor(((x+range)/binsize)+0.5);
		if ((bin>=0)&&(bin<bins))
			dist[bin]++;
	}

	for (bin = 0; bin < bins; bin++)
	{
		dist[bin] /= (double) trials;
		dist[bin] /= binsize;
	}

	for (bin = 0; bin < bins; bin++)
	{
		x = -range + bin*binsize;
		fprintf(fp, "%g\t%g\t%g\n", x, NormalPDF(x), dist[bin]);
	}

	free(dist);
	fclose(fp);
	return EXIT_SUCCESS;
}

int dmain(void)
{
	FILE *fp;	
	GPSTX *tx, *lx_s, *lx_c;
	double t, signal, local_s, local_c;
	double t_total, dt;
	double sig_r, noise, cor_s, cor_c, correlation;
	int step;
	double power, power_n, power_r, power_s, power_c;
	double alpha, beta;
	double noise_power, noise_scale;

	fp = fopen("gps_data.txt", "wt");
	if (!fp)
		exit (EXIT_FAILURE);

	srand(time(NULL) + clock());

	tx = GPSTX_New(6, 10, 1023000, 10.0e6, 1.0, 0.0, 0.0);
	lx_s = GPSTX_New(6, 1, 1023000, 10.0e6, 1.0, 0.0, 0.0);
	lx_c = GPSTX_New(6, 1, 1023000, 10.0e6, 1.0, 0.0, 0.0);

	lx_s->offset[0] = 0;
	lx_c->offset[0] = 0;

	t_total = 20.0*(1023.0/tx->cps_Hz);
	dt = 1.0/(10.0*tx->freq_Hz);
	noise_power = 5.0;

	noise_scale = sqrt(noise_power / NORMALIZED_AWG_POWER);

	for (step = 0; step < tx->offsets; step++)
	{
		printf("Offset %02i: %i\n", step+1, tx->offset[step]);
	}

	for (step = 0; step < 1023; step++)
	{
		printf("%i    \r", step);
		tx->dist_m = step*292.27761;
		
		power = 0.0;
		power_r = 0.0;
		power_s = 0.0;
		power_c = 0.0;
		power_n = 0.0;
		lx_s->phase = PI * ((double)rand()/(double)RAND_MAX);
		lx_c->phase = lx_s->phase + PI/2.0;
		cor_s = cor_c = 0.0;
		for (t = 0.0; t < t_total; t += dt)
		{
			//chip = GPSTX_Chip(tx, t);
			signal = GPSTX_Signal(tx, t);
			noise = Noise_AWG_raw()*noise_scale;
			sig_r = signal+noise;
			local_s = GPSTX_Signal(lx_s, t);
			local_c = GPSTX_Signal(lx_c, t);
			//fprintf(fp, "%g %g %i\n", t, sig, 3+chip);
			cor_s += sig_r*local_s;
			cor_c += sig_r*local_c;

			power   += signal*signal;
			power_n += noise*noise;
			power_r += sig_r*sig_r;
			power_s += local_s*local_s;
			power_c += local_c*local_c;
		}
		power *= dt/t_total;
		power_r *= dt/t_total;
		power_n *= dt/t_total;
		power_s *= dt/t_total;
		power_c *= dt/t_total;
		alpha = sqrt(1.0/power);
		beta = sqrt(1.0/sqrt(power_s*power_s + power_c*power_c));
		//printf("Power: %g, %g, %g\n", power, power_s, power_c);
		//printf("Scale: %g, %g, %g\n", alpha, beta, alpha*beta);
		cor_s *= dt/t_total;
		cor_c *= dt/t_total;
		correlation = alpha*beta*sqrt(cor_s*cor_s + cor_c*cor_c);
		fprintf(fp, "%g\t%g\n", (double)step, correlation);
	}
	printf("Average Signal   Power: %g\n", power);
	printf("Average Noise    Power: %g\n", power_n);
	printf("Average Received Power: %g\n", power_r);
	if (power_n > 0.0)
		printf("SNR: %g (%g dB)\n", power/power_n, 10.0*log10(power/power_n));

	fclose(fp);
	GPSTX_Delete(tx);
	GPSTX_Delete(lx_s);
	GPSTX_Delete(lx_c);

	return EXIT_SUCCESS;
}

int amain(void)
{
	double err;

	err = Test_Noise_AWG(10.0);

	printf("Error = %g%%\n", 100.0*err);

	return EXIT_SUCCESS;
}

int TimeWaveform(void)
{
	FILE *fp;	
	GPSTX *tx, *lx_s, *lx_c;
	double t, signal;
	double t_total, dt;
	double sig_r, noise;
	int step;
	double power, power_n, power_r, power_s, power_c;
	double noise_power, noise_scale;

	int noise_levels = 3;
	double noise_level[] = {-10.0, 0.0, 10.0};
	int noise_index;
	double snr;
	double chip;

	fp = fopen("gps_timedata.txt", "wt");
	if (!fp)
		exit (EXIT_FAILURE);

	srand(time(NULL) + clock());

	tx = GPSTX_New(GPS_SAT_ID, OFFSETS, CHIP_RATE_HZ, IF_HZ, 1.0, 0.0, 0.0);
	lx_s = GPSTX_New(GPS_SAT_ID, 1, CHIP_RATE_HZ, IF_HZ, 1.0, 0.0, 0.0);
	lx_c = GPSTX_New(GPS_SAT_ID, 1, CHIP_RATE_HZ, IF_HZ, 1.0, 0.0, 0.0);

	lx_s->offset[0] = 0;
	lx_c->offset[0] = 0;

	// Set time to output ten chips
	t_total = 10.0/CHIP_RATE_HZ;
	dt = 1.0/FS_HZ;

	noise_power = 5.0;

	noise_scale = sqrt(noise_power / NORMALIZED_AWG_POWER);

	for (step = 0; step < tx->offsets; step++)
	{
		printf("Offset %02i: %i\n", step+1, tx->offset[step]);
	}

	// Pick a random phase relationship between RX and TX
	lx_s->phase = PI * ((double)rand()/(double)RAND_MAX);
	lx_c->phase = lx_s->phase + PI/2.0;


	power = 0.0;
	power_r = 0.0;
	power_s = 0.0;
	power_c = 0.0;
	power_n = 0.0;
	for (t = 0.0; t < t_total; t += dt)
	{
		// Print time to data file
		fprintf(fp, "%g", t*1e6); // Time (in us)

		// Print basic Gold code sequence and waveform to file
		chip = GPSTX_Chip(lx_s, t);
		signal = GPSTX_Signal(lx_s, t);
		fprintf(fp, "\t%g\t%g", chip, signal);

		// Print BBC-encoded code sequence and waveform to file
		chip = GPSTX_Chip(tx, t);
		signal = GPSTX_Signal(tx, t);
		fprintf(fp, "\t%g\t%g", chip, signal);

		// Print various noisy waveforms to file
		for (noise_index = 0; noise_index < noise_levels; noise_index++)
		{
			snr = pow(10.0, noise_level[noise_index]/10.0);
			noise_scale = sqrt(snr);
			noise = Noise_AWG_raw()*noise_scale;
			sig_r = signal+noise;
			fprintf(fp, "\t%g", sig_r);
		}
		/*
		local_s = GPSTX_Signal(lx_s, t);
		local_c = GPSTX_Signal(lx_c, t);
		//fprintf(fp, "%g %g %i\n", t, sig, 3+chip);
		cor_s += sig_r*local_s;
		cor_c += sig_r*local_c;

		power   += signal*signal;
		power_n += noise*noise;
		power_r += sig_r*sig_r;
		power_s += local_s*local_s;
		power_c += local_c*local_c;
		*/
		fprintf(fp, "\n");
	}

	fclose(fp);
	GPSTX_Delete(tx);
	GPSTX_Delete(lx_s);
	GPSTX_Delete(lx_c);

	return EXIT_SUCCESS;
}

int CorrelationData(void)
{
	FILE *fp;	
	GPSTX *tx;
	double t, signal;
	double t_total, dt;
	double sig_r, noise, correlation;
	int step;
	double power, power_n, power_r, power_s, power_c;
	double alpha, beta;
	double noise_scale;
	double phase;
	int noise_levels = 3;
	double noise_level[] = {-10.0, 0.0, 10.0};
	double index;
	int level;
	double snr;
	double *cor_s, *cor_c;
	double sig_s[2], sig_c[2];
	double dot_time;
	int offset_data[] = {123,160,171,323,337,353,557,574,757,816};

	int i,j,k;
	double temp;
	double min_correlation;

	fp = fopen("gps_corrdata.txt", "wt");
	if (!fp)
		exit (EXIT_FAILURE);

	srand(time(NULL) + clock());

	// Instantiate Transmitter
	tx = GPSTX_New(GPS_SAT_ID, OFFSETS, CHIP_RATE_HZ, IF_HZ, 20.0, 0.0, 0.0);

	// Force the same set of offsets for comparison purposes
	for (step = 0; step < 10; step++)
		tx->offset[step] = offset_data[step];

	// Initialize data bins
	cor_s = (double *) malloc(SEQ_LEN*sizeof(double));
	cor_c = (double *) malloc(SEQ_LEN*sizeof(double));
	if ((NULL == cor_s) || (NULL == cor_c))
		return EXIT_FAILURE;

	for (step = 0; step < SEQ_LEN; step++)
	{
		cor_s[step] = 0.0;
		cor_c[step] = 0.0;
	}

	t_total = (double) SEQ_LEN / (double) CHIP_RATE_HZ; // One cycle of sequence
	//t_total = 1.0/(double) DATA_RATE_HZ; // One data bit period

	dt = 1.0/FS_HZ;

	for (step = 0; step < tx->offsets; step++)
	{
		printf("Offset %02i: %i\n", step+1, tx->offset[step]);
	}
	
	snr = 100.0; //dB
	noise_scale = sqrt( (0.5*pow(10, -(snr/10.0))) / NORMALIZED_AWG_POWER );

	// Pick a random phase relationship between RX and TX
	phase = 2.0*PI * ((double)rand()/(double)RAND_MAX);

	power = 0.0;
	power_r = 0.0;
	power_n = 0.0;
	power_s = 0.0;
	power_c = 0.0;
	dot_time = 0.0;

	for (t = 0.0; t < t_total; t += dt)
	{
		// Update progress display
		if (t > dot_time)
		{
			printf(".");
			dot_time += t_total/(79);
		}

		// Get the TX waveform (without noise)
		signal = GPSTX_Signal(tx, t);
		// Generate the noise component
		noise = Noise_AWG_raw()*noise_scale;
		sig_r = signal+noise;

		// calculate the possible singleton levels.
		sig_s[1] = sin(2.0*PI*tx->freq_Hz*t + 0.5*PI + phase);;
		sig_s[0] = sin(2.0*PI*tx->freq_Hz*t - 0.5*PI + phase);;
		sig_c[1] = sin(2.0*PI*tx->freq_Hz*t + 1.0*PI + phase);;
		sig_c[0] = sin(2.0*PI*tx->freq_Hz*t - 0.0*PI + phase);;

		// calculate basic index for present time
		index = t * tx->cps_Hz;
		while (index < 0.0)
			index += SEQ_LEN;

		for (step = 0; step < SEQ_LEN; step++)
		{
			level = (tx->sat->sequence[((int)(index+step))%SEQ_LEN] > 0)? 1 : 0;
			cor_s[step] += sig_r * sig_s[level];
			cor_c[step] += sig_r * sig_c[level];
		}

		// Update Power Level Calculations
		power   += signal*signal;
		power_n += noise*noise;
		power_r += sig_r*sig_r;
		power_s += sig_s[level]*sig_s[level]; // Use last signal level
		power_c += sig_c[level]*sig_c[level]; // Use last signal level
	}
	printf("\n");

	power *= dt/t_total;
	power_r *= dt/t_total;
	power_n *= dt/t_total;
	power_s *= dt/t_total;
	power_c *= dt/t_total;
	alpha = sqrt(1.0/power);
	beta = sqrt(1.0/sqrt(power_s*power_s + power_c*power_c));
	printf("Power: %g, %g, %g\n", power, power_s, power_c);
	printf("Scale: %g, %g, %g\n", alpha, beta, alpha*beta);
	printf("Average Signal   Power: %g\n", power);
	printf("Average Noise    Power: %g\n", power_n);
	printf("Average Received Power: %g\n", power_r);
	if (power_n > 0.0)
		printf("SNR: %g (%g dB)\n", power/power_n, 10.0*log10(power/power_n));

	for (step = 0; step < SEQ_LEN; step++)
	{
		cor_s[step] *= dt/t_total;
		cor_c[step] *= dt/t_total;
		correlation = alpha*beta*sqrt(cor_s[step]*cor_s[step] + cor_c[step]*cor_c[step]);
		fprintf(fp, "%g\t%g\n", (double)step, correlation);
		cor_s[step] = correlation;
		cor_c[step] = correlation;
	}

	// Minimum threshold needed to collect all transmitted offsets;
	min_correlation = cor_c[tx->offset[0]];
	for (i = 0; i < OFFSETS; i++)
	{
		if (min_correlation > cor_c[tx->offset[i]])
			min_correlation = cor_c[tx->offset[i]];
	}

	// Stupid sort
	for (i = 0; i < SEQ_LEN-1; i++)
	{
		k = i;
		for (j = i+1; j < SEQ_LEN; j++)
		{
			if (cor_s[j] >= cor_s[k]) 
				k = j;
		}
		if (k!=i)
		{
			temp = cor_s[i];
			cor_s[i] = cor_s[k];
			cor_s[k] = temp;
		}
	}

	k = 0;
	while (cor_s[k] > min_correlation)
		k++;

	printf("Gain: %f\n", tx->gain);
	printf("Offsets at minimum correlation: %i\n", k+1);
	printf("2x margin: %f\n", min_correlation/cor_s[2*OFFSETS-1]);
	printf("50%% margin: %f\n", min_correlation/cor_s[(SEQ_LEN-1)/2]);

	fclose(fp);
	GPSTX_Delete(tx);

	return EXIT_SUCCESS;
}

int SingleGainData(FILE *fp, GPSTX *tx, int offsets, double gain)
{
	double t, signal;
	double t_total, dt;
	double sig_r, noise, correlation;
	int step;
	double power, power_n, power_r, power_s, power_c;
	double alpha, beta;
	double noise_scale;
	double phase;
	int noise_levels = 3;
	double noise_level[] = {-10.0, 0.0, 10.0};
	double index;
	int level;
	double snr;
	double *cor_s, *cor_c;
	double sig_s[2], sig_c[2];
	double dot_time;

	int i,j,k;
	double temp;
	double min_correlation;

	// Initialize data bins
	cor_s = (double *) malloc(SEQ_LEN*sizeof(double));
	cor_c = (double *) malloc(SEQ_LEN*sizeof(double));
	if ((NULL == cor_s) || (NULL == cor_c))
		return EXIT_FAILURE;

	for (step = 0; step < SEQ_LEN; step++)
	{
		cor_s[step] = 0.0;
		cor_c[step] = 0.0;
	}

	t_total = (double) SEQ_LEN / (double) CHIP_RATE_HZ; // One cycle of sequence
	//t_total = 1.0/(double) DATA_RATE_HZ; // One data bit period

	dt = 1.0/FS_HZ;

	snr = 100.0; //dB
	noise_scale = sqrt( (0.5*pow(10, -(snr/10.0))) / NORMALIZED_AWG_POWER );

	// Pick a random phase relationship between RX and TX
	phase = 2.0*PI * ((double)rand()/(double)RAND_MAX);

	power = 0.0;
	power_r = 0.0;
	power_n = 0.0;
	power_s = 0.0;
	power_c = 0.0;
	dot_time = 0.0;

	for (t = 0.0; t < t_total; t += dt)
	{
		// Update progress display
		if (t > dot_time)
		{
			printf(".");
			dot_time += t_total/(79);
		}

		// Get the TX waveform (without noise)
		signal = GPSTX_Signal(tx, t);
		// Generate the noise component
		noise = Noise_AWG_raw()*noise_scale;
		sig_r = signal+noise;

		// calculate the possible singleton levels.
		sig_s[1] = sin(2.0*PI*tx->freq_Hz*t + 0.5*PI + phase);;
		sig_s[0] = sin(2.0*PI*tx->freq_Hz*t - 0.5*PI + phase);;
		sig_c[1] = sin(2.0*PI*tx->freq_Hz*t + 1.0*PI + phase);;
		sig_c[0] = sin(2.0*PI*tx->freq_Hz*t - 0.0*PI + phase);;

		// calculate basic index for present time
		index = t * tx->cps_Hz;
		while (index < 0.0)
			index += SEQ_LEN;

		for (step = 0; step < SEQ_LEN; step++)
		{
			level = (tx->sat->sequence[((int)(index+step))%SEQ_LEN] > 0)? 1 : 0;
			cor_s[step] += sig_r * sig_s[level];
			cor_c[step] += sig_r * sig_c[level];
		}

		// Update Power Level Calculations
		power   += signal*signal;
		power_n += noise*noise;
		power_r += sig_r*sig_r;
		power_s += sig_s[level]*sig_s[level]; // Use last signal level
		power_c += sig_c[level]*sig_c[level]; // Use last signal level
	}
	printf("\r");

	power *= dt/t_total;
	power_r *= dt/t_total;
	power_n *= dt/t_total;
	power_s *= dt/t_total;
	power_c *= dt/t_total;
	alpha = sqrt(1.0/power);
	beta = sqrt(1.0/sqrt(power_s*power_s + power_c*power_c));

	for (step = 0; step < SEQ_LEN; step++)
	{
		cor_s[step] *= dt/t_total;
		cor_c[step] *= dt/t_total;
		correlation = alpha*beta*sqrt(cor_s[step]*cor_s[step] + cor_c[step]*cor_c[step]);
		cor_s[step] = correlation;
		cor_c[step] = correlation;
	}

	// Minimum threshold needed to collect all transmitted offsets;
	min_correlation = cor_c[tx->offset[0]];
	for (i = 0; i < offsets; i++)
	{
		if (min_correlation > cor_c[tx->offset[i]])
			min_correlation = cor_c[tx->offset[i]];
	}

	// Stupid sort
	for (i = 0; i < SEQ_LEN-1; i++)
	{
		k = i;
		for (j = i+1; j < SEQ_LEN; j++)
		{
			if (cor_s[j] >= cor_s[k]) 
				k = j;
		}
		if (k!=i)
		{
			temp = cor_s[i];
			cor_s[i] = cor_s[k];
			cor_s[k] = temp;
		}
	}

	k = 0;
	while (cor_s[k] > min_correlation)
		k++;

	fprintf(stdout, "%8i", offsets);
	fprintf(stdout, "%6.2f", tx->gain);
	fprintf(stdout, "%8i", k+1);
	fprintf(stdout, "%9.4f", min_correlation/cor_s[2*offsets-1]);
	fprintf(stdout, "%9.4f", min_correlation/cor_s[(SEQ_LEN-1)/2]);
	fprintf(stdout, "     \n");

	fprintf(fp, "%8i", offsets);
	fprintf(fp, "%g", tx->gain);
	fprintf(fp, "\t%i", k+1);
	fprintf(fp, "\t%g", min_correlation/cor_s[2*offsets-1]);
	fprintf(fp, "\t%g", min_correlation/cor_s[(SEQ_LEN-1)/2]);
	fprintf(fp, "\n");

	return EXIT_SUCCESS;
}

int GainData(void)
{
	FILE *fp;
	double gain;
	GPSTX *tx;
	int offset_data[] = {123,160,171,323,337,353,557,574,757,816};
	int offsets;
	int step;

	fp = fopen("gps_gaindata.txt", "wt");
	if (!fp)
		exit (EXIT_FAILURE);

	srand(time(NULL) + clock());

	offsets = 100;
	gain = 1.0;

	// Instantiate Transmitter
	tx = GPSTX_New(GPS_SAT_ID, offsets, CHIP_RATE_HZ, IF_HZ, 1.0, 0.0, 0.0);

	// Force the same set of offsets for comparison purposes
	for (step = 0; step < ((offsets < 10)? offsets:10); step++)
		tx->offset[step] = offset_data[step];

	for (step = 0; step < tx->offsets; step++)
	{
		printf("Offset %02i: %i\n", step+1, tx->offset[step]);
	}
	
	printf("Offsets ");
	printf("  Gain  ");
	printf("Covers  ");
	printf("2x marg  ");
	printf("50%% marg ");
	printf("\n");

	//for (gain = 0.1; gain <= offsets; gain *= 1.25)
	for (offsets = 25; offsets <= 50; offsets+=1)
	{
		tx->offsets = offsets;
		tx->gain = gain;
		SingleGainData(fp, tx, offsets, gain);
	}

	GPSTX_Delete(tx);
	fclose(fp);

	return EXIT_SUCCESS;
}

int AttackData(void)
{
	FILE *fp;	
	GPSTX *tx, *tx2;
	double t, signal, signal2;
	double t_total, dt;
	double sig_r, noise, correlation;
	int step;
	double power, power_n, power_r, power_s, power_c;
	double alpha, beta;
	double noise_scale;
	double phase;
	int noise_levels = 3;
	double noise_level[] = {-10.0, 0.0, 10.0};
	double index;
	int level;
	double snr;
	double *cor_s, *cor_c;
	double sig_s[2], sig_c[2];
	double dot_time;
	int offset_data[] = {123,160,171,323,337,353,557,574,757,816};

	int i,j,k;
	double temp;
	double min_correlation;
	double attack_gain;

	fp = fopen("gps_tackdata.txt", "wt");
	if (!fp)
		exit (EXIT_FAILURE);

	srand(time(NULL) + clock());

	// Instantiate Transmitter
	tx = GPSTX_New(GPS_SAT_ID, OFFSETS, CHIP_RATE_HZ, IF_HZ, 1.0, 0.0, 0.0);

	// Instantiate Attack Transmitter
	tx2 = GPSTX_New(GPS_SAT_ID+1, 1, CHIP_RATE_HZ, IF_HZ, 1.0, 0.0, 0.0);
	attack_gain = 1;

	// Force the same set of offsets for comparison purposes
	for (step = 0; step < 10; step++)
		tx->offset[step] = offset_data[step];

	// Initialize data bins
	cor_s = (double *) malloc(SEQ_LEN*sizeof(double));
	cor_c = (double *) malloc(SEQ_LEN*sizeof(double));
	if ((NULL == cor_s) || (NULL == cor_c))
		return EXIT_FAILURE;

	for (step = 0; step < SEQ_LEN; step++)
	{
		cor_s[step] = 0.0;
		cor_c[step] = 0.0;
	}

	t_total = 1.0*(double) SEQ_LEN / (double) CHIP_RATE_HZ; // One cycle of sequence
	//t_total = 1.0/(double) DATA_RATE_HZ; // One data bit period

	dt = 1.0/FS_HZ;

	for (step = 0; step < tx->offsets; step++)
	{
		printf("Offset %02i: %i\n", step+1, tx->offset[step]);
	}
	
	snr = 100.0; //dB
	noise_scale = sqrt( (0.5*pow(10, -(snr/10.0))) / NORMALIZED_AWG_POWER );

	// Pick a random phase relationship between RX and TX
	phase = 2.0*PI * ((double)rand()/(double)RAND_MAX);

	power = 0.0;
	power_r = 0.0;
	power_n = 0.0;
	power_s = 0.0;
	power_c = 0.0;
	dot_time = 0.0;

	for (t = 0.0; t < t_total; t += dt)
	{
		// Update progress display
		if (t > dot_time)
		{
			printf(".");
			dot_time += t_total/(79);
		}

		// Get the TX waveform (without noise)
		signal = GPSTX_Signal(tx, t);
		// Get the TX waveform (without noise)
		signal2 = attack_gain*GPSTX_Signal(tx2, t);
		// Generate the noise component
		noise = signal2+Noise_AWG_raw()*noise_scale;
		sig_r = signal+noise;

		// calculate the possible singleton levels.
		sig_s[1] = sin(2.0*PI*tx->freq_Hz*t + 0.5*PI + phase);;
		sig_s[0] = sin(2.0*PI*tx->freq_Hz*t - 0.5*PI + phase);;
		sig_c[1] = sin(2.0*PI*tx->freq_Hz*t + 1.0*PI + phase);;
		sig_c[0] = sin(2.0*PI*tx->freq_Hz*t - 0.0*PI + phase);;

		// calculate basic index for present time
		index = t * tx->cps_Hz;
		while (index < 0.0)
			index += SEQ_LEN;

		for (step = 0; step < SEQ_LEN; step++)
		{
			level = (tx->sat->sequence[((int)(index+step))%SEQ_LEN] > 0)? 1 : 0;
			cor_s[step] += sig_r * sig_s[level];
			cor_c[step] += sig_r * sig_c[level];
		}

		// Update Power Level Calculations
		power   += signal*signal;
		power_n += noise*noise;
		power_r += sig_r*sig_r;
		power_s += sig_s[level]*sig_s[level]; // Use last signal level
		power_c += sig_c[level]*sig_c[level]; // Use last signal level
	}
	printf("\n");

	power *= dt/t_total;
	power_r *= dt/t_total;
	power_n *= dt/t_total;
	power_s *= dt/t_total;
	power_c *= dt/t_total;
	alpha = sqrt(1.0/power);
	beta = sqrt(1.0/sqrt(power_s*power_s + power_c*power_c));
	printf("Power: %g, %g, %g\n", power, power_s, power_c);
	printf("Scale: %g, %g, %g\n", alpha, beta, alpha*beta);
	printf("Average Signal   Power: %g\n", power);
	printf("Average Noise    Power: %g\n", power_n);
	printf("Average Received Power: %g\n", power_r);
	if (power_n > 0.0)
		printf("SNR: %g (%g dB)\n", power/power_n, 10.0*log10(power/power_n));

	for (step = 0; step < SEQ_LEN; step++)
	{
		cor_s[step] *= dt/t_total;
		cor_c[step] *= dt/t_total;
		correlation = alpha*beta*sqrt(cor_s[step]*cor_s[step] + cor_c[step]*cor_c[step]);
		fprintf(fp, "%g\t%g\n", (double)step, correlation);
		cor_s[step] = correlation;
		cor_c[step] = correlation;
	}

	// Minimum threshold needed to collect all transmitted offsets;
	min_correlation = cor_c[tx->offset[0]];
	for (i = 0; i < OFFSETS; i++)
	{
		if (min_correlation > cor_c[tx->offset[i]])
			min_correlation = cor_c[tx->offset[i]];
	}

	// Stupid sort
	for (i = 0; i < SEQ_LEN-1; i++)
	{
		k = i;
		for (j = i+1; j < SEQ_LEN; j++)
		{
			if (cor_s[j] >= cor_s[k]) 
				k = j;
		}
		if (k!=i)
		{
			temp = cor_s[i];
			cor_s[i] = cor_s[k];
			cor_s[k] = temp;
		}
	}

	k = 0;
	while (cor_s[k] > min_correlation)
		k++;

	printf("Gain: %f\n", tx->gain);
	printf("Offsets at minimum correlation: %i\n", k+1);
	printf("2x margin: %f\n", min_correlation/cor_s[2*OFFSETS-1]);
	printf("50%% margin: %f\n", min_correlation/cor_s[(SEQ_LEN-1)/2]);

	fclose(fp);
	GPSTX_Delete(tx);

	return EXIT_SUCCESS;
}

int main(void)
{
	return TimeWaveform();
	return CorrelationData();
	return GainData();
	return AttackData();
}