#include "uhd_device.h"
#include <QThread>
#include <QDebug>
#include <QCoreApplication>
using uhd::tune_request_t;
using uhd::tx_streamer;
using uhd::usrp::multi_usrp;
using uhd::rx_streamer;
using std::cerr;
using std::endl;
using std::string;
using std::vector;
using std::shared_ptr;

#define UHD_CALL(X) try{ X;} catch (...) {fprintf(stderr,"Exception in %s:%d\n",__FILE__,__LINE__);}

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)
	,m_rx_streaming(false)
	,m_rx_points(0)
	,m_initThread (new uhd_io_thread(0))
{
	m_data_iq_rx = new short [m_rx_bufsz+1024*1024][2]{{0,0}};
	m_data_iq_tx = new short [m_tx_bufsz+1024*1024][2]{{0,0}};
}

uhd_device::uhd_device(const std::string &args)
	:stop_signal_called(false)
	,m_rx_streaming(false)
	,m_dev_args (args)
	,m_rx_points(0)
	,m_initThread (new uhd_io_thread(0))

{
	m_data_iq_rx = new short [m_rx_bufsz+1024*1024][2]{{0,0}};
	m_data_iq_tx = new short [m_tx_bufsz+1024*1024][2]{{0,0}};
}
uhd_device::~uhd_device()
{
	stop_signal_called = true;
	m_initThread->terminate();
	m_initThread->deleteLater();
	while (m_rx_streaming)
	{
		QThread::msleep(100);
	}

	delete[]	m_data_iq_rx;
	delete[]	m_data_iq_tx;

}

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);
	m_initThread->setRunner([&]()->void{
								try{
									usrp = multi_usrp::make(m_dev_args);
								}
								catch (...)
								{
									fprintf (stderr,"Error in USRP init.\n");
								}
								finished = true;
							});

	m_initThread->start();
	while (!m_initThread->wait(100))
	{
		QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents);
	}



	if (!usrp)
		return false;
	try{
		usrp->set_clock_source(ref,multi_usrp::ALL_MBOARDS);
	}
	catch(...)
	{

	}

	//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 (m_rx_streaming)
	{
		QThread::msleep(1000);
	}
	usrp.reset();

	return true;
}

/*!
 * 范例落盘函数,使用环形队列保持跟随收发
 */
void uhd_device::run_RxIO()
{
	//创建流对象实例
	uhd::stream_args_t stream_args_rx("sc16", "sc16");
	stream_args_rx.channels.push_back(m_rx_channel);
	rx_streamer::sptr rx_stream = usrp->get_rx_stream(stream_args_rx);

	memset (m_data_iq_rx,0,m_rx_bufsz*sizeof(m_data_iq_rx[0]));
	m_rx_points = 0;
	//接收线程
	uhd::rx_metadata_t md_rx;
	uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
	stream_cmd.num_samps  = size_t(m_rxblock_size);
	stream_cmd.stream_now = true;
	stream_cmd.time_spec  = uhd::time_spec_t();
	m_rx_streaming = true;
	rx_stream->issue_stream_cmd(stream_cmd);
	while (!stop_signal_called)
	{
		try {
			while (!stop_signal_called)
			{
				const size_t off = m_rx_points % m_rx_bufsz;
				size_t real_size = rx_stream->recv((void *)(m_data_iq_rx[off]),m_rxblock_size,md_rx,10,false);
				m_rx_points += real_size;
				const size_t new_begin = m_rx_points % m_rx_bufsz;
				if (new_begin < m_rxblock_size)
					memcpy(m_data_iq_rx, &m_data_iq_rx[m_rx_bufsz], sizeof(short)*2*new_begin);
				//md_rx可以读取时戳
				//m_buffer_tmFrag[off] = md_rx.time_spec.get_frac_secs();
				//m_buffer_tmSec[off] = md_rx.time_spec.get_full_secs();
				if (md_rx.error_code == uhd::rx_metadata_t::ERROR_CODE_TIMEOUT)
				{
					fputs("Tout",stderr);
					rx_stream->issue_stream_cmd(stream_cmd);
				}
				else if (md_rx.error_code == uhd::rx_metadata_t::ERROR_CODE_OVERFLOW)
				{
					fputs("O",stderr);
					rx_stream->issue_stream_cmd(stream_cmd);
				}
				else if (md_rx.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE)
				{
					cerr << "Receiver error: "<< md_rx.strerror() << endl ;
					rx_stream->issue_stream_cmd(stream_cmd);
				}
			}

		}  catch (...) {
			//stop_signal_called = true;
		}
	}
	//stream_cmd.stream_now = false;
	//rx_stream->issue_stream_cmd(stream_cmd);
	m_rx_streaming = false;


}

quint64 uhd_device::set_rx_freq(const quint64 freq_in_hz)
{
	if (usrp)
	{
		//double rate = usrp->get_rx_rate(m_rx_channel);
		using uhd::tune_request_t;
		tune_request_t tune_request_rx = tune_request_t((double)freq_in_hz/*,rate/2*/);
		usrp->set_rx_freq(tune_request_rx,m_rx_channel);
		return (quint64)(usrp->get_rx_freq(m_rx_channel));
	}
	else
		return freq_in_hz;
}

double  uhd_device::set_rx_rate(const double sprate)
{
	if (usrp)
	{
		usrp->set_rx_rate(sprate);
		return usrp->get_rx_rate(m_rx_channel);
	}
	return sprate;
}

void  uhd_device::set_rx_atn(const std::string & atn)
{
	if (usrp)
		usrp->set_rx_antenna(atn.c_str(),m_rx_channel);
}
double uhd_device::set_rx_gain(double gain)
{
	if (usrp)
	{
		usrp->set_rx_gain(gain,m_rx_channel);
		return usrp->get_rx_gain(m_rx_channel);
	}
	return gain;
}

double uhd_device::set_rx_bandwidth(double bw)
{
	if (usrp)
	{
		double srate = usrp->get_rx_rate(m_rx_channel);
		fprintf(stderr, "Current RX Rate: %f...\n", srate);
		if (bw < 200000 || bw >=srate)
			bw = srate;
		usrp->set_rx_bandwidth(bw,m_rx_channel);
		bw = usrp->get_rx_bandwidth(m_rx_channel);
	}

	return bw;

}

void uhd_device::run_TxIO()
{
	//创建流对象实例
	uhd::stream_args_t stream_args_tx("sc16", "sc16");
	stream_args_tx.channels.push_back(m_tx_channel);
	tx_streamer::sptr tx_stream = usrp->get_tx_stream(stream_args_tx);

	m_tx_points = 0;
	memset (m_data_iq_tx,0,m_tx_bufsz*sizeof(m_data_iq_tx[0]));
	//发射线程
	uhd::tx_metadata_t md_tx;
	m_tx_streaming = true;
	md_tx.has_time_spec = false;
	md_tx.start_of_burst = false;
	md_tx.end_of_burst = false;
	while (!stop_signal_called)
	{
		try {
			while (!stop_signal_called)
			{
				const size_t off = m_tx_points % m_tx_bufsz;
				const size_t new_begin = (m_tx_points +m_txblock_size ) % m_tx_bufsz;
				if (new_begin < m_txblock_size)
					memcpy(&m_data_iq_tx[m_tx_bufsz], m_data_iq_tx, sizeof(short)*2*new_begin);
				const void * addr = (void *)m_data_iq_tx[off];
				size_t sent = tx_stream->send(addr, m_txblock_size,md_tx, 1);
				m_tx_points += sent;
			}

		}  catch (...) {
			//stop_signal_called = true;
		}
		QThread::msleep(10);
	}
	m_tx_streaming = false;
}

quint64 uhd_device::set_tx_freq(const quint64 freq_in_hz)
{
	if (usrp)
	{
		double rate = usrp->get_tx_rate(m_tx_channel);
		using uhd::tune_request_t;
		tune_request_t tune_request_tx = tune_request_t((double)freq_in_hz/*,rate/2*/);
		usrp->set_tx_freq(tune_request_tx,m_tx_channel);
		return (quint64)(usrp->get_tx_freq(m_tx_channel));
	}
	else
		return freq_in_hz;
}

double  uhd_device::set_tx_rate(const double sprate)
{
	if (usrp)
	{
		usrp->set_tx_rate(sprate);
		return usrp->get_tx_rate(m_tx_channel);
	}
	return sprate;
}

double uhd_device::set_tx_gain(double gain)
{
	if (usrp)
	{
		usrp->set_tx_gain(gain,m_tx_channel);
		return usrp->get_tx_gain(m_tx_channel);
	}
	return gain;
}

double uhd_device::set_tx_bandwidth(double bw)
{
	if (usrp && bw>=200000)
	{
		usrp->set_tx_bandwidth(bw,m_tx_channel);
		bw = usrp->get_tx_bandwidth(m_tx_channel);
	}
	else if (usrp )
	{
		usrp->set_tx_bandwidth(usrp->get_tx_rate(m_tx_channel),m_tx_channel);
		bw = usrp->get_tx_bandwidth(m_tx_channel);
	}

	return bw;

}
