#include "uhd_device_win32.h"
#include <QThread>
#include <QDebug>
#include <QCoreApplication>
using std::endl;
using std::string;
using std::vector;
using std::shared_ptr;

#define UHD_DO(X) \
{\
    uhd_error e = (X);\
    char buf_errs[512];\
    if (e) { snprintf(buf_errs,sizeof(buf_errs),"Error in %s line %d, NO %d.",__FILE__,__LINE__,e);\
    std::string ev = __FILE__;\
    ev += ":";\
    ev += buf_errs;\
    return_code = 1;\
    fputs(buf_errs,stderr);\
    }\
    };

uhd_io_thread::uhd_io_thread (std::function<void (void)> runner,QObject * p)
    :QThread(p)
    ,m_runner(runner)

{

}
uhd_io_thread::uhd_io_thread (QObject * p)
    :QThread(p)

{

}

void uhd_io_thread::run()
{
    if (m_runner)
        m_runner();
}

uhd_device::uhd_device()
    :stop_signal_called(false)
    ,streaming(false)
    ,rx_count(0)
{
    m_buffer_tmFrag = new double [bufsz];
    m_buffer_tmSec  = new long long [bufsz];
    m_buffer_iq_all = new short [bufsz * m_spb_size * 2];
    m_buffer_iq = new short * [bufsz];
    m_buf_iqsize = new size_t [bufsz];
    for (int i=0;i<bufsz;++i)
    {
        m_buffer_tmFrag[i] = 0;
        m_buffer_tmSec[i] = 0;
        m_buffer_iq[i] = &m_buffer_iq_all[i * m_spb_size * 2];
        m_buf_iqsize[i] = 0;
    }
}

uhd_device::uhd_device(const std::string &args)
    :stop_signal_called(false)
    ,streaming(false)
    ,m_dev_args (args)
    ,rx_count(0)

{
    m_buffer_tmFrag = new double [bufsz];
    m_buffer_tmSec  = new long long [bufsz];
    m_buffer_iq_all = new short [bufsz * m_spb_size * 2];
    m_buffer_iq = new short * [bufsz];
    m_buf_iqsize = new size_t [bufsz];
    for (int i=0;i<bufsz;++i)
    {
        m_buffer_tmFrag[i] = 0;
        m_buffer_tmSec[i] = 0;
        m_buffer_iq[i] = &m_buffer_iq_all[i * m_spb_size * 2];
        m_buf_iqsize[i] = 0;
    }
}
uhd_device::~uhd_device()
{
    stop_signal_called = true;
    while (streaming)
    {
        QThread::msleep(100);
    }

    delete[]	m_buffer_tmFrag;
    delete[]	m_buffer_tmSec;
    delete[]	m_buffer_iq_all ;
    delete[]	m_buffer_iq;
    delete[]	m_buf_iqsize;


}

void uhd_device::setDevArgs(const std::string &args)
{
    this->m_dev_args = args;

}
std::string uhd_device::devArgs() const
{
    return m_dev_args;
}

bool uhd_device::open_device()
{
    string ref = "internal";
    std::atomic<bool> finished(false);
    std::thread thr([&]()->void{
                        try{
                            UHD_DO(uhd_usrp_make(&usrp, m_dev_args.c_str()));
                        }
                        catch(std::string er)
                        {
                            fputs(er.c_str(),stderr);
                        }
                        finished = true;
                    });

    while (!finished)
    {
        QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
        QThread::msleep(20);
    }
    thr.join();


    if (!usrp)
        return false;
    try{
        uhd_usrp_set_clock_source(usrp,ref.c_str(),0);
    }
    catch(std::string er)
    {
        fputs(er.c_str(),stderr);
    }

    //if (rx_op.subdev.size())
    //	usrp->set_rx_subdev_spec(rx_op.subdev,multi_usrp::ALL_MBOARDS);
    stop_signal_called = false;
    return usrp?true:false;
}

bool uhd_device::close_device()
{
    stop_signal_called = true;
    while (streaming)
    {
        QThread::msleep(1000);
    }
    if (usrp)
        uhd_usrp_free(&usrp);
    usrp = 0;
    return true;
}

/*!
 * 范例落盘函数,使用环形队列保持跟随收发
 */
void uhd_device::run_IO()
{
    //设置接收通道，母板号为0（multi-usrp支持级联)
    //UHD_DO(uhd_usrp_set_rx_subdev_spec(usrp,0,0));

    //创建流对象实例
    char rx_cpu_format[] = "sc16";
    char rx_otw_format[] = "sc16";
    char rx_args[] = "";
    const size_t rx_channel_count = 1;
    //接收信号。MIMO时，可以指定0,1
    size_t rx_channel[]	= {(size_t)m_channel};
    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_rx_streamer_handle rx_streamer = 0;
    uhd_rx_metadata_handle rx_meta = 0;


    rx_count = 0;
    //接收线程
    auto thcall_rx = [&]()->void{
        // Create RX streamer
        UHD_DO(uhd_rx_streamer_make(&rx_streamer));
        // Create RX metadata
        UHD_DO(uhd_rx_metadata_make(&rx_meta));
        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
        };
        streaming = true;
        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, &m_spb_size));
        fprintf(stderr, "Buffer size in samples: %zu\n", m_spb_size);

        try {
            UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
            while (!stop_signal_called)
            {
                const int off = rx_count % bufsz;
                UHD_DO(uhd_rx_streamer_recv(rx_streamer,(void **) &(m_buffer_iq[off]), m_spb_size, &rx_meta, 10, true, &m_buf_iqsize[off]));
                ++rx_count;
                //md_rx可以读取时戳
                UHD_DO(uhd_rx_metadata_time_spec(rx_meta,&m_buffer_tmSec[off],&m_buffer_tmFrag[off]));
                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);
                    uhd_usrp_last_error(usrp, error_string, 512);
                    fputs(error_string,stderr);
                    UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
                }
            }

        }  catch (...) {
            stop_signal_called = true;
        }
        rx_stream_cmd.stream_now = false;
        UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
        streaming = false;
    };
    //启动线程
    uhd_io_thread * rx_thread = new uhd_io_thread(thcall_rx,0);
    rx_thread->start(QThread::TimeCriticalPriority);
    //主线程不断打印状态
    while (!stop_signal_called)
    {
        //cerr<<"RX" << rx_count<<"\r";
        std::this_thread::sleep_for(std::chrono::milliseconds(400));
    }
    if (rx_streamer) uhd_rx_streamer_free(&rx_streamer);
    if (rx_meta) uhd_rx_metadata_free(&rx_meta);
    //退出
    rx_thread->wait();
    rx_thread->deleteLater();
}

quint64 uhd_device::set_center_freq(const quint64 freq_in_hz)
{
    if (!usrp)
        return freq_in_hz;
   fprintf(stderr, "Setting RX frequency: %f MHz...\n", freq_in_hz/1e6);
    uhd_tune_result_t rx_tune_result;
    uhd_tune_request_t rx_tune_request =
    {
        /*.target_freq =*/ (double)freq_in_hz,
        /*.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_DO(uhd_usrp_set_rx_freq(usrp, &rx_tune_request, m_channel, &rx_tune_result));
    double freq = 0;
    UHD_DO(uhd_usrp_get_rx_freq(usrp, m_channel, &freq));
    fprintf(stderr, "Actual RX frequency: %f MHz...\n", freq / 1e6);
    return (quint64)(freq * 1e6 +.5);
}
double  uhd_device::set_sample_rate(const double sprate)
{
    if (usrp)
    {
        double srate = sprate;
        double bw = sprate;
        if (bw >56e6)
            bw = 56e6;
        if (bw <200000)
            bw = 200000;
        fprintf(stderr, "Setting RX Rate: %f...\n", sprate);
        UHD_DO(uhd_usrp_set_rx_rate(usrp, sprate, m_channel));

        // See what rate actually is
        UHD_DO(uhd_usrp_get_rx_rate(usrp, m_channel, &srate));
        fprintf(stderr, "Actual RX Rate: %f...\n", srate);

        fprintf(stderr, "Setting RX Bandwidth: %f MHz...\n", bw/1e6);
        UHD_DO(uhd_usrp_set_rx_bandwidth(usrp, bw, m_channel));

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

        return srate;
    }
    return sprate;
}

void  uhd_device::set_rx_atn(const std::string & atn)
{
    if (usrp)
        uhd_usrp_set_rx_antenna(usrp,atn.c_str(),m_channel);
}
double uhd_device::set_rx_gain(double gain)
{
    if (!usrp)
        return gain;
    // Set gain
    fprintf(stderr, "Setting RX Gain: %f dB...\n",(float) gain);
    UHD_DO(uhd_usrp_set_rx_gain(usrp, gain,m_channel,""));

    // See what gain actually is
    UHD_DO(uhd_usrp_get_rx_gain(usrp, m_channel,"", &gain));
    fprintf(stderr, "Actual RX Gain: %lf...\n", gain);
    return gain;
}
double uhd_device::set_rx_bandwidth(double bw)
{
    if (usrp)
    {
        fprintf(stderr, "Setting RX Bandwidth: %f MHz...\n", bw/1e6);
        UHD_DO(uhd_usrp_set_rx_bandwidth(usrp, bw, m_channel));

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

}