#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 "../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;\
	stop_signal_called = true;\
	}\
	}

void sigint_handler(int code){
	(void)code;
	stop_signal_called = true;
}
#define READ_BUF_SIZE 1024*1024
std::shared_ptr<SPTYPE[][2]> readbuf[2];
std::atomic<int> curr_active = 0;
std::atomic<int> curr_deal = 0;

//Sample rate in Hz
double sprate		=  240000 ;
const int searchRange = 1001;
std::atomic<int> syn = searchRange/10;
//消费者线程，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];

	//接收频率
	double rx_freq		= 610e6;
	double rx_sprate	= sprate;
	double rx_gain		= 0.0;
	bool   rx_agc		= false;
	double rx_bw		= 240000;
	//接收信号。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], ""));

	// See what gain actually is
	UHD_DO(uhd_usrp_get_rx_gain(usrp, rx_channel[0], "", &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");

	readbuf[0] = std::shared_ptr<SPTYPE[][2]> (new SPTYPE[READ_BUF_SIZE+rx_sps_buff*4][2]);
	readbuf[1] = std::shared_ptr<SPTYPE[][2]> (new SPTYPE[READ_BUF_SIZE+rx_sps_buff*4][2]);
	size_t total_red = 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 num_rx_samps = 0;
		SPTYPE * rx_buff = (SPTYPE *) (readbuf[curr_active].get() + total_red);
		void ** rx_buff_ptr = (void **)&rx_buff;
		// Handle data
		UHD_DO(uhd_rx_streamer_recv(rx_streamer, rx_buff_ptr, rx_sps_buff, &rx_meta, 1, false, &num_rx_samps));
		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);
			puts(error_string);
		}
		total_red += num_rx_samps;
		//切换缓存
		if (total_red > READ_BUF_SIZE)
		{
			fprintf(stderr, "Switch Cache to %d\n", (int) curr_active);
			size_t start_move = READ_BUF_SIZE - WAVSIZE;
			size_t end_move = total_red-1;
			size_t copyto = end_move - start_move + 1;
			SPTYPE (* dest)[2] = readbuf[1-curr_active].get();
			SPTYPE (* src)[2] = readbuf[curr_active].get();
			src += start_move;
			memcpy(dest, src , copyto * sizeof(SPTYPE) * 2 );
			total_red = copyto;
			curr_active = 1 - curr_active;
			if (syn>3)
			{
				rx_stream_cmd.stream_mode = UHD_STREAM_MODE_STOP_CONTINUOUS;
				UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
				while (curr_active != curr_deal)
				{
					std::this_thread::sleep_for(std::chrono::milliseconds(1000));
					fprintf(stderr,"Waiting for syn.\n");
				}
				rx_stream_cmd.stream_mode = UHD_STREAM_MODE_START_CONTINUOUS;
				UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
			}
		}
	}
	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()
{
	const int DealEnd = READ_BUF_SIZE - WAVSIZE;
	float  (*cross[searchRange])[2][2] ;
	for (int i=0;i<searchRange;++i)
		cross[i] = new float[DealEnd][2][2];

	float  (*judge[searchRange])[2] ;
	for (int i=0;i<searchRange;++i)
		judge[i] = new float[DealEnd][2];

	float  (*freq_mv[searchRange])[2];
	for (int i=0;i<searchRange;++i)
		freq_mv[i] = new float[DealEnd][2];

	float  (*sig[searchRange])[2] ;
	for (int i=0;i<searchRange;++i)
		sig[i] = new float[DealEnd][2];


	const float pi = 3.1415927;

#pragma omp parallel for
	for (int df = 0;df<searchRange;++df)
	{
		float f = (df-searchRange/2.0)*10;
		for (int pv = 0;pv<DealEnd;++pv)
		{
			float t = pv / sprate;
			freq_mv[df][pv][0] = cos(2*pi*f*t);
			freq_mv[df][pv][1] = sin(2*pi*f*t);
		}
	}
	int startDF =0, endDF = searchRange;
	while (!stop_signal_called) {
		if ((int)curr_active==(int)curr_deal)
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(1));
			continue;
		}
		int deal = 1-curr_active;
		short (*pBufRx)[2] = readbuf[deal].get();
		fprintf(stderr,"Dealing %d\n",deal);
		float maxAbs = 0;
		int maxDF = 2;
		int stp = syn;
#pragma omp parallel for
		for (int df = startDF;df<endDF;df+=stp)
		{
			//xorr
#pragma omp simd
			for (size_t i=0;i<DealEnd;++i)
			{
				sig[df][i][0] = (pBufRx[i][0] * freq_mv[df][i][0] - pBufRx[i][1] * freq_mv[df][i][1]);
				sig[df][i][1] = (pBufRx[i][0] * freq_mv[df][i][1] + pBufRx[i][1] * freq_mv[df][i][0]);
			}
#pragma omp simd
			for (size_t i=0;i<DealEnd;++i)
			{
				cross[df][i][0][0] = 0;
				cross[df][i][0][1] = 0;
				cross[df][i][1][0] = 0;
				cross[df][i][1][1] = 0;
				for (size_t j=0;j<WAVSIZE;++j)
				{
					cross[df][i][0][0] += (sig[df][i+j][0] * wav_xorr[0][j][0] - sig[df][i+j][1] * wav_xorr[0][j][1]);
					cross[df][i][0][1] += (sig[df][i+j][0] * wav_xorr[0][j][1] + sig[df][i+j][1] * wav_xorr[0][j][0]);
					cross[df][i][1][0] += (sig[df][i+j][0] * wav_xorr[1][j][0] - sig[df][i+j][1] * wav_xorr[1][j][1]);
					cross[df][i][1][1] += (sig[df][i+j][0] * wav_xorr[1][j][1] + sig[df][i+j][1] * wav_xorr[1][j][0]);
				}
				judge[df][i][0] = (cross[df][i][0][0]/32768 * cross[df][i][0][0]/32768 + cross[df][i][0][1]/32768 * cross[df][i][0][1]/32768 );
				judge[df][i][1] = (cross[df][i][1][0]/32768 * cross[df][i][1][0]/32768 + cross[df][i][1][1]/32768 * cross[df][i][1][1]/32768 );
			}
			float maxv = 0;
#pragma omp simd
			for (size_t i=0;i<DealEnd;++i)
			{
				if (maxv<judge[df][i][0])
					maxv = judge[df][i][0];
				if (maxv<judge[df][i][1])
					maxv = judge[df][i][1];
			}
#pragma omp critical
			{
				if (maxv > maxAbs)
				{
					maxAbs = maxv;
					maxDF = df;
				}
			}

		}

		printf ("MaxDF=%d\n",maxDF);
		for (size_t i=0;i<DealEnd;++i)
		{
			if (judge[maxDF][i][0] >= 5000 || judge[maxDF][i][1] >= 5000 )
			{
				if (judge[maxDF][i][0] * 32 < judge[maxDF][i][1])
				{
					printf("%lu:0=%f, 1=%f, 1\n",i,judge[maxDF][i][0],judge[maxDF][i][1]);
					fflush(stdout);
					i+=WAVSIZE;
					startDF = (maxDF - stp*2)<0?0:(maxDF - stp*2);
					endDF = (maxDF + stp*2)>searchRange?searchRange:((maxDF + stp*2));
					if (syn > 3)
						syn = (endDF - startDF)/10;
					if (syn<1)
						syn = 1;
				}
				else if (judge[maxDF][i][1] * 32 < judge[maxDF][i][0])
				{
					printf("%lu:0=%f, 1=%f, 0\n",i,judge[maxDF][i][0],judge[maxDF][i][1]);
					fflush(stdout);
					i+=WAVSIZE;
					startDF = (maxDF - stp*2)<0?0:(maxDF - stp*2);
					endDF = (maxDF + stp*2)>searchRange?searchRange:((maxDF + stp*2));
					if (syn > 3)
						syn = (endDF - startDF)/10;
					if (syn<1)
						syn = 1;
				}
			}

		}
		//判决
		curr_deal = 1-deal;
	}
	for (int i=0;i<searchRange;++i)
	{
		delete [] cross[i];
		delete [] judge[i];
		delete [] freq_mv[i];
		delete [] sig[i];
	}
}
int main()
{
	// Ctrl+C will exit loop
	init_wavform();
	signal(SIGINT, &sigint_handler);
	std::thread th(runDeal);
	runRecieve();
	th.join();
	return 0;
}