/*
 * Copyright 2015 Ettus Research LLC
 * Copyright 2018 Ettus Research, a National Instruments Company
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#include <uhd.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <string>
#include <thread>
#include <vector>
#include <mutex>
bool stop_signal_called = false;
#define UHD_DO(X) \
{\
	uhd_error e = (X);\
	char buf_errs[512];\
	if (e) { snprintf(buf_errs,sizeof(buf_errs),"Error in line %d, NO %d.",__LINE__,e);\
	std::string ev = __FILE__;\
	ev += ":";\
	ev += buf_errs;\
	return_code = 1;\
	throw ev;\
	}\
	};
void sigint_handler(int code){
	(void)code;
	stop_signal_called = true;
}


int main(int /*argc*/, char * /*argv*/[])
{
	int return_code = EXIT_SUCCESS;
	char dev_args[] = "";
	char error_string[512];
	//10MHz
	double sprate		= 2500000;
	//交通广播 FM 90.8
	double rx_freq		= 90.8e6;
	double rx_sprate	= sprate;
	double rx_gain		= 55.0;
	bool   rx_agc		= false;
	double rx_bw		= 200000;
	size_t rx_channel	= 0;
	//转发到106
	double tx_freq		= 106e6;
	double tx_sprate	= sprate;
	double tx_gain		= 70;
	double tx_bw		= 200000;
	size_t tx_channel	= 0;

	//设备句柄
	uhd_usrp_handle usrp = 0;
	uhd_rx_streamer_handle rx_streamer = 0;
	uhd_rx_metadata_handle rx_meta = 0;
	uhd_tx_streamer_handle tx_streamer = 0;
	uhd_tx_metadata_handle tx_meta = 0;
	float *rx_buff = NULL;
	float* tx_buff = NULL;
	void **rx_buff_ptr = NULL;
	const void** tx_buff_ptr = NULL;

	try{
		fprintf(stderr, "Creating USRP with args \"%s\"...\n", dev_args);
		UHD_DO(uhd_usrp_make(&usrp, dev_args));

		// Create RX streamer
		UHD_DO(uhd_rx_streamer_make(&rx_streamer));

		// Create TX streamer
		UHD_DO(uhd_tx_streamer_make(&tx_streamer));

		// Create RX metadata
		UHD_DO(uhd_rx_metadata_make(&rx_meta));

		// Create TX metadata
		UHD_DO(uhd_tx_metadata_make(&tx_meta, false, 0, 0.1, true, false));

		// 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_otw_format[] = "sc16";
		char rx_args[] = "";
		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 = 1
		};

		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
		};

		// Create other necessary structs for TX
		uhd_tune_request_t tx_tune_request = {
			.target_freq = tx_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 tx_tune_result;
		char tx_cpu_format[] = "fc32";
		char tx_otw_format[] = "sc16";
		char tx_args[] = "";
		uhd_stream_args_t tx_stream_args = {
			.cpu_format = tx_cpu_format,
			.otw_format = tx_otw_format,
			.args = tx_args,
			.channel_list = &tx_channel,
			.n_channels = 1
		};
		size_t rx_sps_buff = 0;
		size_t tx_sps_buff = 0;


		// Set rate
		fprintf(stderr, "Setting RX Rate: %f...\n", rx_sprate);
		UHD_DO(uhd_usrp_set_rx_rate(usrp, rx_sprate, rx_channel));

		// See what rate actually is
		UHD_DO(uhd_usrp_get_rx_rate(usrp, rx_channel, &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, ""));

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

		if (rx_agc)
		{
			uhd_usrp_set_rx_agc(usrp,true,rx_channel);
			uhd_usrp_set_rx_dc_offset_enabled(usrp,true,rx_channel);
		}
		// 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, &rx_tune_result));

		// See what frequency actually is
		UHD_DO(uhd_usrp_get_rx_freq(usrp, rx_channel, &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));

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


		// Set rate
		fprintf(stderr, "Setting TX Rate: %f...\n", tx_sprate);
		UHD_DO(uhd_usrp_set_tx_rate(usrp, tx_sprate, tx_channel));

		// See what rate actually is
		UHD_DO(uhd_usrp_get_tx_rate(usrp, tx_channel, &tx_sprate));
		fprintf(stderr, "Actual TX Rate: %f...\n\n", tx_sprate);

		// Set gain
		fprintf(stderr, "Setting TX Gain: %f db...\n", tx_gain);
		UHD_DO(uhd_usrp_set_tx_gain(usrp, tx_gain, 0, ""));

		// See what gain actually is
		UHD_DO(uhd_usrp_get_tx_gain(usrp, tx_channel, "", &tx_gain));
		fprintf(stderr, "Actual TX Gain: %f...\n", tx_gain);

		// Set frequency
		fprintf(stderr, "Setting TX frequency: %f MHz...\n", tx_freq / 1e6);
		UHD_DO(uhd_usrp_set_tx_freq(usrp, &tx_tune_request, tx_channel, &tx_tune_result));

		// See what frequency actually is
		UHD_DO(uhd_usrp_get_tx_freq(usrp, tx_channel, &tx_freq));
		fprintf(stderr, "Actual TX frequency: %f MHz...\n", tx_freq / 1e6);

		//Band
		fprintf(stderr, "Setting TX Bandwidth: %f MHz...\n", tx_bw/1e6);
		UHD_DO(uhd_usrp_set_tx_bandwidth(usrp, tx_bw,tx_channel));

		UHD_DO(uhd_usrp_get_tx_bandwidth(usrp, tx_channel, &tx_bw));
		fprintf(stderr, "Actual TX Bandwidth: %f MHz...\n", tx_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);
		rx_buff = (float *) malloc(rx_sps_buff * 2 * sizeof(float));
		rx_buff_ptr = (void**)&rx_buff;

		// Set up streamer
		tx_stream_args.channel_list = &tx_channel;
		UHD_DO(uhd_usrp_get_tx_stream(usrp, &tx_stream_args, tx_streamer));

		// Set up buffer
		UHD_DO(uhd_tx_streamer_max_num_samps(tx_streamer, &tx_sps_buff));
		fprintf(stderr, "Buffer size in samples: %zu\n", tx_sps_buff);
		tx_buff = (float *) calloc(sizeof(float), tx_sps_buff * 2);
		tx_buff_ptr = (const void**)&tx_buff;

		// Ctrl+C will exit loop
		signal(SIGINT, &sigint_handler);
		fprintf(stderr, "Press Ctrl+C to stop streaming...\n");

		// Issue stream command
		fprintf(stderr, "Issuing stream command.\n");
		UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));

		uhd_usrp_set_time_now(usrp,0,0,0);
		size_t num_samps_sent = 0;
		// Actual streaming
		std::mutex mtx;
		std::list<std::vector<float> > buf_queue;

		std::thread th_send(
					[&]()->void
					{
						try{
							//::sleep(1);
							while (1) {
								if (stop_signal_called) break;
								std::vector<float> tx_data;
								mtx.lock();
								if (buf_queue.size())
								{
									tx_data = std::move(*buf_queue.begin());
									buf_queue.pop_front();
								}
								mtx.unlock();
								if (tx_data.size())
								{
									for (size_t i=0;i< tx_data.size()/2;++i)
									{
										tx_buff[i*2] =   tx_data[i*2];
										tx_buff[i*2+1] = tx_data[i*2+1];
									}
								}
								else
								{
									for (size_t i=0;i< tx_sps_buff;++i)
									{
										tx_buff[i*2] =   0;
										tx_buff[i*2+1] = 0;
									}
								}
								UHD_DO(uhd_tx_streamer_send(tx_streamer, tx_buff_ptr, tx_sps_buff, &tx_meta, 1, &num_samps_sent));
							}
						}
						catch(std::string er)
						{
							fputs(er.c_str(),stderr);
						}
					}
					);
		//Read
		while (1) {
			if (stop_signal_called) break;
			size_t num_rx_samps = 0;
			static int pc = 0;
			// Handle data
			UHD_DO(uhd_rx_streamer_recv(rx_streamer, rx_buff_ptr, rx_sps_buff, &rx_meta, 1, false, &num_rx_samps));
			float max_v = 0;
			std::vector<float> newq;
			for (size_t i=0;i< num_rx_samps && i<rx_sps_buff;++i)
			{
				newq.push_back(rx_buff[i*2]);
				newq.push_back(rx_buff[i*2+1]);
				if (max_v < rx_buff[i*2]) max_v = rx_buff[i*2];
				if (max_v < rx_buff[i*2+1]) max_v = rx_buff[i*2+1];
			}
			mtx.lock();
			buf_queue.push_back(std::move(newq));
			mtx.unlock();



			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, "Error code 0x%x was returned during streaming. Aborting.\n", error_code);
				uhd_get_last_error(error_string,sizeof(error_string));
				throw std::string(error_string);
			}

			if (++pc % 1000==0) {
				time_t full_secs;
				double frac_secs;
				uhd_rx_metadata_time_spec(rx_meta, &full_secs, &frac_secs);
				fprintf(stderr, "Received packet: %zu samples, %lf full secs, %lf frac secs, maxv = %lf\n",
						num_rx_samps,
						difftime(full_secs, (int64_t) 0),
						frac_secs,max_v);
			}
		}

		th_send.join();
	}
	catch(std::string s)
	{
		fputs(s.c_str(),stderr);
	}

	if(tx_buff)	free(tx_buff);

	if(rx_buff)	free(rx_buff);

	rx_buff = NULL;
	rx_buff_ptr = NULL;

	if (tx_streamer) uhd_tx_streamer_free(&tx_streamer);
	if (rx_streamer) uhd_rx_streamer_free(&rx_streamer);
	if (tx_meta) uhd_tx_metadata_free(&tx_meta);
	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;
}