/*
   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 rxgain, double samplerate) :
    m_txgain(txgain),
    m_rxgain(rxgain),
    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);
    MDEBUG("OutputUHD:Actual TX rate: %f\n", m_usrp->get_tx_rate());
    m_usrp->set_rx_rate(m_samplerate);
    MDEBUG("OutputUHD:Actual RX rate: %f\n", m_usrp->get_rx_rate());

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

    m_usrp->set_rx_freq(234.208e6);
    double set_rx_frequency = m_usrp->get_rx_freq();
    MDEBUG("OutputUHD:Actual RX frequency: %f\n", set_rx_frequency);

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

    MDEBUG("OutputUHD:Setting RX Gain: %f ...\n", m_rxgain);
    m_usrp->set_rx_gain(m_rxgain);
    MDEBUG("OutputUHD:Actual RX Gain: %f ...\n", m_usrp->get_rx_gain());

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

    md.start_of_burst = false;
    md.end_of_burst   = false;
    md.has_time_spec  = true;

    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(
        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();
    myRxStream->issue_stream_cmd(stream_cmd);

}

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

    size_t usrp_max_num_samps = myTxStream->get_max_num_samps();

    md.time_spec      = uhd::time_spec_t(transmit_time);

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

ssize_t OutputUHD::Receive(complexf *samples, size_t sizeIn, double *first_sample_time)
{
    const double rx_timeout = 20.0;
    const size_t usrp_max_num_samps = myRxStream->get_max_num_samps();
    const size_t samps_to_rx = std::min(sizeIn, usrp_max_num_samps);

    uhd::rx_metadata_t md_rx;
    ssize_t num_rx_samps = myRxStream->recv(samples, samps_to_rx, md_rx, rx_timeout);

    if (md_rx.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE){
        MDEBUG("RX Error %s\n", md_rx.strerror().c_str());
        num_rx_samps = -1;
    }
    else if (md_rx.has_time_spec) {
        *first_sample_time = md_rx.time_spec.get_real_secs();
    }
    else {
        *first_sample_time = 0;
    }

    return num_rx_samps;
}

void OutputUHD::SetTxGain(double gain)
{
    m_txgain = gain;

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

void OutputUHD::SetRxGain(double gain)
{
    m_rxgain = gain;

    MDEBUG("OutputUHD:Setting RX Gain: %f ...\n", m_rxgain);
    m_usrp->set_rx_gain(m_rxgain);
    MDEBUG("OutputUHD:Actual RX Gain: %f ...\n", m_usrp->get_rx_gain());
}