#include "uhd_device.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_rx(false)
	,stop_signal_called_tx(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_rx(false)
	,stop_signal_called_tx(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_rx = true;
	stop_signal_called_tx = true;
	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";
	try{
		UHD_DO(uhd_usrp_make(&usrp, m_dev_args.c_str()));
	}
	catch(std::string er)
	{
		fputs(er.c_str(),stderr);
	}
	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_rx = false;
	stop_signal_called_tx = false;

	return usrp?true:false;
}

bool uhd_device::close_device()
{
	stop_signal_called_rx = true;
	stop_signal_called_tx = true;

	while (m_rx_streaming)
	{
		QThread::msleep(1000);
	}
	if (usrp)
		uhd_usrp_free(&usrp);
	usrp = 0;
	return true;
}

/*!
 * 范例落盘函数,使用环形队列保持跟随收发
 */
void uhd_device::run_RxIO()
{
	//设置接收通道，母板号为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_rx_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;

	memset (m_data_iq_rx,0,m_rx_bufsz*sizeof(m_data_iq_rx[0]));
	m_rx_points = 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
		};
		m_rx_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_rxblock_size));
		fprintf(stderr, "Buffer size in samples: %zu\n", m_rxblock_size);

		try {
			UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
			while (!stop_signal_called_rx)
			{
				const size_t off = m_rx_points % m_rx_bufsz;
				size_t real_size = 0;
				void * addr = (void *)&m_data_iq_rx[off][0];
				UHD_DO(uhd_rx_streamer_recv(rx_streamer,(void **) &addr, m_rxblock_size, &rx_meta, 10, true, &real_size));
				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可以读取时戳
				//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 m_rx_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_rx = true;
		}
		rx_stream_cmd.stream_mode = UHD_STREAM_MODE_STOP_CONTINUOUS;
		UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
		m_rx_streaming = false;
	};
	//启动线程
	uhd_io_thread * rx_thread = new uhd_io_thread(thcall_rx,0);
	rx_thread->start(QThread::TimeCriticalPriority);
	//主线程不断打印状态
	while (!stop_signal_called_rx)
	{
		//cerr<<"RX" << m_rx_points<<"\r";
		QThread::msleep(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_rx_freq(const quint64 freq_in_hz)
{
	if (!usrp)
		return freq_in_hz;
	//double srate = 0;
	//UHD_DO(uhd_usrp_get_rx_rate(usrp, m_rx_channel, &srate));

	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_rx_channel, &rx_tune_result));
	double freq = 0;
	UHD_DO(uhd_usrp_get_rx_freq(usrp, m_rx_channel, &freq));
	fprintf(stderr, "Actual RX frequency: %f MHz...\n", freq / 1e6);
	return (quint64)(freq +.5);
}
double  uhd_device::set_rx_rate(const double sprate)
{
	if (usrp)
	{
		double srate = sprate;
		double bw = sprate;
		if (bw >56e6)
			bw = 56e6;
		if (bw <200000 || bw > sprate)
			bw = sprate;
		fprintf(stderr, "Setting RX Rate: %f...\n", sprate);
		UHD_DO(uhd_usrp_set_rx_rate(usrp, sprate, m_rx_channel));

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

		//Band
		UHD_DO(uhd_usrp_get_rx_bandwidth(usrp, m_rx_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_rx_channel);
}
double uhd_device::set_rx_gain(double gain, const char * name)
{
	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_rx_channel,name));

	// See what gain actually is
	UHD_DO(uhd_usrp_get_rx_gain(usrp, m_rx_channel,name, &gain));
	fprintf(stderr, "Actual RX Gain: %lf...\n", gain);
	return gain;
}
double uhd_device::set_rx_bandwidth(double bw)
{
	if (usrp)
	{
		double srate = 0;
		// See what rate actually is
		UHD_DO(uhd_usrp_get_rx_rate(usrp, m_rx_channel, &srate));
		fprintf(stderr, "Current RX Rate: %f...\n", srate);

		if (bw < 200000 || bw >=srate)
			bw = srate;

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

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

}

void uhd_device::run_TxIO()
{
	//创建流对象实例
	char tx_cpu_format[] = "sc16";
	char tx_otw_format[] = "sc16";
	char tx_args[] = "";
	const size_t tx_channel_count = 1;
	//接收信号。MIMO时，可以指定0,1
	size_t tx_channel[]	= {(size_t)m_tx_channel};
	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
	};
	uhd_tx_streamer_handle tx_streamer = 0;
	uhd_tx_metadata_handle tx_meta = 0;

	memset (m_data_iq_tx,0,m_tx_bufsz*sizeof(m_data_iq_tx[0]));
	m_tx_points = 0;
	//发射线程
	auto thcall_tx = [&]()->void{
		// Create TX streamer
		UHD_DO(uhd_tx_streamer_make(&tx_streamer));
		// Create TX metadata
		UHD_DO(uhd_tx_metadata_make(&tx_meta,false,0,0,true,false));
		m_tx_streaming = true;
		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, &m_txblock_size));
		fprintf(stderr, "TX Buffer size in samples: %zu\n", m_txblock_size);

		try {
			while (!stop_signal_called_tx)
			{
				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);
				size_t real_size = 0;
				const void * addr = (void *)m_data_iq_tx[off];
				UHD_DO(uhd_tx_streamer_send(tx_streamer, &addr, m_txblock_size, &tx_meta, 1, &real_size));
				m_tx_points += real_size;
			}

		}  catch (...) {
			stop_signal_called_tx = true;
		}
		m_tx_streaming = false;
	};
	//启动线程
	uhd_io_thread * tx_thread = new uhd_io_thread(thcall_tx,0);
	tx_thread->start(QThread::TimeCriticalPriority);
	//主线程不断打印状态
	while (!stop_signal_called_tx)
	{
		QThread::msleep(400);
	}
	if (tx_streamer) uhd_tx_streamer_free(&tx_streamer);
	if (tx_meta) uhd_tx_metadata_free(&tx_meta);
	//退出
	tx_thread->wait();
	tx_thread->deleteLater();
}

quint64 uhd_device::set_tx_freq(const quint64 freq_in_hz)
{
	if (!usrp)
		return freq_in_hz;
	double srate = 0;
	UHD_DO(uhd_usrp_get_tx_rate(usrp, m_rx_channel, &srate));

	fprintf(stderr, "Setting TX frequency: %f MHz...\n", freq_in_hz/1e6);
	uhd_tune_result_t tx_tune_result;
	uhd_tune_request_t tx_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_tx_freq(usrp, &tx_tune_request, m_tx_channel, &tx_tune_result));
	double freq = 0;
	UHD_DO(uhd_usrp_get_tx_freq(usrp, m_tx_channel, &freq));
	fprintf(stderr, "Actual TX frequency: %f MHz...\n", freq / 1e6);
	return (quint64)(freq +.5);
}
double uhd_device::set_tx_rate(const double sprate)
{
	if (usrp)
	{
		double srate = sprate;
		double bw = sprate;
		if (bw >56e6)
			bw = 56e6;
		if (bw <200000 || bw > sprate)
			bw = sprate;
		fprintf(stderr, "Setting TX Rate: %f...\n", sprate);
		UHD_DO(uhd_usrp_set_tx_rate(usrp, sprate, m_tx_channel));

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

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

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

		return srate;
	}
	return sprate;
}
double uhd_device::set_tx_gain(double gain, const char * name)
{
	if (!usrp)
		return gain;
	// Set gain
	fprintf(stderr, "Setting TX Gain: %f dB...\n",(float) gain);
	UHD_DO(uhd_usrp_set_tx_gain(usrp, gain,m_tx_channel,name));

	// See what gain actually is
	UHD_DO(uhd_usrp_get_tx_gain(usrp, m_tx_channel,name, &gain));
	fprintf(stderr, "Actual TX Gain: %lf...\n", gain);
	return gain;
}
double uhd_device::set_tx_bandwidth(double bw)
{
	if (usrp)
	{
		double srate = 0;
		// See what rate actually is
		UHD_DO(uhd_usrp_get_tx_rate(usrp, m_tx_channel, &srate));
		fprintf(stderr, "Current TX Rate: %f...\n", srate);

		if (bw < 200000 || bw >=srate)
			bw = srate;

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

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

bool uhd_device::set_rx_agc(bool bAgc)
{
	if (usrp)
	{
		UHD_DO(uhd_usrp_set_rx_agc(usrp, bAgc, m_rx_channel));
	}
	return bAgc;
}
std::vector<std::string> uhd_device::get_rx_atns()
{
	std::vector<std::string> res;
	if (usrp)
	{
		uhd_string_vector_handle stringv;
		uhd_string_vector_make(&stringv);
		uhd_usrp_get_rx_antennas(usrp,m_rx_channel,&stringv);
		size_t szStrings = 0;
		uhd_string_vector_size(stringv, &szStrings);
		char bufo[1024];
		for (size_t i=0;i<szStrings;++i)
		{
			uhd_string_vector_at(stringv,i,bufo,1024);
			res.push_back(bufo);
		}
		uhd_string_vector_free(&stringv);
	}

	return res;
}
std::vector<std::string> uhd_device::get_tx_atns()
{
	std::vector<std::string> res;
	if (usrp)
	{
		uhd_string_vector_handle stringv;
		uhd_string_vector_make(&stringv);
		uhd_usrp_get_tx_antennas(usrp,m_tx_channel,&stringv);
		size_t szStrings = 0;
		uhd_string_vector_size(stringv, &szStrings);
		char bufo[1024];
		for (size_t i=0;i<szStrings;++i)
		{
			uhd_string_vector_at(stringv,i,bufo,1024);
			res.push_back(bufo);
		}
		uhd_string_vector_free(&stringv);
	}

	return res;

}
std::vector<std::string> uhd_device::get_rx_gain_names()
{
	std::vector<std::string> res;
	if (usrp)
	{
		uhd_string_vector_handle stringv;
		uhd_string_vector_make(&stringv);
		uhd_usrp_get_rx_gain_names(usrp,m_rx_channel,&stringv);
		size_t szStrings = 0;
		uhd_string_vector_size(stringv, &szStrings);
		char bufo[1024];
		for (size_t i=0;i<szStrings;++i)
		{
			uhd_string_vector_at(stringv,i,bufo,1024);
			res.push_back(bufo);
		}
		uhd_string_vector_free(&stringv);
	}
	return res;

}
std::vector<std::string> uhd_device::get_tx_gain_names()
{
	std::vector<std::string> res;
	if (usrp)
	{
		uhd_string_vector_handle stringv;
		uhd_string_vector_make(&stringv);
		uhd_usrp_get_tx_gain_names(usrp,m_tx_channel,&stringv);
		size_t szStrings = 0;
		uhd_string_vector_size(stringv, &szStrings);
		char bufo[1024];
		for (size_t i=0;i<szStrings;++i)
		{
			uhd_string_vector_at(stringv,i,bufo,1024);
			res.push_back(bufo);
		}
		uhd_string_vector_free(&stringv);
	}
	return res;

}
