#include <iostream>
#include <cstdlib>
#include <cstring>
#include <thread>
#include <atomic>
#include <mutex>
#include <vector>
#include <complex>
#include <chrono>
#include <format>
#include <experimental/net>

#include <GLFW/glfw3.h>

#include <imgui.h>
#include <imgui_impl_glfw.h>
#include <imgui_impl_opengl3.h>

#include <libbladeRF.h>

namespace net = std::experimental::net;


void fft_mag(std::complex<float>* in, float* out, size_t len);


static const size_t block_len = 16384;
static const int wf_width = 1000;
static const int wf_height = 2000;


static std::atomic<bool> terminate_dsp;

static struct bladerf* bladerf_dev;

static std::mutex wf_data_mutex;
static unsigned int wf_data[wf_width*wf_height];

static std::atomic<float> freq_setpoint;
static std::atomic<float> freq_peak;
static std::atomic<float> tec_bias;
static std::atomic<float> tec_p;
static std::atomic<float> tec_current;

static std::atomic<int> fps;

static void dsp_thread()
{
    using namespace std::literals::chrono_literals;

    net::io_context io_context;
    net::ip::tcp::socket tec_socket(io_context);
    tec_socket.connect(net::ip::tcp::endpoint(net::ip::make_address("192.168.1.27"), 23));

    std::vector<std::complex<int16_t>> frames(block_len);
    std::vector<std::complex<float>> frames_f(block_len);
    std::vector<float> frames_mag(block_len);

    int iterations = 0;
    auto last_second = std::chrono::steady_clock::now();
    auto last_tec = std::chrono::steady_clock::now();
    while(!terminate_dsp) {
        int status;
        if((status = bladerf_sync_rx(bladerf_dev, frames.data(), block_len, NULL, 0)) != 0) {
            std::cerr << "failed to receive samples from bladeRF: " << bladerf_strerror(status) << std::endl;
            break;
        }

        fft_mag(frames_f.data(), frames_mag.data(), block_len);

        // stabilize laser
        bool tick = false;
        if((std::chrono::steady_clock::now() - last_tec) >= 100ms) {
            float freq_peak_local;
            freq_peak_local = 40.0f*float(distance(frames_mag.begin(), max_element(frames_mag.begin(), frames_mag.end())))/float(block_len);
            freq_peak = freq_peak_local;
            float freq_error = freq_peak_local - freq_setpoint;
            float tec_current_local = std::max(tec_bias+tec_p*freq_error, 0.0f);
            tec_current = tec_current_local;
            // FIXME: net::write seems unimplemented as of libstdc++ 13
            tec_socket.write_some(net::buffer(std::format("pwm 0 i_set {:.6f}\n", tec_current_local)));
            std::string reply;
            net::read(tec_socket, net::dynamic_buffer(reply),
                [&reply](auto ec, auto n) -> std::size_t
                {
                    if(ec || (reply.size() > 0 && reply.compare(reply.size()-1, 1, "\n") == 0))
                        return 0;
                    else
                        return 1;
                });
            last_tec += 100ms;
            tick = true;
        }

        // update waterfall
        {
            std::lock_guard<std::mutex> guard(wf_data_mutex);
            std::memmove(&wf_data[wf_width], &wf_data[0], sizeof(int)*wf_width*(wf_height - 1));
            for(int i=0;i<wf_width;i++) {
                int j = i*block_len/wf_width;
                wf_data[i] = 0xff000000 | 0x010101*std::min(int(frames_mag[j]/900.0f), 255);
            }
            wf_data[int(freq_setpoint*wf_width)/40] = 0xff0000ff;
            if(tick)
                for(int i=0;i<100;i++)
                    wf_data[i] = 0xffff0000;
        }

        // FPS counter
        iterations++;
        if((std::chrono::steady_clock::now() - last_second) >= 1s) {
            fps = iterations;
            iterations = 0;
            last_second += 1s;
        }
    }
}

int main(int argc, char* argv[])
{
    if(!glfwInit()) {
        std::cerr << "failed to initialize GLFW" << std::endl;
        return 1;
    }
    std::atexit(glfwTerminate);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
    static GLFWwindow* window = glfwCreateWindow(1500, 2200, "fastsa", nullptr, nullptr);
    if(window == nullptr) {
        std::cerr << "failed to create GLFW window" << std::endl;
        return 1;
    }
    static auto DestroyWindow = []() {
        glfwDestroyWindow(window);
    };
    std::atexit(DestroyWindow);
    glfwMakeContextCurrent(window);
    glfwSwapInterval(1);

    IMGUI_CHECKVERSION();
    ImGui::CreateContext();
    static auto ImGuiDestroyContext = []() {
        ImGui::DestroyContext();
    };
    std::atexit(ImGuiDestroyContext);
    ImGuiIO& io = ImGui::GetIO();
    io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard;
    ImGui::StyleColorsDark();
    ImGui_ImplGlfw_InitForOpenGL(window, true);
    std::atexit(ImGui_ImplGlfw_Shutdown);
    ImGui_ImplOpenGL3_Init("#version 130");
    std::atexit(ImGui_ImplOpenGL3_Shutdown);

    GLuint wftex[2];
    for(size_t i=0;i<2;i++) {
        glGenTextures(1, &wftex[i]);
        glBindTexture(GL_TEXTURE_2D, wftex[i]);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
    }

    int status;
    struct bladerf_devinfo dev_info;
    bladerf_init_devinfo(&dev_info);
    if((status = bladerf_open_with_devinfo(&bladerf_dev, &dev_info)) != 0) {
        std::cerr << "cannot open bladeRF device: " << bladerf_strerror(status) << std::endl;
        return 1;
    }
    static auto BladeRFClose = []() {
        bladerf_close(bladerf_dev);
    };
    std::atexit(BladeRFClose);
    if((status = bladerf_set_frequency(bladerf_dev, BLADERF_CHANNEL_RX(0), 1'655'000'000)) != 0) {
        std::cerr << "failed to set bladeRF frequency: " << bladerf_strerror(status) << std::endl;
        return 1;
    }
    if((status = bladerf_set_sample_rate(bladerf_dev, BLADERF_CHANNEL_RX(0), 40'000'000, NULL)) != 0) {
        std::cerr << "failed to set bladeRF sample rate: " << bladerf_strerror(status) << std::endl;
        return 1;
    }
    if((status = bladerf_set_bandwidth(bladerf_dev, BLADERF_CHANNEL_RX(0), 35'000'000, NULL)) != 0) {
        std::cerr << "failed to set bladeRF bandwidth: " << bladerf_strerror(status) << std::endl;
        return 1;
    }
    if((status = bladerf_set_gain(bladerf_dev, BLADERF_CHANNEL_RX(0), 20)) != 0) {
        std::cerr << "failed to set bladeRF gain: " << bladerf_strerror(status) << std::endl;
        return 1;
    }
    if((status = bladerf_sync_config(bladerf_dev, BLADERF_RX_X1, BLADERF_FORMAT_SC16_Q11,
            16,   /* num_buffers */
            8192, /* buffer_size */
            8,    /* num_transfers */
            3500  /* timeout_ms */)) != 0) {
        std::cerr << "failed to set bladeRF sync settings: " << bladerf_strerror(status) << std::endl;
        return 1;
    }
    if((status = bladerf_enable_module(bladerf_dev, BLADERF_RX, true) != 0)) {
        std::cerr << "failed to enable bladeRF RX: " << bladerf_strerror(status) << std::endl;
        return 1;
    }
    static auto BladeRFDisableRX = []() {
        bladerf_enable_module(bladerf_dev, BLADERF_RX, false);
    };
    std::atexit(BladeRFDisableRX);

    terminate_dsp = false;
    static std::thread dsp_thread_h = std::thread(dsp_thread);
    static auto TerminateDSP = []() {
        terminate_dsp = true;
        dsp_thread_h.join();
    };
    std::atexit(TerminateDSP);

    bool exit = false;
    bool update_wf = true;
    float freq_setpoint_local = 0.0f;
    float tec_bias_local = 0.02f;
    float tec_p_local = 0.001f;
    while(!exit && !glfwWindowShouldClose(window)) {
        glfwPollEvents();
        glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT);

        if(update_wf) {
            #if defined(GL_UNPACK_ROW_LENGTH) && !defined(__EMSCRIPTEN__)
            glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
            #endif
            glBindTexture(GL_TEXTURE_2D, wftex[0]);
            {
                std::lock_guard<std::mutex> guard(wf_data_mutex);
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_data);
            }
            glBindTexture(GL_TEXTURE_2D, wftex[1]);
            {
                std::lock_guard<std::mutex> guard(wf_data_mutex);
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_data);
            }
        }

        ImGui_ImplOpenGL3_NewFrame();
        ImGui_ImplGlfw_NewFrame();
        ImGui::NewFrame();

        ImGui::SetNextWindowPos(ImVec2(0.0f, 0.0f));
        ImGui::SetNextWindowSize(io.DisplaySize);
        ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f);
        ImGui::Begin("fastsa", nullptr, ImGuiWindowFlags_NoDecoration | ImGuiWindowFlags_NoResize);

        ImGui::BeginTable("fastsa", 3, ImGuiTableFlags_SizingStretchSame | ImGuiTableFlags_Resizable);
        ImGui::TableSetupColumn("SBS laser control", 0, 280.0f);
        ImGui::TableSetupColumn("Pump/output heterodyne", 0, 1000.0f);
        ImGui::TableSetupColumn("Linewidth", 0, 1000.0f);
        ImGui::TableHeadersRow();
        ImGui::TableNextColumn();
        ImGui::Checkbox("Update waterfall plots", &update_wf);
        ImGui::Text("Baseband peak: %.3f MHz", (float)freq_peak);
        ImGui::Text("TEC current: %.6f mA", (float)tec_current);
        ImGui::SliderFloat("Setpoint", &freq_setpoint_local, 0.0f, 40.0f);
        ImGui::SliderFloat("TEC bias", &tec_bias_local, 0.0f, 0.5f);
        ImGui::SliderFloat("TEC P", &tec_p_local, -1.0f, 1.0f);
        freq_setpoint = freq_setpoint_local;
        tec_bias = tec_bias_local;
        tec_p = 0.05f*tec_p_local;
        if(ImGui::Button("Exit"))
            exit = true;
        ImGui::Text("FPS: %d", (int)fps);
        ImGui::TableNextColumn();
        ImGui::Image((void*)(intptr_t)wftex[0], ImVec2(wf_width, wf_height));
        ImGui::TableNextColumn();
        ImGui::Image((void*)(intptr_t)wftex[1], ImVec2(wf_width, wf_height));
        ImGui::EndTable();

        ImGui::End();
        ImGui::PopStyleVar(1);

        ImGui::Render();
        ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());
        int display_w, display_h;
        glfwGetFramebufferSize(window, &display_w, &display_h);
        glViewport(0, 0, display_w, display_h);
        glfwSwapBuffers(window);
    }

    return 0;
}