#include <uhd/rfnoc/block_id.hpp>
#include <uhd/rfnoc/mb_controller.hpp>
#include <uhd/rfnoc/radio_control.hpp>
#include <uhd/rfnoc_graph.hpp>
#include <uhd/types/tune_request.hpp>
#include <uhd/utils/graph_utils.hpp>
#include <uhd/utils/math.hpp>
#include <uhd/utils/safe_main.hpp>
#include <boost/format.hpp>
#include <chrono>
#include <csignal>
#include <iostream>
#include <thread>

using uhd::rfnoc::radio_control;
using namespace std::chrono_literals;

/****************************************************************************
 * SIGINT handling
 ***************************************************************************/
static bool stop_signal_called = false;
void sig_int_handler(int)
{
    stop_signal_called = true;
}

/****************************************************************************
 * main
 ***************************************************************************/
int UHD_SAFE_MAIN(int argc, char* argv[])
{
    std::string args, rx_ant, tx_ant, rx_blockid, tx_blockid, ref, pps;
    size_t total_num_samps, spp, rx_chan, tx_chan;
    double rate, rx_freq, tx_freq, rx_bw, tx_bw, total_time, setup_time;
    bool rx_timestamps;
    
    rate = 200e6;
    
    spp = 0;
    
    rx_freq = 0.0;
    tx_freq = 0.0;
    
    rx_bw = 32e6;
    tx_bw = 32e6;
    
    rx_chan = 0;
    tx_chan = 0;
    
    total_num_samps = 0;
    total_time = 0.0;
    
    rx_blockid = "0/Radio#0";
    tx_blockid = "0/Radio#1";
    
    // Set timestamps on RX
    rx_timestamps = false;
    
    // Setup time
    setup_time = 0.1;
    
    ref = "internal";
    pps = "internal";
    

    /************************************************************************
     * Create device and block controls
     ***********************************************************************/
    std::cout << std::endl;
    std::cout << boost::format("Creating the RFNoC graph with args: %s...") % args
              << std::endl;
    uhd::rfnoc::rfnoc_graph::sptr graph = uhd::rfnoc::rfnoc_graph::make(args);

    // Create handles for radio objects
    uhd::rfnoc::block_id_t rx_radio_ctrl_id(rx_blockid);
    uhd::rfnoc::block_id_t tx_radio_ctrl_id(tx_blockid);
    // This next line will fail if the radio is not actually available
    uhd::rfnoc::radio_control::sptr rx_radio_ctrl =
        graph->get_block<uhd::rfnoc::radio_control>(rx_radio_ctrl_id);
    uhd::rfnoc::radio_control::sptr tx_radio_ctrl =
        graph->get_block<uhd::rfnoc::radio_control>(tx_radio_ctrl_id);
    std::cout << "Using RX radio " << rx_radio_ctrl_id << ", channel " << rx_chan
              << std::endl;
    std::cout << "Using TX radio " << tx_radio_ctrl_id << ", channel " << tx_chan
              << std::endl;
    size_t rx_mb_idx = rx_radio_ctrl_id.get_device_no();

    /************************************************************************
     * Set up radio
     ***********************************************************************/
    // Only forward properties once per block in the chain. In the case of
    // looping back to a single radio block, skip property propagation after
    // traversing back to the starting point of the chain.
    const bool skip_pp = rx_radio_ctrl_id == tx_radio_ctrl_id;

    std::cout << "skip_pp: " << skip_pp << std::endl;

    // Connect manually
    
    uhd::rfnoc::block_id_t rx_radio_ddc_id("0/DDC#0");
    uhd::rfnoc::block_id_t rx_radio_duc_id("0/DUC#0");
    
    uhd::rfnoc::block_id_t tx_radio_duc_id("0/DUC#1");
    
    // RX radio channels to DDCs
    graph->connect(rx_blockid, 0, rx_radio_ddc_id, 0, false);
    graph->connect(rx_blockid, 1, rx_radio_ddc_id, 1, false);
    
    // DDCs to DUCs
    graph->connect(rx_radio_ddc_id, 0, tx_radio_duc_id, 0, false);
    graph->connect(rx_radio_ddc_id, 1, rx_radio_duc_id, 0, false);
    
    // DUCs to radios
    graph->connect(tx_radio_duc_id, 0, tx_radio_ctrl_id, 0, false);
    graph->connect(rx_radio_duc_id, 0, rx_radio_ctrl_id, 0, true);

    graph->commit();

    std::vector<uhd::rfnoc::graph_edge_t> active_connections = graph->enumerate_active_connections();

    std::vector<uhd::rfnoc::graph_edge_t>::iterator iter = active_connections.begin();

    // Print active connections
    for(iter; iter < active_connections.end(); iter++)
    {
	    std::cout << (*iter).to_string() << std::endl;;
    }

    rx_radio_ctrl->enable_rx_timestamps(rx_timestamps, rx_chan);

    // Set time and clock reference
    for (size_t i = 0; i < graph->get_num_mboards(); ++i) {
        graph->get_mb_controller(i)->set_clock_source(ref);
    }
    
    // Lock mboard clocks
    for (size_t i = 0; i < graph->get_num_mboards(); ++i) {
        graph->get_mb_controller(i)->set_time_source(pps);
    }
    
    rate = rx_radio_ctrl->set_rate(rate);

    // set the sample rate
    std::cout << boost::format("Setting RX Rate: %f Msps...") % (rate / 1e6)
              << std::endl;
    rate = rx_radio_ctrl->set_rate(rate);
    std::cout << boost::format("Actual RX Rate: %f Msps...") % (rate / 1e6)
              << std::endl
              << std::endl;

    // set the center frequency

    std::cout << boost::format("Setting RX Freq: %f MHz...") % (rx_freq / 1e6)
              << std::endl;
    
    rx_radio_ctrl->set_rx_frequency(rx_freq, rx_chan);
    std::cout << boost::format("Actual RX Freq: %f MHz...")
                     % (rx_radio_ctrl->get_rx_frequency(rx_chan) / 1e6)
              << std::endl
              << std::endl;

    std::cout << boost::format("Setting TX Freq: %f MHz...") % (tx_freq / 1e6)
              << std::endl;

    tx_radio_ctrl->set_tx_frequency(tx_freq, tx_chan);
    std::cout << boost::format("Actual TX Freq: %f MHz...")
                     % (tx_radio_ctrl->get_tx_frequency(tx_chan) / 1e6)
              << std::endl
              << std::endl;

    std::cout << boost::format("Setting RX Bandwidth: %f MHz...") % (rx_bw / 1e6)
              << std::endl;
    rx_radio_ctrl->set_rx_bandwidth(rx_bw, rx_chan);
    std::cout << boost::format("Actual RX Bandwidth: %f MHz...")
                     % (rx_radio_ctrl->get_rx_bandwidth(rx_chan) / 1e6)
              << std::endl
              << std::endl;

    std::cout << boost::format("Setting TX Bandwidth: %f MHz...") % (tx_bw / 1e6)
              << std::endl;
    tx_radio_ctrl->set_tx_bandwidth(tx_bw, tx_chan);
    std::cout << boost::format("Actual TX Bandwidth: %f MHz...")
                     % (tx_radio_ctrl->get_tx_bandwidth(tx_chan) / 1e6)
              << std::endl
              << std::endl;

    // Antennas

    rx_radio_ctrl->set_rx_antenna("A", 0);
    rx_radio_ctrl->set_rx_antenna("B", 1);
    
    tx_radio_ctrl->set_tx_antenna("A", 0);    
    rx_radio_ctrl->set_tx_antenna("A", 0);


    std::cout << "Setting samples per packet to: " << spp << std::endl;
    rx_radio_ctrl->set_property<int>("spp", spp, 0);
    spp = rx_radio_ctrl->get_property<int>("spp", 0);
    std::cout << "Actual samples per packet = " << spp << std::endl;


    // Allow for some setup time
    std::this_thread::sleep_for(1s * setup_time);

    // Arm SIGINT handler
    std::signal(SIGINT, &sig_int_handler);

    // Calculate timeout and set timers
    // We just need to check is nsamps was set, otherwise we'll use the duration
    if (total_num_samps > 0) {
        total_time = total_num_samps / rate;
        std::cout << boost::format("Expected streaming time: %.3f") % total_time
                  << std::endl;
    }

    // Start streaming
    uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
    stream_cmd.num_samps  = size_t(total_num_samps);
    stream_cmd.stream_now = false;
    stream_cmd.time_spec =
        graph->get_mb_controller(rx_mb_idx)->get_timekeeper(rx_mb_idx)->get_time_now()
        + setup_time;
    std::cout << "Issuing start stream cmd..." << std::endl;
    
    // Are two stream commands required for 2 channel RX?
    rx_radio_ctrl->issue_stream_cmd(stream_cmd, 0);
    rx_radio_ctrl->issue_stream_cmd(stream_cmd, 1);
    
    std::this_thread::sleep_for(1s * setup_time);
    
    std::cout << "Wait..." << std::endl;

    // Wait until we can exit
    uhd::time_spec_t elapsed_time = 0.0;
    while (not stop_signal_called) {
        std::this_thread::sleep_for(100ms);
        if (total_time > 0.0) {
            elapsed_time += 0.1;
            if (elapsed_time > total_time) {
                break;
            }
        }
    }

    // Stop radio
    stream_cmd.stream_mode = uhd::stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS;
    std::cout << "Issuing stop stream cmd..." << std::endl;
    rx_radio_ctrl->issue_stream_cmd(stream_cmd, rx_chan);
    std::cout << "Done" << std::endl;
    // Allow for the samples and ACKs to propagate
    std::this_thread::sleep_for(100ms);

    return EXIT_SUCCESS;
}