#include <uhd/types/tune_request.hpp>
#include <uhd/utils/thread_priority.hpp>
#include <uhd/utils/safe_main.hpp>
#include <uhd/usrp/multi_usrp.hpp>
#include <uhd/exception.hpp>
#include <iostream>
#include <fstream>
#include <csignal>
#include <complex>

using namespace std;
 
int UHD_SAFE_MAIN(int argc, char *argv[]){
    uhd::set_thread_priority_safe();
 
// create usrp handle 
    std::string device_args("type=b200");
    std::cout << boost::format("Creating the usrp device with: %s...") % device_args << std::endl;
    uhd::usrp::multi_usrp::sptr usrp = uhd::usrp::multi_usrp::make(device_args);

// set the motherboard reference clock  
    std::string ref("internal");
    usrp->set_clock_source(ref);
 

// always select the subdevice first, the channel mapping affects the other settings
    std::string subdev("A:A");
    std::cout << boost::format("subdev set to: %f") % subdev << std::endl;
    usrp->set_rx_subdev_spec(string(subdev));
    std::cout << boost::format("Using Device: %s") % usrp->get_pp_string() << std::endl;


// set the antenna
    std::string ant("TX/RX");
    std::cout << boost::format("Setting RX Antenna: %s") % ant << std::endl;
    usrp->set_rx_antenna(ant);
    std::cout << boost::format("Actual RX Antenna: %s") % usrp->get_rx_antenna() << std::endl << std::endl;

// set the sampling rate 
    double rate = 20000000; 
    std::cout << boost::format("Setting RX Rate: %f Msps...") % (rate / 1e6) << std::endl;
    usrp->set_rx_rate(rate);
    std::cout << boost::format("Actual RX Rate: %f Msps...") % (usrp->get_rx_rate() / 1e6) << std::endl << std::endl;


// set the center frequency
    double freq = 2417e6;
    std::cout << boost::format("Setting RX Freq: %f MHz...") % (freq / 1e6) << std::endl;
    uhd::tune_request_t tune_request(freq);
    usrp->set_rx_freq(tune_request);
    std::cout << boost::format("Actual RX Freq: %f MHz...") % (usrp->get_rx_freq() / 1e6) << std::endl << std::endl;

// set the gain
    double gain = 10;
    usrp->set_rx_gain(gain);

// set the bandwidth
    double bw = 20e6;
    std::cout << boost::format("Setting RX Bandwidth: %f MHz...") % (bw / 1e6) << std::endl;
    usrp->set_rx_bandwidth(bw);
    std::cout << boost::format("Actual RX Bandwidth: %f MHz...") % (usrp->get_rx_bandwidth() / 1e6) << std::endl << std::endl;

 
// GPIO Stuff 

    std::string gpio_bank = "FP0";

// Set all pins to manual mode
    usrp->set_gpio_attr(gpio_bank, "CTRL", 0x00, 0xff);

// Set all pins to be output pins
    usrp->set_gpio_attr(gpio_bank, "DDR", 0xff, 0xff);

// Manually set all GPIO pins high
    usrp->set_gpio_attr(gpio_bank, "OUT", 0xff, 0xff);
 
    sleep(1);
 
/////////////// RECEIVE CODE
 
    //create a receive streamer
    uhd::stream_args_t stream_args("sc16","sc16");
    uhd::rx_streamer::sptr rx_stream = usrp->get_rx_stream(stream_args);
 
    uhd::rx_metadata_t md;
    std::vector<std::complex<short> > buffer(50000);
 
    //setup streaming
    uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
    stream_cmd.num_samps = 0;
    stream_cmd.stream_now = true;
    stream_cmd.time_spec = uhd::time_spec_t();
    rx_stream->issue_stream_cmd(stream_cmd);
 
    for(int i = 0; i < 1000; i++) {



        size_t num_rx_samps = rx_stream->recv(&buffer.front(), buffer.size(), md, 3.0, false);
 
        if (md.error_code == uhd::rx_metadata_t::ERROR_CODE_TIMEOUT) { cerr << "timeout"  << endl; }
        if (md.error_code == uhd::rx_metadata_t::ERROR_CODE_OVERFLOW){ cerr << "overflow" << endl; }
        if (md.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE)    { cerr <<  md.strerror() << endl; }

        cout << "data size: " << num_rx_samps << " first: " << (&buffer.front())[0] << endl;
    }
 
    // Will never get here
    stream_cmd.stream_mode = uhd::stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS;
    rx_stream->issue_stream_cmd(stream_cmd);
}