/*
 * 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>
static 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;
}

typedef short SPTYPE;
int  main(int /*argc*/, char * /*argv*/[])
{
    int return_code = EXIT_SUCCESS;
    char dev_args[] = "";
    char error_string[512];
    //通道,B210左侧为A:A，右侧为A:B
    const char subdev_rx[] = "A:A";
    uhd_subdev_spec_handle subdev_hrx;
    UHD_DO(uhd_subdev_spec_make(&subdev_hrx,subdev_rx))

	    //Sample rate in Hz
	    double sprate		=  61.44e6/2 ;
    //接收频率
    double rx_freq		= 107.7e6;
    double rx_sprate	= sprate;
    double rx_gain		= 55.0;
    bool   rx_agc		= false;
    double rx_bw		= 400000;
    //接收信号。MIMO时，可以指定0,1
    size_t rx_channel[]	= {0};
    //转发频率
    const char subdev_tx[] = "A:B";
    uhd_subdev_spec_handle subdev_htx;
    UHD_DO(uhd_subdev_spec_make(&subdev_htx,subdev_tx))


	    double tx_freq		= 89.0e6;
    double tx_sprate	= sprate;
    double tx_gain		= 70;
    double tx_bw		= 400000;
    //发射信号。MIMO时，可以指定0,1
    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;
    //ring buffer
    const size_t sz_buffer = 65536;
    SPTYPE * io_buff[sz_buffer];
    memset(io_buff,0,sizeof(io_buff));
    long long rx_pos = 0, tx_pos = 0;

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

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

	getchar();

	//设置接收通道，母板号为0（multi-usrp支持级联)
	UHD_DO(uhd_usrp_set_rx_subdev_spec(usrp,subdev_hrx,0));
	//设置发射通道，子板号为0（multi-usrp支持级联)
	UHD_DO(uhd_usrp_set_tx_subdev_spec(usrp,subdev_htx,0));

	//设置天线，子板号为0（multi-usrp支持级联)
	UHD_DO(uhd_usrp_set_rx_antenna(usrp,"RX2",0));
	UHD_DO(uhd_usrp_set_tx_antenna(usrp,"TX/RX",0));

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

	// 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_cpu_format[] = "sc16";
	char tx_otw_format[] = "sc16";
	char tx_args[] = "";
	const size_t tx_channel_count = sizeof(tx_channel)/sizeof(tx_channel[0]);

	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 = tx_channel_count
	};
	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[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 rate
	fprintf(stderr, "Setting TX Rate: %f...\n", tx_sprate);
	UHD_DO(uhd_usrp_set_tx_rate(usrp, tx_sprate, tx_channel[0]));

	// See what rate actually is
	UHD_DO(uhd_usrp_get_tx_rate(usrp, tx_channel[0], &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[0], "", &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[0], &tx_tune_result));

	// See what frequency actually is
	UHD_DO(uhd_usrp_get_tx_freq(usrp, tx_channel[0], &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[0]));

	UHD_DO(uhd_usrp_get_tx_bandwidth(usrp, tx_channel[0], &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);

	// 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);

	if (rx_sps_buff!=tx_sps_buff)
	    throw ("RX buffer assumed equal to TX buffer. Stopped.");
	for (size_t i=0;i<sz_buffer;++i)
	    io_buff[i] = (SPTYPE *) malloc(rx_sps_buff * 2 * sizeof(SPTYPE));

	// 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, TX Threading
	std::thread th_send([&]()->void
	{

	    try{
		while (!stop_signal_called) {
		    if (rx_pos<5)
			continue;
		    SPTYPE * tx_buff = io_buff[tx_pos % sz_buffer];
		    const void ** tx_buff_ptr = (const void **) &tx_buff;
		    UHD_DO(uhd_tx_streamer_send(tx_streamer, tx_buff_ptr, tx_sps_buff, &tx_meta, 1, &num_samps_sent));
		    if (tx_pos<rx_pos)
			++tx_pos;
		}
	    }
	    catch(std::string er)
	    {
		fputs(er.c_str(),stderr);
	    }
	}
	);
	//Read, RX in Main Thread
	const int spn = (int(sprate)/rx_sps_buff)<1?1:(int(sprate)/rx_sps_buff);
	while (!stop_signal_called) {
	    size_t num_rx_samps = 0;
	    SPTYPE * rx_buff = (SPTYPE *)io_buff[rx_pos % sz_buffer];
	    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));
	    ++rx_pos;

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

	    if (rx_pos % spn==0) {
		time_t full_secs;
		double frac_secs;
		uhd_rx_metadata_time_spec(rx_meta, &full_secs, &frac_secs);
		fprintf(stderr, "%u samples x %lld cabs, %lf secs, %lf frac secs\r",
			num_rx_samps, rx_pos,
			difftime(full_secs, (int64_t) 0),
			frac_secs);
	    }
	}

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



    for (int i=0;i<sz_buffer;++i) {
	if (io_buff[i])
	    free(io_buff[i]);
    }
    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;
}