path: root/main.cpp

/*
   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 <zmq.hpp>
#include <thread>
#include <vector>
#include <deque>
#include <mutex>
#include <atomic>
#include <csignal>
#include <iostream>
#include <future>

std::atomic<bool> running;

void sig_int_handler(int) {
    running = false;
}

const size_t samps_per_buffer = 20480;
const size_t samplerate = 2048000;

size_t read_samples(FILE* fd, std::vector<complexf>& samples, size_t count)
{
    if (samples.size() < count) {
        MDEBUG("HAD TO RESIZE BUFFER!\n");
        samples.resize(count);
    }

    size_t num_read = fread(&samples.front(), sizeof(complexf), count, fd);
    if (num_read == 0) {
        rewind(fd);
        num_read = fread(&samples.front(), sizeof(complexf), count, fd);
    }

    return num_read;
}

class AlignSample {
    public:
        AlignSample() {
            m_rx_sample_time = 0;
            m_tx_sample_time = 0;
        }

        void push_tx_samples(complexf* samps, size_t len, double first_sample_time) {
            std::lock_guard<std::mutex> lock(m_mutex);
            std::copy(samps, samps + len, std::back_inserter(m_txsamples));

            if (m_tx_sample_time == 0) {
                m_tx_sample_time = first_sample_time;
            }
        }

        void push_rx_samples(complexf* samps, size_t len, double first_sample_time) {
            std::lock_guard<std::mutex> lock(m_mutex);
            std::copy(samps, samps + len, std::back_inserter(m_rxsamples));

            if (m_rx_sample_time == 0) {
                m_rx_sample_time = first_sample_time;
            }
        }

        bool ready() {
            std::lock_guard<std::mutex> lock(m_mutex);
            return aligned() and m_rxsamples.size() > 8000 and m_txsamples.size() > 8000;
        }

        void debug() {
            MDEBUG("Aligner\n");
            MDEBUG("  RX: %f %zu\n", m_rx_sample_time, m_rxsamples.size());
            MDEBUG("  TX: %f %zu\n", m_tx_sample_time, m_txsamples.size());
        }

        std::vector<complexf> crosscorrelate(size_t max_offset, size_t len) {
            std::vector<complexf> rxsamps;
            std::vector<complexf> txsamps;

            // Do a quick copy, so as to free the mutex
            {
                std::lock_guard<std::mutex> lock(m_mutex);

                if (m_rxsamples.size() < len or
                    m_txsamples.size() < len + max_offset) {
                    return {};
                }

                std::copy(m_rxsamples.begin(), m_rxsamples.begin() + len, std::back_inserter(rxsamps));
                std::copy(m_txsamples.begin(), m_txsamples.begin() + len + max_offset, std::back_inserter(txsamps));
            }

            std::vector<complexf> xcorrs(max_offset);

            for (size_t offset = 0; offset < max_offset; offset++) {
                complexf xcorr(0, 0);

                for (size_t i = 0; i < len; i++) {
                    xcorr += rxsamps[i] * std::conj(txsamps[i+offset]);
                }
                xcorrs[offset] = xcorr;
            }
            return xcorrs;
        }

        void consume(size_t samples)
        {
            std::lock_guard<std::mutex> lock(m_mutex);
            if (aligned() and m_rxsamples.size() > samples and m_txsamples.size() > samples) {
                m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + samples);
                m_rx_sample_time += (double)samples / samplerate;

                m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + samples);
                m_tx_sample_time += (double)samples / samplerate;
            }
        }

    private:
        bool aligned() {
            if (std::abs(m_rx_sample_time - m_tx_sample_time) < 1e-6) {
                return true;
            }
            else if (m_rx_sample_time < m_tx_sample_time) {
                size_t rx_samples_to_skip = (m_tx_sample_time - m_rx_sample_time) * samplerate;

                if (rx_samples_to_skip > m_rxsamples.size()) {
                    return false;
                }

                m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + rx_samples_to_skip);
                m_rx_sample_time += (double)rx_samples_to_skip / samplerate;
                return true;
            }
            else if (m_rx_sample_time > m_tx_sample_time) {
                size_t tx_samples_to_skip = (m_rx_sample_time - m_tx_sample_time) * samplerate;

                if (tx_samples_to_skip > m_txsamples.size()) {
                    return false;
                }

                m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + tx_samples_to_skip);
                m_tx_sample_time += (double)tx_samples_to_skip / samplerate;
                return true;
            }
            return false;
        }

        std::mutex m_mutex;
        double m_rx_sample_time;
        std::deque<complexf> m_rxsamples;

        double m_tx_sample_time;
        std::deque<complexf> m_txsamples;
};

AlignSample aligner;

size_t do_receive(OutputUHD* output_uhd)
{
    std::vector<complexf> samps(samps_per_buffer);
    double first_sample_time = 0;

    size_t total_received = 0;
    double last_print_time = 0;

    MDEBUG("Starting do_receive\n");
    while (running) {
        size_t received = output_uhd->Receive(&samps.front(), samps.size(), &first_sample_time);
        aligner.push_rx_samples(&samps.front(), received, first_sample_time);
        total_received += received;

        if (first_sample_time - last_print_time > 1) {
            //MDEBUG("Rx %zu samples at t=%f\n", received, first_sample_time);
            last_print_time = first_sample_time;
        }
    }
    MDEBUG("Leaving do_receive\n");

    return total_received;
}

void find_peak_correlation()
{
    while (running) {
        if (aligner.ready()) {
            const size_t max_offset = 100000; // 48ms at 2048000
            const size_t correlation_length = 100;
            std::vector<complexf> correlations(max_offset);
            double max_ampl = 0.0;
            size_t pos_max = 0;

            auto xcs = aligner.crosscorrelate(max_offset, correlation_length);

            for (size_t offset = 0; offset < xcs.size(); offset++) {
                complexf xc = xcs[offset];
                if (std::abs(xc) >= max_ampl) {
                    max_ampl = std::abs(xc);
                    pos_max = offset;
                }
            }
            MDEBUG("Max correlation is %f at %zu\n", max_ampl, pos_max);
            std::this_thread::sleep_for(std::chrono::microseconds(1));

            // Eat much more than we correlate, because correlation is slow
            aligner.consume(2048000);
            aligner.debug();
        }
        else {
            MDEBUG("Waiting for correlation\n");
            aligner.debug();
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
    }
}


int main(int argc, char **argv)
{
    double txgain = 0;
    double rxgain = 0;

    if (argc >= 3) {
        txgain = strtod(argv[2], nullptr);
        if (!(0 <= txgain and txgain < 80)) {
            MDEBUG("txgain wrong: %f\n", txgain);
            return -1;
        }
    }

    if (argc >= 4) {
        rxgain = strtod(argv[3], nullptr);
        if (!(0 <= rxgain and rxgain < 80)) {
            MDEBUG("rxgain wrong: %f\n", rxgain);
            return -1;
        }
    }

    MDEBUG("TX Gain is %f\n", txgain);
    MDEBUG("RX Gain is %f\n", rxgain);

    if (argc < 2) {
        MDEBUG("Require input file or url\n");
        return -1;
    }

    std::string uri = argv[1];

    OutputUHD output_uhd(txgain, rxgain, samplerate);

    zmq::context_t ctx;
    zmq::socket_t zmq_sock(ctx, ZMQ_SUB);

    FILE* fd = nullptr;
    if (uri.find("tcp://") != 0) {
        fd = fopen(uri.c_str(), "rb");
        if (!fd) {
            MDEBUG("Could not open file\n");
            return -1;
        }
    }
    else {
        zmq_sock.connect(uri);
        zmq_sock.setsockopt(ZMQ_SUBSCRIBE, NULL, 0);
    }

    std::vector<complexf> input_samples(samps_per_buffer);
    size_t samps_read = 0;
    size_t total_samps_read = samps_read;

    double last_print_time = 0;
    size_t sent = 0;

    std::signal(SIGINT, &sig_int_handler);

    running = true;

    std::thread receive_thread(do_receive, &output_uhd);

    std::thread correlator_thread(find_peak_correlation);

    do {
        double first_sample_time = 0;

        if (fd) {
            samps_read = read_samples(fd, input_samples, samps_per_buffer);
            sent = output_uhd.Transmit(&input_samples.front(), samps_read, &first_sample_time);
            aligner.push_tx_samples(&input_samples.front(), samps_read, first_sample_time);
        }
        else {
            zmq::message_t msg;
            if (not zmq_sock.recv(&msg)) {
                MDEBUG("zmq recv error\n");
                return -1;
            }

            if (msg.size() % sizeof(complexf) != 0) {
                MDEBUG("Received incomplete size %zu\n", msg.size());
                return -1;
            }
            samps_read = msg.size() / sizeof(complexf);

            sent = output_uhd.Transmit((complexf*)msg.data(), samps_read, &first_sample_time);

            aligner.push_tx_samples((complexf*)msg.data(), samps_read, first_sample_time);

        }

        if (first_sample_time - last_print_time > 1) {
            //MDEBUG("Tx %zu samples at t=%f\n", samps_read, first_sample_time);
            last_print_time = first_sample_time;
        }

        total_samps_read += samps_read;
    }
    while (samps_read and sent and running);
    MDEBUG("Leaving main loop with running=%d\n", running ? 1 : 0);

    running = false;

    receive_thread.join();
    correlator_thread.join();

}