#include <stdio.h>
#include <uhd.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <thread>
#include <memory>
#include <atomic>
#include <math.h>
#include <fftw3.h>
#include "../waveform.h"
static bool stop_signal_called = false;
#define UHD_DO(X) \
{\
	uhd_error e = (X);\
	if (e) { fprintf(stderr,"Error in line %d, NO %d.",__LINE__,e);\
	return_code = 1;\
	}\
	}

void sigint_handler(int code){
	(void)code;
	stop_signal_called = true;
}
static const size_t rxbuf_points = 1000*1000*16;
static std::shared_ptr<short [][2]> rx_buf_ptr(new short[rxbuf_points+1024*1024][2]{{0,0}});
static std::atomic< unsigned long long> deal_count (0);
static std::atomic< unsigned long long> rx_count (0);
//消费者线程，for rx
void dealer();
int runRecieve()
{
	int return_code = EXIT_SUCCESS;
	//Need soapyuhd soapysdr soapyplutosdr soapyosmo soapy_power
	//要用libuhd操作RTLSDR，需要soapyuhd 来代理
	char dev_args[] = "available=Yes,driver=rtlsdr,label=Generic RTL2832U :: 77771111153705700,manufacturer=Generic,product=RTL2832U,rtl=0,serial=77771111153705700,tuner=Rafael Micro R820T,type=soapy";
	char error_string[4096];

	//GainName
	const char * gainName = "PGA";

	//接收频率
	double rx_freq		= 200e6;
	double rx_sprate	= SIGRATE;
	double rx_gain		= 10.0;
	bool   rx_agc		= true;
	double rx_bw		= SIGRATE;
	//接收信号。MIMO时，可以指定0,1
	size_t rx_channel[]	= {0};
	//设备句柄
	uhd_usrp_handle usrp = 0;
	uhd_rx_streamer_handle rx_streamer = 0;
	uhd_rx_metadata_handle rx_meta = 0;
	size_t rx_sps_buff = 100000;
	//ring buffer
	fprintf(stderr, "Creating USRP with args \"%s\"...\n", dev_args);
	UHD_DO(uhd_usrp_make(&usrp, dev_args));

	fprintf(stderr, "Press Ctrl+C to stop streaming...\n");
	UHD_DO(uhd_usrp_set_rx_antenna(usrp,"RX",rx_channel[0]));
	// Create RX streamer
	UHD_DO(uhd_rx_streamer_make(&rx_streamer));
	// Create RX metadata
	UHD_DO(uhd_rx_metadata_make(&rx_meta));
	// Create other necessary structs
	uhd_tune_request_t rx_tune_request =
	{
		.target_freq = rx_freq,
		.rf_freq_policy = UHD_TUNE_REQUEST_POLICY_AUTO,
		.rf_freq = 0,
		.dsp_freq_policy = UHD_TUNE_REQUEST_POLICY_AUTO,
		.dsp_freq = 0,
		.args = 0
	};
	uhd_tune_result_t rx_tune_result;
	//char rx_cpu_format[] = "fc32";
	char rx_cpu_format[] = "sc16";
	char rx_otw_format[] = "sc16";
	char rx_args[] = "";
	const size_t rx_channel_count = sizeof(rx_channel)/sizeof(rx_channel[0]);
	uhd_stream_args_t rx_stream_args = {
		.cpu_format = rx_cpu_format,
		.otw_format = rx_otw_format,
		.args = rx_args,
		.channel_list = rx_channel,
		.n_channels = rx_channel_count
	};

	uhd_stream_cmd_t rx_stream_cmd = {
		.stream_mode = UHD_STREAM_MODE_START_CONTINUOUS,
		.num_samps = 0,
		.stream_now = true,
		.time_spec_full_secs = 0,
		.time_spec_frac_secs = 0
	};
	// Set rate
	fprintf(stderr, "Setting RX Rate: %f...\n", rx_sprate);
	UHD_DO(uhd_usrp_set_rx_rate(usrp, rx_sprate, rx_channel[0]));

	// See what rate actually is
	UHD_DO(uhd_usrp_get_rx_rate(usrp, rx_channel[0], &rx_sprate));
	fprintf(stderr, "Actual RX Rate: %f...\n", rx_sprate);

	// Set gain
	fprintf(stderr, "Setting RX Gain: %f dB...\n", rx_gain);
	UHD_DO(uhd_usrp_set_rx_gain(usrp, rx_gain, rx_channel[0], gainName));

	// See what gain actually is
	UHD_DO(uhd_usrp_get_rx_gain(usrp, rx_channel[0], gainName, &rx_gain));
	fprintf(stderr, "Actual RX Gain: %f...\n", rx_gain);

	if (rx_agc)
	{
		uhd_usrp_set_rx_agc(usrp,true,rx_channel[0]);
		uhd_usrp_set_rx_dc_offset_enabled(usrp,true,rx_channel[0]);
	}
	// Set frequency
	fprintf(stderr, "Setting RX frequency: %f MHz...\n", rx_freq/1e6);
	UHD_DO(uhd_usrp_set_rx_freq(usrp, &rx_tune_request, rx_channel[0], &rx_tune_result));

	// See what frequency actually is
	UHD_DO(uhd_usrp_get_rx_freq(usrp, rx_channel[0], &rx_freq));
	fprintf(stderr, "Actual RX frequency: %f MHz...\n", rx_freq / 1e6);

	fprintf(stderr, "Setting RX Bandwidth: %f MHz...\n", rx_bw/1e6);
	UHD_DO(uhd_usrp_set_rx_bandwidth(usrp, rx_bw, rx_channel[0]));

	//Band
	UHD_DO(uhd_usrp_get_rx_bandwidth(usrp, rx_channel[0], &rx_bw));
	fprintf(stderr, "Actual RX Bandwidth: %f MHz...\n", rx_bw / 1e6);

	// Set up streamer
	rx_stream_args.channel_list = rx_channel;
	UHD_DO(uhd_usrp_get_rx_stream(usrp, &rx_stream_args, rx_streamer));

	// Set up buffer
	UHD_DO(uhd_rx_streamer_max_num_samps(rx_streamer, &rx_sps_buff));
	fprintf(stderr, "Buffer size in samples: %zu\n", rx_sps_buff);

	// Issue stream command
	fprintf(stderr, "Issuing stream command.\n");

	short (*pBufRx)[2] = rx_buf_ptr.get();
	rx_count = 0;
	deal_count = 0;

	UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
	//Read, RX in Main Thread
	while (!stop_signal_called)
	{
		size_t red = 0;
		void * rx_buff_ptr = (void *)(pBufRx[rx_count % rxbuf_points]);
		UHD_DO(uhd_rx_streamer_recv(rx_streamer,&rx_buff_ptr, rx_sps_buff, &rx_meta, 1, false, &red));
		rx_count += red;
		const size_t newBg = rx_count % rxbuf_points;
		if (newBg < rx_sps_buff &&  newBg > 0)
			memcpy(pBufRx[0], pBufRx[rxbuf_points], sizeof(short) * 2 * newBg);
		uhd_rx_metadata_error_code_t error_code;
		UHD_DO(uhd_rx_metadata_error_code(rx_meta, &error_code));
		if(error_code != UHD_RX_METADATA_ERROR_CODE_NONE){
			fprintf(stderr, "Warning: Error code 0x%x was returned during streaming.\n", error_code);
		}
	}

	if (rx_streamer) uhd_rx_streamer_free(&rx_streamer);
	if (rx_meta) uhd_rx_metadata_free(&rx_meta);
	if(return_code != EXIT_SUCCESS && usrp != NULL){
		uhd_usrp_last_error(usrp, error_string, 512);
		fprintf(stderr, "USRP reported the following error: %s\n", error_string);
	}
	uhd_usrp_free(&usrp);
	fprintf(stderr, (return_code ? "Failure\n" : "Success\n"));
	return return_code;


}

void runDeal()
{
	unsigned long long next_test = WAVSIZE;
	const unsigned int half_sz = WAVSIZE/2;
	short (*pBufRx)[2] = rx_buf_ptr.get();
	double (* ampwin)[2] = new double[WAVSIZE][2];

	fftw_complex *in[2], *out[2];
	fftw_plan p[2];

	in[0] = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * WAVSIZE);
	out[0] = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * WAVSIZE);
	p[0] = fftw_plan_dft_1d(WAVSIZE, in[0], out[0], FFTW_FORWARD, FFTW_ESTIMATE);
	in[1] = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * WAVSIZE);
	out[1] = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * WAVSIZE);
	p[1] = fftw_plan_dft_1d(WAVSIZE, in[1], out[1], FFTW_FORWARD, FFTW_ESTIMATE);
	//fftw_execute(p); /* repeat as needed */
	double globalMax = 0;
	int count = 0;
	char c = 0;
	while (!stop_signal_called)
	{
		//快速捕获
		if (next_test + WAVSIZE * 2 < deal_count)
		{
			next_test = rx_count;
		}
		if (deal_count + WAVSIZE >= rx_count || rx_count < WAVSIZE )
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(1));
			continue;
		}

		if (deal_count < next_test)
		{
			ampwin[deal_count % WAVSIZE][0] = 0;
			ampwin[deal_count % WAVSIZE][1] = 0;
			++deal_count;
			continue;
		}
		unsigned long long start_x = deal_count - WAVSIZE;
		//xorr
		for (size_t i=0;i<WAVSIZE;++i)
		{
			const int xb = (start_x + i) % rxbuf_points;
			in[0][i][0] = (pBufRx[xb][0] * wav_xorr[0][i][0] - pBufRx[xb][1] * wav_xorr[0][i][1])/16384.0;
			in[0][i][1] = (pBufRx[xb][0] * wav_xorr[0][i][1] + pBufRx[xb][1] * wav_xorr[0][i][0])/16384.0;
			in[1][i][0] = (pBufRx[xb][0] * wav_xorr[1][i][0] - pBufRx[xb][1] * wav_xorr[1][i][1])/16384.0;
			in[1][i][1] = (pBufRx[xb][0] * wav_xorr[1][i][1] + pBufRx[xb][1] * wav_xorr[1][i][0])/16384.0;
		}
		fftw_execute(p[0]);
		fftw_execute(p[1]);
		double max_abs0 = 0, max_abs1 = 0;
		for (size_t i=0;i<WAVSIZE;++i)
		{
			const double a0 = out[0][i][0]*out[0][i][0] + out[0][i][1]*out[0][i][1];
			const double a1 = out[1][i][0]*out[1][i][0] + out[1][i][1]*out[1][i][1];
			if (a0 > max_abs0)
				max_abs0 = a0;
			if (a1 > max_abs1)
				max_abs1 = a1;
		}

		ampwin[deal_count % WAVSIZE][0] = max_abs0;
		ampwin[deal_count % WAVSIZE][1] = max_abs1;

		if (globalMax  < max_abs0)
		{
			//printf("%lf,%lf\n",max_abs0,max_abs1);
			globalMax = max_abs0;
		}
		if (globalMax  < max_abs1)
		{
			//printf("%lf,%lf\n",max_abs0,max_abs1);
			globalMax = max_abs1;
		}


		bool best0 = true, best1=true;

		for (size_t i=0;i<WAVSIZE && (best0||best1);++i)
		{
			if (i!=half_sz)
			{
				if (ampwin[(deal_count +i - WAVSIZE  )% WAVSIZE][0] > ampwin[(deal_count +half_sz - WAVSIZE  )% WAVSIZE][0])
					best0 = false;
				if (ampwin[(deal_count +i - WAVSIZE  )% WAVSIZE][1] > ampwin[(deal_count +half_sz - WAVSIZE  )% WAVSIZE][1])
					best1 = false;
			}
		}
		if (best0)
			if (ampwin[(deal_count +half_sz - WAVSIZE  )% WAVSIZE][1]*5 > ampwin[(deal_count +half_sz - WAVSIZE  )% WAVSIZE][0])
			{
				best0 = false;
			}
		if (best1)
			if (ampwin[(deal_count +half_sz - WAVSIZE  )% WAVSIZE][0]*5 > ampwin[(deal_count +half_sz - WAVSIZE  )% WAVSIZE][1])
			{
				best1 = false;
			}



		if (best0  )
		{
			next_test = deal_count + WAVSIZE/2 - MODRATE_N * 16;
			if (count==0)
				count = 9;
			if (count > 0 && count < 9)
			{
				//putchar('0');
				c <<= 1;

				fflush(stdout);
			}
			if (count > 0)
			{
				--count;
				if (count==0)
					putchar (c);
			}
		}
		else if (best1 )
		{
			next_test = deal_count + WAVSIZE/2 - MODRATE_N * 16;
			if (count>0)
			{
				//putchar('1');
				c <<= 1;
				c+=1;
				fflush(stdout);
				--count;
				if (count==0)
					putchar (c);
			}

		}
		++deal_count;

	}
	delete [] ampwin;
	fftw_destroy_plan(p[0]);
	fftw_free(in[0]); fftw_free(out[0]);
	fftw_destroy_plan(p[1]);
	fftw_free(in[1]); fftw_free(out[1]);


}
int main()
{
	// Ctrl+C will exit loop
	init_wavform();
	signal(SIGINT, &sigint_handler);
	std::thread th(runDeal);
	runRecieve();
	th.join();
	return 0;
}
