/*
 * Copyright 2015 Ettus Research LLC
 * Copyright 2018 Ettus Research, a National Instruments Company
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 * 丁劲犇修改 2021
 */

#include <uhd/types/tune_request.hpp>
#include <uhd/usrp/multi_usrp.hpp>
#include <uhd/utils/safe_main.hpp>
#include <uhd/utils/thread.hpp>
#include <chrono>
#include <complex>
#include <csignal>
#include <fstream>
#include <iostream>
#include <thread>
#include <vector>
#include <atomic>
using uhd::tune_request_t;
using uhd::tx_streamer;
using uhd::usrp::multi_usrp;
using uhd::rx_streamer;
using std::thread;
using std::cerr;
using std::endl;
using std::string;
using std::vector;
using std::shared_ptr;

static std::atomic<bool> stop_signal_called (false);
void sigint_handler(int code){
	(void)code;
	stop_signal_called = true;
}
/*!
 * \brief The tag_channelOptions struct
 * 通道配置参数
 */
struct tag_channelOptions{
	string type = "sc16";		//样点类型，为上位机上类型: fc64, fc32, or sc16
	string subdev="";			//通道，A:A, A:B两个通道，对应B210左侧、右侧接口.一般默认配置，用信道号channels选取接口。
	string ant = "TX/RX";		//天线选取，B210 有 TX/RX 或者 RX2两个
	string wirefmt = "sc16";	//内部类型 (sc8 or sc16)，是片上处理的类型
	vector<size_t> channels {0};//通道号,可以设置0,1之类的。默认subdev时，0=A:A，1=A:B，subdev被修改，则采取修改后的编号
	size_t spb	=	20000;			//缓冲大小，太小会丢包，太大会超时
	double rate = 50e6;		//采样率，单位Hz
	double freq = 1.0e9;			//射频频率，单位Hz
	double gain = 55;				//射频增益，单位dB
	double bw   = 56e6;				//滤波带宽，默认为采样窗口
	double lo_offset = 0;			//LO偏移，单位 Hz (缺省)
	bool   int_n_mod = false;		//int-n 模式（本例不配置）
	bool   docheck = true;			//在开始前执行检查
	double setup_time = 1.0;		//rx配置检查时间，可选。
};
//通道检查函数
bool check_rx_status(const string & ref, multi_usrp::sptr usrp,const tag_channelOptions & op);



/*!
 * 范例落盘函数,使用环形队列保持跟随收发
 */
template <class FMT = short>
void do_save(
		const tag_channelOptions & oprx,
		rx_streamer::sptr rx)
{
	if (oprx.channels.size()>1)
	{
		cerr << "multi channels IO is not suitable for this simple demo."<<endl;
		return;
	}
	//初始化队列
	vector< shared_ptr< FMT > > vec_buffer;
	vector< int > vec_buffersz;
	const int bufsz = 65536;
	for (int i=0;i<bufsz;++i)
	{
		vec_buffer.push_back(shared_ptr< FMT > (new FMT[oprx.spb * 2]));
		vec_buffersz.push_back(0);
	}
	//收发计数
	std::atomic<long long> rx_count (0), tx_count (0);
	//接收线程
	auto thcall_rx = [&]()->void{
		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(oprx.spb);
		stream_cmd.stream_now = true;
		stream_cmd.time_spec  = uhd::time_spec_t();
		rx->issue_stream_cmd(stream_cmd);
		while (!stop_signal_called)
		{
			vec_buffersz[rx_count % bufsz] = rx->recv((void *)(vec_buffer[rx_count % bufsz].get()),oprx.spb,md_rx,0.1,false);
			//md_rx可以读取时戳
			//auto tm_first = md_rx.time_spec;
			++rx_count;
			if (md_rx.error_code == uhd::rx_metadata_t::ERROR_CODE_TIMEOUT)
				fputs("Time out.",stderr);
			else if (md_rx.error_code == uhd::rx_metadata_t::ERROR_CODE_OVERFLOW)
			{
				fputs("Over flow",stderr);
				stop_signal_called = true;
			}
			else if (md_rx.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE)
			{
				cerr << "Receiver error: "<< md_rx.strerror() << endl ;
				stop_signal_called = true;
			}
		}

	};
	//存储线程
	auto th_save = [&]()->void{

		FILE * fp = fopen ("/run/media/user/SYSTEM/56Msps_IQ_F2450MHz.pcm","wb");
		while (!stop_signal_called)
		{
			//等待一会数据，以便TX可以保持跟随。也可以不设置，这样开始时会争夺。
			if (tx_count + 10 >=rx_count)
				continue;
			if (tx_count + 65500 <=rx_count)
				stop_signal_called = true;
			//fwrite((void *)(vec_buffer[tx_count % bufsz].get()),sizeof(FMT)*2,vec_buffersz[tx_count % bufsz],fp);
			++tx_count;
		}
		fclose(fp);
	};

	//启动线程
	thread rx_thread(thcall_rx);
	thread tx_thread(th_save);
	cerr<<"Press ^C to Stop."<<endl;
	//主线程不断打印状态
	while (!stop_signal_called)
	{
		cerr<<"RX" << rx_count<< " TX " << tx_count<<"\r";
		std::this_thread::sleep_for(std::chrono::milliseconds(400));
	}
	//退出
	rx_thread.join();
	tx_thread.join();

}


int UHD_SAFE_MAIN(int /*argc*/, char* /*argv*/[])
{
	//1.创建一个设备，可以不填写，则使用第一台设备。
	string args ("");
	//2.设置时钟参考 (internal, external, mimo)
	string ref = "internal";
	cerr << "Creating the usrp device with: "  << args  <<"..."<< endl;
	multi_usrp::sptr usrp = multi_usrp::make(args);
	usrp->set_clock_source(ref,multi_usrp::ALL_MBOARDS);


	//3.配置接收
	tag_channelOptions rx_op;
	rx_op.ant = "RX2";
	rx_op.rate = 50e6;
	rx_op.bw = 50e6;
	rx_op.freq = 2450e6;
	rx_op.gain = 35;
	rx_op.channels[0] = 0;
	//3.1 子设备
	if (rx_op.subdev.size())
		usrp->set_rx_subdev_spec(rx_op.subdev,multi_usrp::ALL_MBOARDS);
	cerr << "RX Using Device: " << usrp->get_pp_string() << endl;
	//3.2 采样率
	cerr << "Setting RX Rate: " <<  (rx_op.rate / 1e6) << "Msps..." << endl;
	usrp->set_rx_rate(rx_op.rate,multi_usrp::ALL_CHANS);
	cerr << "Actual RX Rate: " << usrp->get_rx_rate(rx_op.channels[0]) / 1e6 << "Msps..." << endl;
	//3.3 中心频率
	cerr << "Setting RX Freq: "  << (rx_op.freq / 1e6) <<"MHz..." << endl;
	cerr << "Setting RX LO Offset: " << (rx_op.lo_offset / 1e6) << "MHz..." <<endl;
	tune_request_t tune_request_rx = tune_request_t(rx_op.freq, rx_op.lo_offset);
	if (rx_op.int_n_mod)
		tune_request_rx.args = uhd::device_addr_t("mode_n=integer");
	usrp->set_rx_freq(tune_request_rx,rx_op.channels[0]);
	cerr << "Actual RX Freq: " <<  (usrp->get_rx_freq(rx_op.channels[0]) / 1e6) << "MHz..." << endl;
	//3.4 增益
	cerr << "Setting RX Gain: " << rx_op.gain <<" dB..." << endl;
	usrp->set_rx_gain(rx_op.gain,rx_op.channels[0]);
	cerr << "Actual RX Gain: " << usrp->get_rx_gain(rx_op.channels[0]) << " dB..." << endl;
	//3.5 前端模拟滤波带宽
	cerr << "Setting RX Bandwidth: " << (rx_op.bw / 1e6) << "MHz..."  << endl;
	usrp->set_rx_bandwidth(rx_op.bw,rx_op.channels[0]);
	cerr << "Actual RX Bandwidth: " << usrp->get_rx_bandwidth(rx_op.channels[0]) / 1e6 << "MHz..." << endl;
	//3.6 选择天线
	if (rx_op.ant.size())
		usrp->set_rx_antenna(rx_op.ant,rx_op.channels[0]);

	//4 检查状态
	if (rx_op.docheck)	check_rx_status(ref,usrp,rx_op);

	//5.创建流对象实例
	uhd::stream_args_t stream_args_rx(rx_op.type, rx_op.wirefmt);
	stream_args_rx.channels             = rx_op.channels;
	rx_streamer::sptr rx_stream = usrp->get_rx_stream(stream_args_rx);

	//开始收发循环
	do_save<short>(rx_op,rx_stream);

	// finished
	cerr << endl << "Done!" << endl << endl;

	return EXIT_SUCCESS;
}


typedef std::function<uhd::sensor_value_t(const string&)> get_sensor_fn_t;
bool check_locked_sensor(vector<string> sensor_names,
						 const char* sensor_name,
						 get_sensor_fn_t get_sensor_fn,
						 double setup_time)
{
	if (std::find(sensor_names.begin(), sensor_names.end(), sensor_name)
			== sensor_names.end())
		return false;

	auto setup_timeout = std::chrono::steady_clock::now()
			+ std::chrono::milliseconds(int64_t(setup_time * 1000));
	bool lock_detected = false;

	std::cerr << "Checking RX Waiting for: " << sensor_name;
	std::cerr.flush();

	while (true) {
		if (lock_detected and (std::chrono::steady_clock::now() > setup_timeout)) {
			std::cerr << " locked." << std::endl;
			break;
		}
		if (get_sensor_fn(sensor_name).to_bool()) {
			std::cerr << "+";
			std::cerr.flush();
			lock_detected = true;
		} else {
			if (std::chrono::steady_clock::now() > setup_timeout) {
				std::cerr << std::endl;
				std::cerr << "timed out waiting for consecutive locks on sensor : "<<sensor_name;
				return false;
			}
			std::cerr.flush();
		}
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
	}
	std::cout << std::endl;
	return true;
}

bool check_rx_status(const string & ref, multi_usrp::sptr usrp,const tag_channelOptions & op)
{
	// check Ref and LO Lock detect
	for (size_t c :op.channels)
	{
		check_locked_sensor(usrp->get_rx_sensor_names(c),
							"lo_locked",
							[usrp, op, c](const string& sensor_name) {
			return usrp->get_rx_sensor(sensor_name, c);
		},
		op.setup_time);
	}
	if (ref == "mimo") {
		check_locked_sensor(usrp->get_mboard_sensor_names(0),
							"mimo_locked",
							[usrp](const string& sensor_name) {
			return usrp->get_mboard_sensor(sensor_name);
		},
		op.setup_time);
	}
	if (ref == "external") {
		check_locked_sensor(usrp->get_mboard_sensor_names(0),
							"ref_locked",
							[usrp](const string& sensor_name) {
			return usrp->get_mboard_sensor(sensor_name);
		},
		op.setup_time);
	}
	return true;
}
