#include <cstring>
#include <glad/glad.h>
#include <GLFW/glfw3.h>
#include <glm/ext/matrix_clip_space.hpp>
#include <glm/ext/matrix_transform.hpp>
#include <glm/ext/vector_float3.hpp>
#include <glm/matrix.hpp>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <string>
#include <tuple>

#include "util.hpp"
#include "shaders.hpp"
#include "body.hpp"
#include "camera_poses.hpp"

static GLFWwindow* window;
static float width, height;

void framebuffer_size_callback(GLFWwindow* window, int w, int h) {
    width = w;
    height = h;
    glViewport(0, 0, width, height);
}

void process_input() {
    if (glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS)
        glfwSetWindowShouldClose(window, true);
}

static bool stop = false;
static pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
const float max_hp
    = 10; // Number of scans (without a particular barcode) for which the sphere will still be visible
static std::vector<std::tuple<char*, Body*, float>>
    camera_bodies; // I would use my array here, but was getting a linking error

void* process_cin(void* args) {
    std::string line;
    while (true) {
        std::getline(std::cin, line);
        Array<char*> words = split_str(line.c_str());
        assert(words.len == 4);
        printf("Received: %s, %s, %s, %s\n", words[0], words[1], words[2], words[3]); // echo for debugging
        float x = atof(words[1]);
        float y = atof(words[2]);
        float z = atof(words[3]);
        glm::vec3 new_loc = glm::vec3(x, y, z);
        bool found_match = false;
        pthread_mutex_lock(&lock);
        for (int i = 0; i < camera_bodies.size(); i++) {
            if (strcmp(words[0], std::get<0>(camera_bodies[i])) == 0 && std::get<1>(camera_bodies[i])) {
                glm::vec4& transl = std::get<1>(camera_bodies[i])->pose[3];
                transl = 0.9f * transl + 0.1f * glm::vec4(new_loc, 1); // filter

                int color_i = i + 1;
                std::get<1>(camera_bodies[i])->color
                    = glm::vec4(color_i & 0x4, color_i & 0x2, color_i & 0x1, 1);
                std::get<2>(camera_bodies[i]) = max_hp;
                found_match = true;
            } else if (std::get<1>(camera_bodies[i])) {
                float& cur_hp = std::get<2>(camera_bodies[i]);
                if (cur_hp > 0)
                    cur_hp -= 1;
                std::get<1>(camera_bodies[i])->color = glm::vec4(i & 0x4, i & 0x2, i & 0x1, cur_hp / max_hp);
            }
        }
        if (!found_match) {
            auto t = std::make_tuple(words[0], (Body*)NULL, max_hp);
            camera_bodies.push_back(t);
        }
        pthread_mutex_unlock(&lock);
    }
}

static bool mouse_pressed = false;
static bool scroll_pressed = false;
static double prev_cursor_x, prev_cursor_y;

static double theta = 0.0; // angle of camera trans vect wrt x-z plane
static double phi = glm::radians(270.0); // angle of camera trans vect wrt x-axis

static glm::vec3 focal_point = glm::vec3(0, 0, 0);
static glm::vec3 camera_loc = glm::vec3(0, 0, -1);
static glm::vec3 up = glm::vec3(0, 1, 0);
static glm::mat4 world_to_camera = glm::lookAt(camera_loc, focal_point, up);

static void cursor_position_callback(GLFWwindow* window, double xpos, double ypos) {
    float dx = (xpos - prev_cursor_x);
    float dy = (ypos - prev_cursor_y);

    glm::mat4 camera_to_world = glm::inverse(world_to_camera);
    // These could be acquired via some cross products. Don't know if that's more efficient.
    glm::vec4 strafe_x = camera_to_world[0];
    glm::vec4 strafe_y = camera_to_world[1];
    prev_cursor_x = xpos;
    prev_cursor_y = ypos;

    if (mouse_pressed) {
        phi += glm::radians(dx * (360 / width)); // * glm::radians(360.0);
        theta += glm::radians(dy * (360 / height)); // * glm::radians(360.0);
        double len = glm::length(camera_loc - focal_point);
        camera_loc.x = focal_point.x + (len * glm::cos(theta) * glm::cos(-phi));
        camera_loc.y = focal_point.y + (len * glm::sin(theta));
        camera_loc.z = focal_point.z + (-len * glm::cos(theta) * glm::sin(-phi));
    }

    if (scroll_pressed) {
        glm::vec4 move_vec = -strafe_x * (dx / 100) + strafe_y * (dy / 100);
        focal_point += move_vec;
        camera_loc += move_vec;
    }

    world_to_camera = glm::lookAt(camera_loc, focal_point, up);
}

void mouse_button_callback(GLFWwindow* window, int button, int action, int mods) {
    if (button == GLFW_MOUSE_BUTTON_RIGHT) {
        mouse_pressed = action == GLFW_PRESS;
    }

    if (button == GLFW_MOUSE_BUTTON_MIDDLE) {
        scroll_pressed = action == GLFW_PRESS;
    }

    glfwGetCursorPos(window, &prev_cursor_x, &prev_cursor_y);
}

void scroll_callback(GLFWwindow* window, double xoffset, double yoffset) {
    glm::vec3 k = camera_loc - focal_point;
    camera_loc -= k * (float)yoffset / 10.0f;
    world_to_camera = glm::lookAt(camera_loc, focal_point, up);
}

bool glfw_setup() {
    glfwInit();
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
    // glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
    window = glfwCreateWindow(800, 800, "LearnOpenGL", NULL, NULL);
    if (window == NULL) {
        printf("Failed to create GLFW window\n");
        glfwTerminate();
        return false;
    }
    glfwMakeContextCurrent(window);
    glfwSetFramebufferSizeCallback(window, framebuffer_size_callback);
    glfwSetCursorPosCallback(window, cursor_position_callback);
    glfwSetMouseButtonCallback(window, mouse_button_callback);
    glfwSetScrollCallback(window, scroll_callback);

    cursor_position_callback(window, 0, 0);
    return true;
}

int main() {
    if (!glfw_setup())
        return -1;

    if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress)) {
        printf("Failed to initialize GLAD\n");
        return -1;
    }
    glViewport(0, 0, 800, 800);
    width = 800;
    height = 800;

    const char* vertex_filepath = "src/shaders/vertex.glsl";
    const char* fragment_filepath = "src/shaders/fragment.glsl";
    if (!load_shader(&shader, vertex_filepath, fragment_filepath))
        return -1;

    // glClearColor(0.2f, 0.3f, 0.3f, 1.0f);
    glClearColor(0, 0, 0, 0);
    glClear(GL_COLOR_BUFFER_BIT); // Write to back buffer

    glfwSwapBuffers(window); // front buffer is now back
    glClear(GL_COLOR_BUFFER_BIT); // Write to back buffer again (former front buf)

    Body b2;
    create_new_sphere(&b2);
    b2.pose = glm::translate(b2.pose, glm::vec3(0, 0, 20));
    b2.color = glm::vec4(1, 0, 0, 1);

    // glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    // set_uniform(shader, "color", glm::vec4 { sin(time), sin(time + glm::radians(45.0f)), sin(time +
    // glm::radians(90.0f)), 1.0 } / 2.0f); time = glfwGetTime();

    glDisable(GL_CULL_FACE);
    glEnable(GL_DEPTH_TEST);

    glm::mat4 projection_t
        = glm::perspective(glm::radians(45.0f), (float)width / (float)height, 0.1f, 2000.0f);

    pthread_t thread_id;
    pthread_create(&thread_id, NULL, process_cin, NULL);

    Array<Body> camera_pose_axes = { NULL, 0 };
    if (!parse_poses(&camera_pose_axes, "poses.csv")) {
        return -1;
    }

    while (!glfwWindowShouldClose(window)) {
        process_input();
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        set_uniform(shader, "camera_t", world_to_camera);
        set_uniform(shader, "projection_t", projection_t);

        draw_body(b2);

        for (int i = 0; i < camera_pose_axes.len; i++) {
            draw_body(camera_pose_axes[i]);
        }

        if (pthread_mutex_trylock(&lock) == 0) {
            for (int i = 0; i < camera_bodies.size(); i++) {
                if (!std::get<1>(camera_bodies[i])) {
                    Body* b = (Body*)malloc(sizeof(Body));
                    create_new_sphere(b);
                    b->color = glm::vec4((i+1) & 0x4, (i+1) & 0x2, (i+1) & 0x1, max_hp);
                    std::get<1>(camera_bodies[i]) = b;
                }
                draw_body(*std::get<1>(camera_bodies[i]));
            }
            pthread_mutex_unlock(&lock);
        }

        glfwSwapBuffers(window);
        glfwPollEvents();
    }
    glfwTerminate();
    return 0;
}