/*
 * Copyright 2015 Ettus Research LLC
 * Copyright 2018 Ettus Research, a National Instruments Company
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#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>
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;

static bool stop_signal_called = false;
void sigint_handler(int code){
	(void)code;
	stop_signal_called = true;
}

struct tag_channelOptions{
	std::string type = "sc16";		//sample type: fc64, fc32, or sc16
	std::string ant = "TX/RX";		//antenna selection
	std::string subdev;				//subdevice specification
	std::string wirefmt;			//wire format (sc8 or sc16)
	size_t channel = 0;				//which channel to use
	size_t spb	=	10000;			//samples per buffer
	double rate = 61.44e6/2;		//rate of outgoing samples
	double freq = 1.0e9;			//RF center frequency in Hz
	double gain = 55;				//gain for the RF chain
	double bw   = rate * 0.8;		//analog frontend filter bandwidth in Hz
	double lo_offset = 0;			//Offset for frontend LO in Hz (optional)
	bool   int_n_mod = false;		//int-n mode
	double setup_time = 3.0;		//setup time for rx
};
bool check_tx_status(const std::string & ref, multi_usrp::sptr usrp);
bool check_rx_status(const std::string & ref, multi_usrp::sptr usrp,const tag_channelOptions & op);

template <class FMT>
void do_io(
		const tag_channelOptions & oprx,
		const tag_channelOptions & optx,
		rx_streamer::sptr rx,
		tx_streamer::sptr tx)
{
	std::vector< std::shared_ptr< FMT > > vec_buffer;
	const int bufsz = 65536;
	for (int i=0;i<bufsz;++i)
		vec_buffer.push_back(std::shared_ptr< FMT > (new FMT[oprx.spb]));

	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)
		{
			const size_t samples_recv = rx->recv((void *)(vec_buffer[rx_count % bufsz].get()),oprx.spb,md_rx,0.1,false);
			++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);
			else if (md_rx.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE)
				cerr << "Receiver error: "<< md_rx.strerror() << endl ;
			if (samples_recv != oprx.spb)
				cerr << "Receiver error, samples_recv "<<samples_recv << "!= spb " <<oprx.spb<< endl ;
		}
	};

	auto thcall_tx = [&]()->void{
		uhd::tx_metadata_t md_tx;
		md_tx.end_of_burst   = false;
		md_tx.start_of_burst = false;
		while (!stop_signal_called)
		{
			const size_t samples_sent = tx->send((void *)(vec_buffer[tx_count % bufsz].get()),oprx.spb,md_tx,0.1);
			if (tx_count<rx_count)
				++tx_count;
			if (samples_sent != optx.spb)
				cerr<<"The tx_stream timed out sending " << optx.spb << " samples ("  << samples_sent << " sent)." << endl;
		}
	};

	std::cout<<"Press Enter to Start."<<endl;

	thread rx_thread(thcall_rx);
	thread tx_thread(thcall_tx);

	std::cout<<"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*/[])
{
	// create a usrp device
	std::string args ("");
	//reference source (internal, external, mimo)
	std::string ref = "internal";
	cerr << "Creating the usrp device with: "  << args  <<"..."<< endl;
	multi_usrp::sptr usrp = multi_usrp::make(args);
	usrp->set_clock_source(ref);

	//Set Tx channal----------------------

	tag_channelOptions tx_op;
	tx_op.channel = 1;//using channel No.2
	tx_op.freq = 88.0e6;
	//subdevice specification
	if (tx_op.subdev.size())
		usrp->set_tx_subdev_spec(tx_op.subdev);
	cerr << "TX Using Device: " << usrp->get_pp_string() << endl;

	cerr << "Setting TX Rate: " <<  (tx_op.rate / 1e6) << "Msps..." << endl;
	usrp->set_tx_rate(tx_op.rate);
	cerr << "Actual TX Rate: " << usrp->get_tx_rate() / 1e6 << "Msps..." << endl;

	// set the center frequency
	cerr << "Setting TX Freq: "  << (tx_op.freq / 1e6) <<"MHz..." << endl;
	cerr << "Setting TX LO Offset: " << (tx_op.lo_offset / 1e6) << "MHz..." <<endl;
	tune_request_t tune_request_tx = tune_request_t(tx_op.freq, tx_op.lo_offset);
	if (tx_op.int_n_mod)
		tune_request_tx.args = uhd::device_addr_t("mode_n=integer");
	usrp->set_tx_freq(tune_request_tx);
	cerr << "Actual TX Freq: " <<  (usrp->get_tx_freq() / 1e6) << "MHz..." << endl;

	// set the rf gain
	cerr << "Setting TX Gain: " << tx_op.gain <<" dB..." << endl;
	usrp->set_tx_gain(tx_op.gain);
	cerr << "Actual TX Gain: " << usrp->get_tx_gain() << " dB..." << endl;

	// set the analog frontend filter bandwidth
	cerr << "Setting TX Bandwidth: " << (tx_op.bw / 1e6) << "MHz..."  << endl;
	usrp->set_tx_bandwidth(tx_op.bw);
	cerr << "Actual TX Bandwidth: " << usrp->get_tx_bandwidth() / 1e6 << "MHz..." << endl;
	// set the antenna
	if (tx_op.ant.size())
		usrp->set_tx_antenna(tx_op.ant);

	//Set Rx Channel
	tag_channelOptions rx_op;
	rx_op.ant = "RX2";
	rx_op.channel = 0;
	//subdevice specification
	if (rx_op.subdev.size())
		usrp->set_rx_subdev_spec(rx_op.subdev);
	cerr << "RX Using Device: " << usrp->get_pp_string() << endl;

	cerr << "Setting RX Rate: " <<  (rx_op.rate / 1e6) << "Msps..." << endl;
	usrp->set_rx_rate(rx_op.rate);
	cerr << "Actual RX Rate: " << usrp->get_rx_rate() / 1e6 << "Msps..." << endl;

	// set the center frequency
	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);
	cerr << "Actual RX Freq: " <<  (usrp->get_rx_freq() / 1e6) << "MHz..." << endl;

	// set the rf gain
	cerr << "Setting RX Gain: " << rx_op.gain <<" dB..." << endl;
	usrp->set_rx_gain(rx_op.gain);
	cerr << "Actual RX Gain: " << usrp->get_rx_gain() << " dB..." << endl;

	// set the analog frontend filter bandwidth
	cerr << "Setting RX Bandwidth: " << (rx_op.bw / 1e6) << "MHz..."  << endl;
	usrp->set_rx_bandwidth(rx_op.bw);
	cerr << "Actual RX Bandwidth: " << usrp->get_rx_bandwidth() / 1e6 << "MHz..." << endl;
	// set the antenna
	if (rx_op.ant.size())
		usrp->set_rx_antenna(rx_op.ant);

	check_tx_status(ref,usrp);
	check_rx_status(ref,usrp,rx_op);
	// create a transmit streamer
	std::vector<size_t> channel_nums_tx;
	uhd::stream_args_t stream_args_tx(tx_op.type, tx_op.wirefmt);
	channel_nums_tx.push_back(tx_op.channel);
	stream_args_tx.channels             = channel_nums_tx;
	tx_streamer::sptr tx_stream = usrp->get_tx_stream(stream_args_tx);

	// create a receive streamer
	uhd::stream_args_t stream_args_rx(rx_op.type, rx_op.wirefmt);
	std::vector<size_t> channel_nums_rx;
	channel_nums_rx.push_back(rx_op.channel);
	stream_args_rx.channels             = channel_nums_rx;
	rx_streamer::sptr rx_stream = usrp->get_rx_stream(stream_args_rx);


	do_io<short>(rx_op,tx_op,rx_stream,tx_stream);

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

	return EXIT_SUCCESS;
}


bool check_tx_status(const std::string & ref, multi_usrp::sptr usrp)
{
	// Check Ref and LO Lock detect
	std::vector<std::string> sensor_names;
	sensor_names = usrp->get_tx_sensor_names(0);
	if (std::find(sensor_names.begin(), sensor_names.end(), "lo_locked")
			!= sensor_names.end()) {
		uhd::sensor_value_t lo_locked = usrp->get_tx_sensor("lo_locked", 0);
		cerr <<"Checking TX: "<< lo_locked.to_pp_string() << std::endl;
		UHD_ASSERT_THROW(lo_locked.to_bool());
	}
	sensor_names = usrp->get_mboard_sensor_names(0);
	if ((ref == "mimo")
			and (std::find(sensor_names.begin(), sensor_names.end(), "mimo_locked")
				 != sensor_names.end())) {
		uhd::sensor_value_t mimo_locked = usrp->get_mboard_sensor("mimo_locked", 0);
		std::cerr << "Checking TX: "<< mimo_locked.to_pp_string()  << std::endl;
		UHD_ASSERT_THROW(mimo_locked.to_bool());
	}
	if ((ref == "external")
			and (std::find(sensor_names.begin(), sensor_names.end(), "ref_locked")
				 != sensor_names.end())) {
		uhd::sensor_value_t ref_locked = usrp->get_mboard_sensor("ref_locked", 0);
		std::cout << "Checking TX: %s ..."<< ref_locked.to_pp_string()  << std::endl;
		UHD_ASSERT_THROW(ref_locked.to_bool());
	}
	return true;
}
typedef std::function<uhd::sensor_value_t(const std::string&)> get_sensor_fn_t;
bool check_locked_sensor(std::vector<std::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 std::string & ref, multi_usrp::sptr usrp,const tag_channelOptions & op)
{
	// check Ref and LO Lock detect
	check_locked_sensor(usrp->get_rx_sensor_names(op.channel),
						"lo_locked",
						[usrp, op](const std::string& sensor_name) {
		return usrp->get_rx_sensor(sensor_name, op.channel);
	},
	op.setup_time);
	if (ref == "mimo") {
		check_locked_sensor(usrp->get_mboard_sensor_names(0),
							"mimo_locked",
							[usrp](const std::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 std::string& sensor_name) {
			return usrp->get_mboard_sensor(sensor_name);
		},
		op.setup_time);
	}
	return true;
}