#include <cstdint>
#include <ios>
#include <iostream>
#include "raylib.h"
#include <raymath.h>
#include <uchar.h>
#include "serialcomm.hpp"
#include <vector>
#include <iomanip>
#include <thread>
#include <mutex>

//#define RAYGUI_IMPLEMENTATION
//#include "ressources/raygui/raygui.h"

#include "imgui.h"
#include "ressources/rlImGui-Raylib_6_0/rlImGui.h"
#include "rlImGui.h"
//#include "glm/glm.hpp"

// global variables for the communication between parsing and main thread
std::mutex quaternion_mutex;
Quaternion newest_quaternion = {0.0f};
std::vector<Quaternion> quaternion_history;
bool read_new_quaternion_data = true;


const int WIDTH = 1280;
const int HEIGTH = 720;


// seperate thread for the serial parsing of the incoming binary data from the mcu
void serial_parser_thread() {
    // define data variables and open serial port
    int serPort = linux_initSerialPort();
    size_t bytes_read;
    std::vector<uint8_t> binary_data_buffer(64);
    std::vector<uint8_t> binary_Packet_Buffer;

    while (read_new_quaternion_data) {
        if (linux_readSerialData(serPort, binary_data_buffer, bytes_read)) {
            binary_Packet_Buffer.insert(binary_Packet_Buffer.end(), binary_data_buffer.begin(), binary_data_buffer.begin() + bytes_read); //binary_data_buffer.end());

            while(true) {
                std::vector<uint8_t>packet = extractPacket(binary_Packet_Buffer);

                if (packet.empty()) break;

                Quaternion quat;
                if (parseBinaryPacket(packet,quat)) {

                    // lock the mutex to update quaternion and add to quaternion_history
                    std::lock_guard<std::mutex> lock(quaternion_mutex);
                    newest_quaternion = quat;
                    quaternion_history.push_back(quat);
                    if (quaternion_history.size() > 200) {
                        quaternion_history.erase((quaternion_history.begin()));
                        }

                    // now print the quaternions formatted into the console for debugging
                    std::cout << std::fixed << std::setprecision(6) <<
                    "w: " << std::setw(10) << quat.w <<
                    " x: " << std::setw(10) << quat.x <<
                    " y: " << std::setw(10) << quat.y <<
                    " z: " << std::setw(10) << quat.z << std::endl;
                }

            }
        } else break;
    }

    if (linux_close_serial_port(serPort)) {
        std::cout << "Serial port " << serPort << " succesfully closed" << std::endl;
    } else {
        std::cout << "[ERROR] closing Serial port " << serPort << std::endl;
    }
    std::cout << "Serial communication thread exiting" << std::endl;

}


int main() {

    // start the parsing thread
    std::thread *serial_parsing_thread = nullptr;
    serial_parsing_thread = new std::thread(serial_parser_thread);
    Quaternion cube_movement_quaternion;
    std::vector<Quaternion> quat_history;

    // setup dear imgui for the UI
    rlImGuiSetup(true);

    // Create the window
    InitWindow(WIDTH, HEIGTH, "Trinity Control");
    SetTargetFPS(60);

    // define 3D camecra
    Camera camera = {0};
    camera.position = (Vector3){3.0f, 10.0f, 10.0f};
    camera.target = (Vector3){0.0f, 0.0f, 0.0f};
    camera.up = (Vector3){0.0f, 1.0f, 0.0f};
    camera.fovy = 45.0f;
    camera.projection = CAMERA_PERSPECTIVE;

    Vector3 CubePosition = {0.0f, 0.0f, 0.0f};
    Vector3 CubeDimensions = {4.0f, 4.0f, 4.0f};

    // load image for the cube texture
    Model model = LoadModelFromMesh(GenMeshCube(CubeDimensions.x, CubeDimensions.y, CubeDimensions.z));
    if (!IsModelValid(model)){
        std::cout << "Loaded model is not valid" << std::endl;
    }
    Mesh mesh = model.meshes[0];

    Image img = LoadImage("kekusmaximus.png");
    Texture2D texture = LoadTextureFromImage(img);
    UnloadImage(img);
    model.materials[0].maps[MATERIAL_MAP_DIFFUSE].texture = texture;

    while (!WindowShouldClose()) {

        // get the newest quaternion and quaternion_history from parsing thread
        Quaternion cube_movement_quaternion;
        {
            std::lock_guard<std::mutex> lock(quaternion_mutex);
            cube_movement_quaternion = newest_quaternion;
            quat_history = quaternion_history;

        }

        BeginDrawing();
        ClearBackground(RAYWHITE);
        DrawText("BIG TITTY GOTH GIRLS", 190, 200, 20, LIGHTGRAY);

        BeginMode3D(camera);

        Matrix rot_matrix = QuaternionToMatrix(cube_movement_quaternion);
        model.transform = MatrixMultiply(rot_matrix, MatrixTranslate(CubePosition.x, CubePosition.y, CubePosition.z));
        DrawMesh(mesh, model.materials[0], model.transform);

        DrawModel(model,CubePosition, 1.0f , WHITE);

        EndMode3D();

        rlImGuiBegin();
        ImGui::Text("Im a good little slut, daddy");
        ImGui::Text("w: %.4f x: %.4f y: %.4f z: %.4f", cube_movement_quaternion.w, cube_movement_quaternion.x, cube_movement_quaternion.y, cube_movement_quaternion.z);
        ImGui::Text("Connection to MCU: ");

        ImGui::BeginDisabled(serial_parsing_thread != nullptr);
        if (ImGui::Button("Connect")) {
            read_new_quaternion_data = true;
            serial_parsing_thread = new std::thread(serial_parser_thread);
        } ImGui::EndDisabled();

        ImGui::SameLine();

        ImGui::BeginDisabled(serial_parsing_thread == nullptr);
        if (ImGui::Button("Disconnect")) {
            read_new_quaternion_data = false;
            serial_parsing_thread -> join();
            delete serial_parsing_thread;
            serial_parsing_thread = nullptr;
        } ImGui::EndDisabled();


        // create scrolling section for last 200 measurements
        ImGui::BeginChild("History", ImVec2(0, 300), true, ImGuiWindowFlags_HorizontalScrollbar);
        // loop through quadHistory vector and display each element as line
        for (uint32_t i = 0; i < quat_history.size(); i++) {
            const Quaternion& q = quat_history[i];
            ImGui::Text("w: %.4f x: %.4f y: %.4f z: %.4f", q.w, q.x, q.y, q.z);
        }
        // enable autoscroll to newest entry
        if (ImGui::GetScrollY() >= ImGui::GetScrollMaxY()) {
            ImGui::SetScrollHereY(1.0f);
        }
        ImGui::EndChild();

        rlImGuiEnd();
        EndDrawing();
    }

    //close parser thread and unload texture/model
    serial_parsing_thread -> join();
    delete serial_parsing_thread;
    UnloadTexture(texture);
    UnloadModel(model);
    CloseWindow();

    return 0;
}