/*
   Copyright (C) 2015
   Matthias P. Braendli, matthias.braendli@mpb.li

    http://opendigitalradio.org
 */
/*
   This file is part of ODR-DPD.

   ODR-DPD is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as
   published by the Free Software Foundation, either version 3 of the
   License, or (at your option) any later version.

   ODR-DPD is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with ODR-DPD.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "OutputUHD.hpp"
#include "utils.hpp"
#include <cmath>
#include <iostream>
#include <assert.h>
#include <stdexcept>
#include <stdio.h>
#include <time.h>
#include <errno.h>
#include <unistd.h>

#include <uhd/utils/thread_priority.hpp>
#include <uhd/utils/safe_main.hpp>

using namespace std;

OutputUHD::OutputUHD(double txgain, double samplerate)
{
    m_txgain = txgain;
    m_samplerate = samplerate;

    uhd::set_thread_priority_safe();

    string device = "master_clock_rate=32768000";

    m_usrp = uhd::usrp::multi_usrp::make(device);

    m_usrp->set_tx_rate(m_samplerate);
    m_usrp->set_rx_rate(m_samplerate);

    m_usrp->set_tx_freq(234.208e6);
    double set_frequency = m_usrp->get_tx_freq();
    MDEBUG("OutputUHD:Actual frequency: %f\n", set_frequency);

    m_usrp->set_tx_gain(m_txgain);
    MDEBUG("OutputUHD:Actual TX Gain: %f ...\n", m_usrp->get_tx_gain());


    double tx_time = m_usrp->get_time_now().get_real_secs();
    MDEBUG("OutputUHD: USRP time %f\n",
            tx_time);

    md.start_of_burst = false;
    md.end_of_burst   = false;
    md.has_time_spec  = true;
    md.time_spec      = uhd::time_spec_t(tx_time + 2);

    uhd::stream_args_t stream_args("fc32"); //complex floats
    myTxStream = m_usrp->get_tx_stream(stream_args);

    myRxStream = m_usrp->get_rx_stream(stream_args);
    uhd::stream_cmd_t stream_cmd(true ?
        uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS:
        uhd::stream_cmd_t::STREAM_MODE_NUM_SAMPS_AND_DONE
    );
    stream_cmd.num_samps = 0;
    stream_cmd.stream_now = true;
    stream_cmd.time_spec = uhd::time_spec_t();
    myRxStream->issue_stream_cmd(stream_cmd);

}

size_t OutputUHD::Transmit(const complexf *samples, size_t sizeIn, double *first_sample_time)
{
    const double tx_timeout = 20.0;

    size_t usrp_max_num_samps = myTxStream->get_max_num_samps();

    *first_sample_time = md.time_spec.get_real_secs();

    size_t num_acc_samps = 0; //number of accumulated samples
    while (num_acc_samps < sizeIn) {
        size_t samps_to_send = std::min(sizeIn - num_acc_samps, usrp_max_num_samps);

        //send a single packet
        size_t num_tx_samps = myTxStream->send(
                &samples[num_acc_samps],
                samps_to_send, md, tx_timeout);

        num_acc_samps += num_tx_samps;

        md.time_spec += uhd::time_spec_t(0, num_tx_samps/m_samplerate);
    }

    return num_acc_samps;
}

size_t OutputUHD::Receive(complexf *samples, size_t sizeIn, double *first_sample_time)
{
    const double rx_timeout = 20.0;

    uhd::rx_metadata_t md;
    size_t usrp_max_num_samps = myRxStream->get_max_num_samps();

    *first_sample_time = md.time_spec.get_real_secs();

    size_t num_acc_samps = 0; //number of accumulated samples
    while (num_acc_samps < sizeIn) {
        size_t samps_to_send = std::min(sizeIn - num_acc_samps, usrp_max_num_samps);

        //send a single packet
        size_t num_rx_samps = myRxStream->recv(
                &samples[num_acc_samps],
                samps_to_send, md, rx_timeout);

        if (num_acc_samps == 0) {
            if (md.has_time_spec) {
                *first_sample_time = md.time_spec.get_real_secs();
            }
            else {
                MDEBUG("samp %zu no time spec!\n", num_acc_samps);
            }
        }

        num_acc_samps += num_rx_samps;
    }

    return num_acc_samps;
}