#include <windows.h>
#include <iostream>
#include <string>
#include <deque>
#include <vector>
#include <thread>
#include <mutex>
#include <atomic>

// ImGui and OpenGL headers
#include "imgui/imgui.h"
#include "imgui/imgui_impl_glfw.h"
#include "imgui/imgui_impl_opengl3.h"
#include <glad/glad.h>
#include <GLFW/glfw3.h>

// Your serial communication header
#include "serialcomm.hpp"

// Global variables
std::mutex quatMutex;
Quaternion currentQuat = {1.0f, 0.0f, 0.0f, 0.0f};
std::deque<float> quaternionHistory[4]; // w, x, y, z history for plotting
const int MAX_HISTORY = 100;
std::atomic<bool> shouldExit(false);

// Function to read data from serial port in a separate thread
void serialReaderThread(const char* portName, DWORD baudRate) {
    char buffer[64];
    DWORD bytesRead;
    std::string incompleteDataBuffer;

    HANDLE hSerial = initSerialPort(portName, baudRate);
    if (hSerial == INVALID_HANDLE_VALUE) {
        std::cerr << "Failed to initialize serial port" << std::endl;
        return;
    }

    while (!shouldExit) {
        if (readSerialData(hSerial, buffer, sizeof(buffer), bytesRead)) {
            if (bytesRead > 0) {
                // Add newly arrived data to buffer
                buffer[bytesRead] = '\0';
                incompleteDataBuffer += buffer;

                // Process any complete lines in the buffer
                size_t pos;
                while ((pos = incompleteDataBuffer.find('\n')) != std::string::npos) {
                    // Create substring of completed line and erase it from incompleteDataBuffer
                    std::string completeLine = incompleteDataBuffer.substr(0, pos);
                    incompleteDataBuffer.erase(0, pos + 1);

                    // Parse the quaternions from line
                    Quaternion quat;
                    if (parseQuaternion(completeLine, quat)) {
                        // Update the current quaternion with mutex protection
                        std::lock_guard<std::mutex> lock(quatMutex);
                        currentQuat = quat;
                    }
                }
            }
        }
        // Small sleep to prevent high CPU usage
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }

    CloseHandle(hSerial);
}

// Function to update history arrays for plotting
void updateQuaternionHistory(const Quaternion& quat) {
    // Update quaternion history for plotting
    quaternionHistory[0].push_back(quat.w);
    quaternionHistory[1].push_back(quat.x);
    quaternionHistory[2].push_back(quat.y);
    quaternionHistory[3].push_back(quat.z);

    // Limit history size
    for (int i = 0; i < 4; i++) {
        if (quaternionHistory[i].size() > MAX_HISTORY) {
            quaternionHistory[i].pop_front();
        }
    }
}

// Function to initialize GLFW window
GLFWwindow* initWindow() {
    // Initialize GLFW
    if (!glfwInit()) {
        std::cerr << "Failed to initialize GLFW" << std::endl;
        return nullptr;
    }

    // GL 3.3 Core Profile
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

    // Create a windowed mode window and its OpenGL context
    GLFWwindow* window = glfwCreateWindow(800, 600, "Quaternion Visualizer", NULL, NULL);
    if (!window) {
        std::cerr << "Failed to create GLFW window" << std::endl;
        glfwTerminate();
        return nullptr;
    }

    // Make the window's context current
    glfwMakeContextCurrent(window);
    glfwSwapInterval(1); // Enable vsync

    // Initialize GLAD
    if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress)) {
        std::cerr << "Failed to initialize GLAD" << std::endl;
        return nullptr;
    }

    return window;
}

// Main function
int main() {
    // Configuration for serial port
    const char* portName = "COM3";
    DWORD baudRate = CBR_115200;

    // Initialize GLFW window
    GLFWwindow* window = initWindow();
    if (!window) {
        return -1;
    }

    // Setup ImGui context
    IMGUI_CHECKVERSION();
    ImGui::CreateContext();
    ImGuiIO& io = ImGui::GetIO(); (void)io;
    io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard;  // Enable Keyboard Controls

    // Setup Dear ImGui style
    ImGui::StyleColorsDark();

    // Setup Platform/Renderer bindings
    ImGui_ImplGlfw_InitForOpenGL(window, true);
    ImGui_ImplOpenGL3_Init("#version 330 core");

    // Start serial reader thread
    std::thread serialThread(serialReaderThread, portName, baudRate);

    // Main loop
    while (!glfwWindowShouldClose(window)) {
        // Poll and handle events
        glfwPollEvents();

        // Start the ImGui frame
        ImGui_ImplOpenGL3_NewFrame();
        ImGui_ImplGlfw_NewFrame();
        ImGui::NewFrame();

        // Get the latest quaternion data (thread-safe)
        Quaternion localQuat;
        {
            std::lock_guard<std::mutex> lock(quatMutex);
            localQuat = currentQuat;
            updateQuaternionHistory(localQuat);
        }

        // Create ImGui window
        ImGui::Begin("Quaternion Data");

        // Display current quaternion values
        ImGui::Text("Current Quaternion:");
        ImGui::Text("w: %.4f", localQuat.w);
        ImGui::Text("x: %.4f", localQuat.x);
        ImGui::Text("y: %.4f", localQuat.y);
        ImGui::Text("z: %.4f", localQuat.z);

        // Verify that quaternion is normalized
        float magnitude = sqrt(localQuat.w * localQuat.w + 
                              localQuat.x * localQuat.x + 
                              localQuat.y * localQuat.y + 
                              localQuat.z * localQuat.z);
        ImGui::Text("Magnitude: %.4f", magnitude);

        // Plot quaternion components over time
        if (!quaternionHistory[0].empty()) {
            static float plotMin = -1.0f;
            static float plotMax = 1.0f;
            // Convert deque to vector for plotting (since deque doesn't have .data() in your version)
            std::vector<float> wValues(quaternionHistory[0].begin(), quaternionHistory[0].end());
            std::vector<float> xValues(quaternionHistory[1].begin(), quaternionHistory[1].end());
            std::vector<float> yValues(quaternionHistory[2].begin(), quaternionHistory[2].end());
            std::vector<float> zValues(quaternionHistory[3].begin(), quaternionHistory[3].end());
            
            ImGui::PlotLines("w", wValues.data(), static_cast<int>(wValues.size()), 
                            0, NULL, plotMin, plotMax, ImVec2(0, 80));
            ImGui::PlotLines("x", xValues.data(), static_cast<int>(xValues.size()), 
                            0, NULL, plotMin, plotMax, ImVec2(0, 80));
            ImGui::PlotLines("y", yValues.data(), static_cast<int>(yValues.size()), 
                            0, NULL, plotMin, plotMax, ImVec2(0, 80));
            ImGui::PlotLines("z", zValues.data(), static_cast<int>(zValues.size()), 
                            0, NULL, plotMin, plotMax, ImVec2(0, 80));
        }

        ImGui::End();

        // Rendering
        ImGui::Render();
        int display_w, display_h;
        glfwGetFramebufferSize(window, &display_w, &display_h);
        glViewport(0, 0, display_w, display_h);
        glClearColor(0.1f, 0.1f, 0.1f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT);
        ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());

        // Swap front and back buffers
        glfwSwapBuffers(window);
    }

    // Cleanup
    shouldExit = true;
    if (serialThread.joinable()) {
        serialThread.join();
    }

    ImGui_ImplOpenGL3_Shutdown();
    ImGui_ImplGlfw_Shutdown();
    ImGui::DestroyContext();

    glfwDestroyWindow(window);
    glfwTerminate();

    return 0;
}