/***********************************************************************
 * Tune RX and TX routine
 **********************************************************************/
static double tune_rx_and_tx(uhd::usrp::multi_usrp::sptr usrp, const double tx_lo_freq, const double rx_offset)
{
    //tune the transmitter with no cordic
    uhd::tune_request_t tx_tune_req(tx_lo_freq);
    tx_tune_req.dsp_freq_policy = uhd::tune_request_t::POLICY_MANUAL;
    tx_tune_req.dsp_freq = 0;
    usrp->set_tx_freq(tx_tune_req);

    //tune the receiver
    double rx_freq = usrp->get_tx_freq() - rx_offset;
    double min_fe_rx_freq = usrp->get_fe_rx_freq_range().start();
    double max_fe_rx_freq = usrp->get_fe_rx_freq_range().stop();
    uhd::tune_request_t rx_tune_req(rx_freq);
    rx_tune_req.dsp_freq_policy = uhd::tune_request_t::POLICY_MANUAL;
    rx_tune_req.dsp_freq = 0;
    if (rx_freq < min_fe_rx_freq)
        rx_tune_req.dsp_freq = rx_freq - min_fe_rx_freq;
    else if (rx_freq > max_fe_rx_freq)
        rx_tune_req.dsp_freq = rx_freq - max_fe_rx_freq;
    usrp->set_rx_freq(rx_tune_req);

    //wait for the LOs to become locked
    boost::this_thread::sleep(boost::posix_time::milliseconds(50));
    boost::system_time start = boost::get_system_time();
    while (not usrp->get_tx_sensor("lo_locked").to_bool() or not usrp->get_rx_sensor("lo_locked").to_bool())
    {
        if (boost::get_system_time() > start + boost::posix_time::milliseconds(100))
            throw std::runtime_error("timed out waiting for TX and/or RX LO to lock");
    }

    return usrp->get_tx_freq();
}
/***********************************************************************
 * Tune RX and TX routine
 **********************************************************************/
static double tune_rx_and_tx(
    uhd::usrp::multi_usrp::sptr usrp, const double tx_lo_freq, const double rx_offset)
{
    // tune the transmitter with no cordic
    uhd::tune_request_t tx_tune_req(tx_lo_freq);
    tx_tune_req.dsp_freq_policy = uhd::tune_request_t::POLICY_MANUAL;
    tx_tune_req.dsp_freq        = 0;
    usrp->set_tx_freq(tx_tune_req);

    // tune the receiver
    double rx_freq        = usrp->get_tx_freq() - rx_offset;
    double min_fe_rx_freq = usrp->get_fe_rx_freq_range().start();
    double max_fe_rx_freq = usrp->get_fe_rx_freq_range().stop();
    uhd::tune_request_t rx_tune_req(rx_freq);
    rx_tune_req.dsp_freq_policy = uhd::tune_request_t::POLICY_MANUAL;
    rx_tune_req.dsp_freq        = 0;
    if (rx_freq < min_fe_rx_freq)
        rx_tune_req.dsp_freq = rx_freq - min_fe_rx_freq;
    else if (rx_freq > max_fe_rx_freq)
        rx_tune_req.dsp_freq = rx_freq - max_fe_rx_freq;
    usrp->set_rx_freq(rx_tune_req);

    wait_for_lo_lock(usrp);

    return usrp->get_tx_freq();
}
/***********************************************************************
 * Tune RX and TX routine
 **********************************************************************/
static double tune_rx_and_tx(shd::smini::multi_smini::sptr smini, const double tx_lo_freq, const double rx_offset) {
    //tune the transmitter with no cordic
    shd::tune_request_t tx_tune_req(tx_lo_freq);
    tx_tune_req.dsp_freq_policy = shd::tune_request_t::POLICY_MANUAL;
    tx_tune_req.dsp_freq = 0;
    smini->set_tx_freq(tx_tune_req);

    //tune the receiver
    smini->set_rx_freq(smini->get_tx_freq() - rx_offset);

    //wait for the LOs to become locked
    boost::this_thread::sleep(boost::posix_time::milliseconds(50));
    boost::system_time start = boost::get_system_time();
    while (not smini->get_tx_sensor("lo_locked").to_bool() or not smini->get_rx_sensor("lo_locked").to_bool()) {
        if (boost::get_system_time() > start + boost::posix_time::milliseconds(100)) {
            throw std::runtime_error("timed out waiting for TX and/or RX LO to lock");
        }
    }

    return smini->get_tx_freq();
}