#include <uhd/utils/thread_priority.hpp>
#include <uhd/utils/safe_main.hpp>
#include <uhd/usrp/multi_usrp.hpp>
#include <uhd/exception.hpp>
#include <uhd/types/tune_request.hpp>
#include <boost/program_options.hpp>
#include <boost/format.hpp>
#include <boost/thread.hpp>
#include <iostream>
#include <fstream>
#include <thread>
#include <csignal>
#include <future>

static bool stop_signal_called = false;

void sig_int_handler(int) {
    std::cout << "SIG INT HANDLED" << std::endl;
    stop_signal_called = true;
}

class USRPController {
public:
    uhd::usrp::multi_usrp::sptr device;

    USRPController(std::string serial, double rate, double freq, double gain, std::string file_name,
                   std::string time_source) :
            serial(serial), rate(rate), freq(freq), gain(gain) {
        std::ostringstream string_stream;
        string_stream << "[DEV " << serial << "] ";
        log_prepend = string_stream.str();
        std::stringstream ss;
        ss << log_prepend << "Creating.." << std::endl;
        std::cout << ss.str();

        this->device = uhd::usrp::multi_usrp::make("serial=" + serial);
        this->device->set_time_source(time_source); //applies to all mboards
        this->device->set_rx_subdev_spec(std::string("A:A"));
        uhd::time_spec_t cmd_time = device->get_time_now() + uhd::time_spec_t(1.0);
        this->device->set_command_time(cmd_time);

        uhd::tune_request_t tune_request(this->freq);
        this->device->set_rx_freq(tune_request);
        this->device->set_rx_gain(this->gain);
        this->device->set_rx_rate(this->rate);
        this->device->set_rx_bandwidth(this->rate);
        this->device->set_rx_antenna("RX2");

        //end timed commands
        this->device->clear_command_time();

        std::string cpu_format = "fc32";
        std::string wire_format = "sc16";
        uhd::stream_args_t stream_args(cpu_format, wire_format);
        this->rx_stream = this->device->get_rx_stream(stream_args);

        full_file_name = "/path/to/records/" + file_name;

        size_t samps_per_buff = 10000;// samples per buffer
        this->buff = std::vector<std::complex<float>>(samps_per_buff);
        outfile.open(full_file_name.c_str(), std::ofstream::binary);
    }

    void start(uhd::time_spec_t cmd_time) {
        //setup streaming
        uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
        stream_cmd.num_samps = size_t(0);
        stream_cmd.stream_now = false;
        stream_cmd.time_spec = uhd::time_spec_t(cmd_time);
        this->rx_stream->issue_stream_cmd(stream_cmd);
    }

    bool work() {
        size_t num_rx_samps = this->rx_stream->recv(&buff.front(), buff.size(), md, 3.0, true);// Try TRUE

        if (md.error_code == uhd::rx_metadata_t::ERROR_CODE_TIMEOUT) {
            std::cout << log_prepend << boost::format("Timeout while streaming") << std::endl;
        } else if (md.error_code == uhd::rx_metadata_t::ERROR_CODE_OVERFLOW) {
            if (overflow_message) {
                overflow_message = false;
                std::cerr << log_prepend << boost::format(
                        "Got an overflow indication. Please consider the following:\n"
                                "  Your write medium must sustain a rate of %fMB/s.\n"
                                "  Dropped samples will not be written to the file.\n"
                                "  Please modify this example for your purposes.\n"
                                "  This message will not appear again.\n"
                ) % (this->device->get_rx_rate() * sizeof(std::complex<float>) / 1e6);
            }
            return true;
        } else if (md.error_code == uhd::rx_metadata_t::ERROR_CODE_NONE) {
//            its o'k'?))
        } else {
            std::string error = str(boost::format("Uncaught error at %s %s") % (log_prepend, md.strerror()));
            throw std::runtime_error(error);
        }

        if (outfile.is_open()) {
            outfile.write((const char *) &buff.front(), num_rx_samps * sizeof(std::complex<float>));
        }

        return true;
    }

    uhd::rx_streamer::sptr rx_stream;

private:
    std::string serial;
    double rate;
    double freq;
    double gain;

    std::vector<std::complex<float>> buff;
    uhd::rx_metadata_t md;
    bool overflow_message = true;
    std::string full_file_name;
    std::string log_prepend;
    std::ofstream outfile;
};

void deviceThreadFunc(USRPController *controller) {
    while (not stop_signal_called) {
        if (not controller->work()) break;
    }
}

uhd::time_spec_t get_time_now() {
    return uhd::time_spec_t::get_system_time();
}

void ppsControl(USRPController *dev1, USRPController *dev2) {
    while (not stop_signal_called) {
        const uhd::time_spec_t dtime1 = dev1->device->get_time_now();
        const uhd::time_spec_t dtime2 = dev2->device->get_time_now();
        const uhd::time_spec_t ttt = get_time_now();
        dev1->device->set_time_now(ttt);
        dev2->device->set_time_now(ttt);
        std::stringstream ss;
        ss << "devs: " << dtime1.get_full_secs() << "." << dtime1.get_tick_count(1e6) << " " << dtime2.get_full_secs()
           << "." << dtime2.get_tick_count(1e6) << " is ";
        if (dtime1.get_real_secs() != dtime2.get_real_secs()) ss << "not same";
        else ss << "same!";
        ss << ", diff " << (dtime1.get_tick_count(1e6) - dtime2.get_tick_count(1e6)) << ", pc: "
           << ttt.get_tick_count(1e6);
        ss << std::endl;
        std::cout << ss.str();
        boost::this_thread::sleep(boost::posix_time::milliseconds(100));
    }
}

void create_device(std::promise<USRPController *> &&p, std::string serial, double rate, double freq,
                   double gain, std::string file_name, std::string time_source) {
    USRPController *device = new USRPController(serial, rate, freq, gain, file_name, time_source);
    p.set_value(device);
}

int UHD_SAFE_MAIN(int argc, char *argv[]) {
    uhd::set_thread_priority_safe();

    std::signal(SIGINT, &sig_int_handler);
    std::signal(SIGHUP, &sig_int_handler);

    double rate(5e6);
    double gain(32);

    std::promise<USRPController *> promise_1;
    std::promise<USRPController *> promise_2;
    auto f1 = promise_1.get_future();
    auto f2 = promise_2.get_future();
    std::thread dev_create_thread_1(&create_device, std::move(promise_1), "serial1", rate, 500e6, gain,
                                    "channels1", "internal");
    std::thread dev_create_thread_2(&create_device, std::move(promise_2), "serial2", rate, 500e6, gain,
                                    "channels2", "internal");
    dev_create_thread_1.join();
    dev_create_thread_2.join();
    USRPController *device_one = f1.get();
    USRPController *device_two = f2.get();

    boost::this_thread::sleep(boost::posix_time::seconds(1));

    std::thread ppmControlThread(ppsControl, device_one, device_two);

    boost::this_thread::sleep(boost::posix_time::seconds(5));

    uhd::time_spec_t start_timeout = uhd::time_spec_t(1.0);
    uhd::time_spec_t cmd_time_one = device_one->device->get_time_now() + start_timeout;
    uhd::time_spec_t cmd_time_two = device_two->device->get_time_now() + start_timeout;

    device_one->start(cmd_time_one);
    device_two->start(cmd_time_two);

    std::thread first(deviceThreadFunc, device_one);
    std::thread second(deviceThreadFunc, device_two);

    first.join();
    second.join();

    return EXIT_SUCCESS;
}