#include <stdio.h>
#include <string>
#include <signal.h>
#include <atomic>
#include <vector>
#include "uhd_thread.h"
#include "cmdlineparser.h"
#include "tb_interface.h"
#include <uhd.h>
using namespace TASKBUS;
using namespace std;
typedef short SPTYPE;
static std::atomic<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;
}
static char error_string[512];
int do_iio(const cmdlineParser & args)
{
    int return_code = EXIT_SUCCESS;
    std::string dev_args = args.toString("dev_args","");

    //Sample rate in Hz
	double sprate		=   args.toDouble("sample_rate",8.0)*1e6;
    const long long time_delay = sprate / 10;
    //收频率
    double rx_freq      =   args.toDouble("rx_rf",1000) * 1e6;
    double rx_sprate    =   sprate;
    double rx_gain      =   args.toDouble("rx_gain",25);
    bool   rx_agc		=   args.toInt("rx_agc",0)?true:false;
    double rx_bw		=   args.toDouble("rx_bw",8.0)*1e6;
    std::string rx_atn  =   args.toString("rx_antenna","RX2");
    bool  rx_on         =   args.toInt("rx_on",0)?true:false;
    int   rx_frame      =   args.toInt("rx_frame",10000);
    //接收信号
    size_t rx_channel[] =   {(size_t)(args.toInt("rx_channel",0)?1:0)};
    //发频率
    double tx_freq		=   args.toDouble("tx_rf",1200) * 1e6;
    double tx_sprate    =   sprate;
    double tx_gain      =   args.toDouble("tx_gain",35);
    double tx_bw		=   args.toDouble("tx_bw",8.0)*1e6;
    std::string tx_atn  =   args.toString("tx_antenna","TX/RX");
    bool  tx_on         =   args.toInt("tx_on",0)?true:false;
    int   tx_frame      =   args.toInt("tx_frame",10000);
    //发射信号
    size_t tx_channel[]   =   {(size_t)(args.toInt("tx_channel",0)?1:0)};
    if (!rx_on && !tx_on)
	{
		fprintf(stderr,"Both RX and TX switch are OFF, nothing can do.\n");
        return 0;
	}
	//const int instance	  = args.toInt("instance",0);
    const int i_wav_rx	  = args.toInt("wav_rx",0);
    const int i_wav_tx	  = args.toInt("wav_tx",0);

    //设备句柄
	uhd_usrp_handle usrp = nullptr;
	uhd_rx_streamer_handle rx_streamer = nullptr;
	uhd_rx_metadata_handle rx_meta = nullptr;
	uhd_tx_streamer_handle tx_streamer = nullptr;
	uhd_tx_metadata_handle tx_meta = nullptr;


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

        fflush(stderr);
        // Ctrl+C will exit loop
        signal(SIGINT, &sigint_handler);
        //设置天线，子板号为0（multi-usrp支持级联)
        if (rx_on)
            UHD_DO(uhd_usrp_set_rx_antenna(usrp,rx_atn.c_str(),0));
        if (tx_on)
            UHD_DO(uhd_usrp_set_tx_antenna(usrp,tx_atn.c_str(),0));

        // Create RX streamer
        if (rx_on)
            UHD_DO(uhd_rx_streamer_make(&rx_streamer));
        // Create TX streamer
        if (tx_on)
            UHD_DO(uhd_tx_streamer_make(&tx_streamer));
        // Create RX metadata
        if (rx_on)
            UHD_DO(uhd_rx_metadata_make(&rx_meta));
        // Create TX metadata
        if (tx_on)
            UHD_DO(uhd_tx_metadata_make(&tx_meta, false, 0, 0.1, true, false));
        fflush(stderr);
        // 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[] = "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
        if (rx_on){
            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);
            fflush(stderr);
        }

        if (tx_on)
        {
            // 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
            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);
            fflush(stderr);
        }

        //ring buffer in short inters
        const size_t sz_buffer_all = 32 * 1024 * 1024;
        std::vector<SPTYPE> buf_rx(sz_buffer_all + 1024*1024,0);
        std::vector<SPTYPE> buf_tx(sz_buffer_all + 1024*1024,0);


        //In IQ Samples, RF io
        long long rx_pos = 0, tx_pos = 0;
        //In IQ Samples, Std IO
        long long stdin_pos = 0, stdout_pos = 0;
        //Define RX Thread, Producer
        std::function<void()> thread_rx = [&]()->void
        {
            SPTYPE * buf_rx_ptr = buf_rx.data();
            try{
                // Issue stream command
                fprintf(stderr, "Issuing stream command.\n");
                UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
                //Read, RX
                while (!stop_signal_called) {
                    size_t num_rx_samps = 0;
                    SPTYPE * rx_buff = &buf_rx_ptr[(rx_pos * 2) % sz_buffer_all];
                    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, 5, true, &num_rx_samps));
                    const size_t bgnext = ((rx_pos + num_rx_samps) * 2) % sz_buffer_all;
                    if (bgnext < num_rx_samps * 2 && bgnext>0)
                    {
						memcpy(buf_rx_ptr, buf_rx_ptr + sz_buffer_all, bgnext * sizeof(SPTYPE) );
                    }
                    rx_pos += 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);
                        fputs(error_string,stderr);
                        //UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
						//stop_signal_called = true;
                    }
                }
             }
             catch (std::string er)
             {
                fputs(er.c_str(),stderr);
                stop_signal_called = true;
             }
        };


        //Define StdOut Thread, Consumer
        std::function<void()> thread_stdout = [&]()->void
        {
            SPTYPE * buf_rx_ptr = buf_rx.data();
            try{
                 //Read, RX
                while (!stop_signal_called) {
                    if (stdout_pos + (long long)(rx_frame + rx_sps_buff + time_delay)  >= rx_pos )
                    {
                        QThread::msleep(1);
                        continue;
                    }
                    const size_t bgnext = ((stdout_pos + rx_frame) * 2) % sz_buffer_all;
                    //Dealing with the tail
                    if (bgnext < rx_frame * 2 && bgnext>0)
                    {
						memcpy( buf_rx_ptr + sz_buffer_all,buf_rx_ptr, bgnext * sizeof(SPTYPE) );
                    }
                    if (i_wav_rx>0)
                        TASKBUS::push_subject(i_wav_rx,0, rx_frame * 2 * sizeof(SPTYPE), (unsigned char *) &buf_rx_ptr[(stdout_pos * 2)% sz_buffer_all] );
                    stdout_pos += rx_frame;
                }
             }
             catch (std::string er)
             {
                fputs(er.c_str(),stderr);
                stop_signal_called = true;
             }
        };

        //Define StdIn Thread, Producer
        std::function<void()> thread_stdin = [&]()->void
        {
            SPTYPE * buf_tx_ptr = buf_tx.data();
            try{
                 //Read, RX
                while (!stop_signal_called) {
                    subject_package_header header;
                    std::vector<unsigned char> packagedta = pull_subject(&header);
                    if (!is_valid_header(header))
                    {
                        fprintf(stderr,"Recived BAD Command.");
                        fflush(stderr);
                        QThread::msleep(100);
                        continue;
                    }
                    if (packagedta.size())
                    {
                        if ( is_control_subject(header))
                        {
                            //收到命令进程退出的广播消息,退出
                            if (strstr((const char *)packagedta.data(),"function=quit;")!=nullptr)
                            {
                                fprintf(stderr,"Recived Quit Command.");
                                fflush(stderr);
                                stop_signal_called = true;
                            }
                        }
                        else if (header.subject_id==i_wav_tx)
                        {
                            int sps_rev = packagedta.size()/2/sizeof(SPTYPE);
                            short * iq = (short *)(packagedta.data());
							memcpy(&buf_tx_ptr[(stdin_pos * 2)% sz_buffer_all],
                                        iq,  sps_rev * sizeof(SPTYPE)*2
                                );
                            const size_t bgnext = ((stdin_pos + sps_rev) * 2) % sz_buffer_all;
                            if (bgnext < sps_rev * 2 && bgnext>0)
                            {
								memcpy(buf_tx_ptr, buf_tx_ptr + sz_buffer_all, bgnext * sizeof(SPTYPE) );
                            }
                            stdin_pos += sps_rev;
                         }
                    }
                }
             }
             catch (std::string er)
             {
                fputs(er.c_str(),stderr);
                stop_signal_called = true;
             }
        };



        //Define thread_tx, Consumer
        std::function<void()> thread_tx = [&]()->void
        {
            try{
                SPTYPE * buf_tx_ptr = buf_tx.data();
                while (!stop_signal_called)
                {
                    size_t num_samps_sent = 0;
                    if (stdin_pos<(long long)(tx_sps_buff + tx_frame + time_delay)*3)
                    {
                        QThread::msleep(1);
                        continue;
                    }
                    if (tx_pos + (long long) (tx_sps_buff + tx_frame + time_delay) >= stdin_pos  )
                    {
                        QThread::msleep(1);
                        continue;
                    }

                    const size_t bgnext = ((tx_pos + tx_sps_buff) * 2) % sz_buffer_all;
                    //Dealing with the tail
                    if (bgnext < tx_sps_buff * 2 && bgnext>0)
                    {
						memcpy( buf_tx_ptr + sz_buffer_all, buf_tx_ptr, bgnext * sizeof(SPTYPE) );
                    }
                    SPTYPE * tx_buff = &buf_tx_ptr[(tx_pos * 2) % sz_buffer_all];
                    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,0.1, &num_samps_sent));
                    tx_pos += num_samps_sent;
                }
            }
            catch(std::string er)
            {
                fputs(er.c_str(),stderr);
                stop_signal_called = true;
            }
        };

        uhd_io_thread th_wav_rx(thread_rx,0);
        uhd_io_thread th_wav_stdout(thread_stdout,0);
        uhd_io_thread th_wav_stdin(thread_stdin,0);
        uhd_io_thread th_wav_tx(thread_tx,0);

        if (rx_on && i_wav_rx>0)
        {
            th_wav_rx.start(QThread::HighestPriority);
            th_wav_stdout.start(QThread::HighestPriority);
        }
        if (tx_on && i_wav_tx>0)
        {
            th_wav_tx.start(QThread::HighestPriority);
        }
        th_wav_stdin.start(QThread::HighestPriority);

        while (!stop_signal_called)
        {
            fprintf (stderr,"%lld,%lld,%lld,%lld\n",rx_pos,stdout_pos,stdin_pos,tx_pos);
            fflush(stderr);
            QThread::msleep(1000);
        }

        th_wav_rx.wait();
        th_wav_stdout.wait();
        th_wav_tx.wait();
        th_wav_stdin.wait();
    }
    catch(std::string s)
    {
        fputs(s.c_str(),stderr);
    }

    fflush(stderr);

    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 != nullptr){
        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"));
    fflush(stderr);
    return return_code;
}
