#include <regex>
#include <iostream>
#include "render.h"
#include "glm/vec3.hpp"
#include "glm/vec4.hpp"
#include "glm/gtc/type_ptr.hpp"
//#include <GL/glext.h>

static const char *vs_code = R"(
    #version 330

    layout (location = 0) in vec3 position;
    layout (location = 1) in vec4 in_color;

    out vec4 interp_color;

    out vec3 vertPos;
    flat out vec3 startPos;
    uniform mat4 mvp;


    void main() {
        vec4 pos = mvp * vec4(position, 1.0);
        gl_Position = pos;

        vertPos      = pos.xyz / pos.w;
        startPos     = vertPos;
        interp_color = in_color;
    }
)";


static const char *ps_code = R"(
    #version 330

    flat in vec3 startPos;
    in vec3 vertPos;

    in vec4 interp_color;
    out vec4 color;

    uniform vec2  u_resolution;
    float u_dashSize = 10; // make this uniform
    float u_gapSize = 10; // make this uniform

    void main() {
        vec2  dir  = (vertPos.xy - startPos.xy) * u_resolution/2.0;
        float dist = length(dir);

        if (fract(dist / (u_dashSize + u_gapSize)) > u_dashSize/(u_dashSize + u_gapSize))
            discard;

        color = interp_color;
    }
)";


void grbl::program_renderer::render(glm::mat4 mvp, glm::vec2 viewport_size) {
    shader->bind();
    shader->set_mat4(glm::value_ptr(mvp), "mvp");
    shader->set_vec2(glm::value_ptr(viewport_size), "u_resolution");

    glBindVertexArray(vao_id);
    glDrawArrays(GL_LINES, 0, vertices_count);


    auto spindle_xform = glm::translate(glm::mat4(1.0f), spindle_pos);
    auto spindle_view = mvp * spindle_xform;
    shader->set_mat4(glm::value_ptr(spindle_view), "mvp");

    glBindVertexArray(spindle_vao_id);
    glDrawArrays(GL_LINES, 0, spindle_vertices_count);

    shader->unbind();

    // draw bit location
}

void grbl::program_renderer::update(const grbl::program& pgm, const grbl::machine& cnc) {
    if (!initialized) {

        shader = new shader_program(vs_code, ps_code);

        initialize_spindle_buffers();
        initialize_program_buffers();

        initialized = true;
    }

    // update program with machine status
    // build vbo and vao
    vertices_count = build_vbo(pgm);

    auto work_pos = cnc.get_status().machine_pos;
    auto offsets = cnc.get_current_work_offset_values();
    for (int i = 0; i < 3; i++) {
        work_pos[i] -= offsets[i];
    }

    spindle_pos = glm::vec3(work_pos[0], work_pos[1], work_pos[2]);
}

void grbl::program_renderer::initialize_spindle_buffers() {
    glGenBuffers(1, &spindle_vbo_id);
    glGenVertexArrays(1, &spindle_vao_id);

    // vertex format: x, y, z, r, g, b, a
    // stride: 28 bytes
    const GLsizei sizeOfVertexInBytes = 28;

    glBindVertexArray(spindle_vao_id);
    glBindBuffer(GL_ARRAY_BUFFER, spindle_vbo_id);

    glEnableVertexAttribArray(0); // vertices on stream 0
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeOfVertexInBytes, (void *) 0);

    glEnableVertexAttribArray(1); // vertex colors on stream 1
    glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, sizeOfVertexInBytes, (void *) (sizeof(float) * 3));

    // unbind vao
    glBindVertexArray(0);

    // construct spindle model
    glm::vec4 col(1, 1, 0, 1); // yellow

    std::vector<float> buffer_data;

    const float spindle_length = 3.5;
    const size_t cone_granularity = 20;
    for (int i = 0; i < cone_granularity; i++) {
        float x = sinf((i / (float) cone_granularity) * 2.0f * M_PI);
        float y = cosf((i / (float) cone_granularity) * 2.0f * M_PI);

        // from
        buffer_data.push_back(0);
        buffer_data.push_back(0);
        buffer_data.push_back(0);

        buffer_data.push_back(col.r);
        buffer_data.push_back(col.g);
        buffer_data.push_back(col.b);
        buffer_data.push_back(col.a);

        // to
        buffer_data.push_back(x);
        buffer_data.push_back(y);
        buffer_data.push_back(spindle_length);

        buffer_data.push_back(col.r);
        buffer_data.push_back(col.g);
        buffer_data.push_back(col.b);
        buffer_data.push_back(col.a);
    }

    glBindBuffer(GL_ARRAY_BUFFER, spindle_vbo_id);
    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);

    spindle_vertices_count = buffer_data.size() * sizeof(float) / sizeOfVertexInBytes;
}

void grbl::program_renderer::initialize_program_buffers() {
    glGenBuffers(1, &vbo_id);
    glGenVertexArrays(1, &vao_id);

    // vertex format: x, y, z, r, g, b, a
// stride: 28 bytes
    const GLsizei sizeOfVertexInBytes = 28;

    glBindVertexArray(vao_id);
    glBindBuffer(GL_ARRAY_BUFFER, vbo_id);

    glEnableVertexAttribArray(0); // vertices on stream 0
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeOfVertexInBytes, (void *) 0);

    glEnableVertexAttribArray(1); // vertex colors on stream 1
    glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, sizeOfVertexInBytes, (void *) (sizeof(float) * 3));

    // unbind vao
    glBindVertexArray(0);
}

GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
    static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))");
    bool is_tool_on = false;


    buffer_data.clear();

    glm::vec3 tool_pos;
    min_pos = max_pos = tool_pos = glm::vec3(0);

    for (auto& i: pgm.instructions) {
        if (i.type == grbl::instruction_type::gcode) {

            std::vector<std::string> tokens;
            std::smatch res;
            std::string::const_iterator start(i.command.cbegin());
            while (std::regex_search(start, i.command.cend(), res, movement_re)) {
                auto str = res[0].str();

                // make upper case
                std::transform(str.begin(), str.end(), str.begin(), ::toupper);

                // remove whitespace from things like "X 123.1234"
                str.erase(remove_if(str.begin(), str.end(), isspace), str.end());

                tokens.push_back(str);
                start = res.suffix().first;
            }

            if (!tokens.empty()) {
                auto new_pos = tool_pos;

                for (auto& t: tokens) {
                    if (t[0] == 'X') {
                        new_pos.x = std::stof(t.substr(1));
                    } else if (t[0] == 'Y') {
                        new_pos.y = std::stof(t.substr(1));
                    } else if (t[0] == 'Z') {
                        new_pos.z = std::stof(t.substr(1));
                    }
                }

                bool has_g1 = tokens[0][0] == 'G' && std::stoi(tokens[0].substr(1)) == 1;
                bool has_g0 = tokens[0][0] == 'G' && std::stoi(tokens[0].substr(1)) == 0;
                bool is_plunge = has_g1 && tokens.size() > 1 && tokens[1][0] == 'Z';
                bool is_retract = has_g0 && tokens.size() > 1 && tokens[1][0] == 'Z';

                is_tool_on = has_g1 || (is_retract ? false : is_tool_on);

                auto from_color = glm::vec4(1.0f);
                auto to_color = glm::vec4(1.0f);
                if (is_tool_on) {
                    from_color = glm::vec4(1, 0, 0, 1);
                    to_color = glm::vec4(1, 0, 0, 1);
                }

                if (is_plunge) {
                    to_color = glm::vec4(1, 0, 0, 1);
                } else if (is_retract) {
                    to_color = glm::vec4(0, 1, 0, 1);
                }

                // from
                buffer_data.push_back(tool_pos.x);
                buffer_data.push_back(tool_pos.y);
                buffer_data.push_back(tool_pos.z);

                buffer_data.push_back(from_color.r);
                buffer_data.push_back(from_color.g);
                buffer_data.push_back(from_color.b);
                buffer_data.push_back(from_color.a);

                // to
                buffer_data.push_back(new_pos.x);
                buffer_data.push_back(new_pos.y);
                buffer_data.push_back(new_pos.z);

                buffer_data.push_back(to_color.r);
                buffer_data.push_back(to_color.g);
                buffer_data.push_back(to_color.b);
                buffer_data.push_back(to_color.a);

                // calculate extents
                min_pos.x = std::min(min_pos.x, new_pos.x);
                min_pos.y = std::min(min_pos.y, new_pos.y);
                min_pos.z = std::min(min_pos.z, new_pos.z);

                max_pos.x = std::max(max_pos.x, new_pos.x);
                max_pos.y = std::max(max_pos.y, new_pos.y);
                max_pos.z = std::max(max_pos.z, new_pos.z);

                tool_pos = new_pos;
            }
        }
    }

    const GLsizei sizeOfVertexInBytes = 28;
    auto number_of_vertices = buffer_data.size() * sizeof(float) / sizeOfVertexInBytes;

    glBindBuffer(GL_ARRAY_BUFFER, vbo_id);
    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);

    return number_of_vertices;
}

std::string get_shader_info_log(GLuint id) {
    GLint log_length = 0;
    glGetShaderiv(id, GL_INFO_LOG_LENGTH, &log_length);

    char error_log[log_length]; // length includes the NULL character
    glGetShaderInfoLog(id, log_length, &log_length, &error_log[0]);

    return std::string(error_log, log_length);
}

std::string get_program_info_log(GLuint id) {
    GLint log_length = 0;
    glGetProgramiv(id, GL_INFO_LOG_LENGTH, &log_length);

    char error_log[log_length]; // length includes the NULL character
    glGetProgramInfoLog(id, log_length, &log_length, &error_log[0]);

    return std::string(error_log, log_length);
}


bool check_compile_error(GLuint shader_id) {
    GLint is_compiled = 0;
    glGetShaderiv(shader_id, GL_COMPILE_STATUS, &is_compiled);
    if (is_compiled == GL_TRUE)
        return true;

    std::cerr << "Shader compile error: " << "(id: " << shader_id << ") - " << get_shader_info_log(shader_id) << std::endl;
    glDeleteShader(shader_id);
    return false;
}

bool check_link_error(GLuint program_id) {
    GLint is_linked = 0;
    glGetProgramiv(program_id, GL_LINK_STATUS, &is_linked);
    if (is_linked == GL_TRUE)
        return true;

    std::cerr << "Shader program link error: " << "(id: " << program_id << ") - " << get_program_info_log(program_id) << std::endl;
    glDeleteProgram(program_id);
    return false;
}

grbl::shader_program::shader_program(const char *vs_content, const char *ps_content)
        : shader_ids{0, 0},
          program_id{0} {

    shader_ids[0] = glCreateShader(GL_VERTEX_SHADER);
    glShaderSource(shader_ids[0], 1, &vs_content, nullptr);
    glCompileShader(shader_ids[0]);
    check_compile_error(shader_ids[0]);

    shader_ids[1] = glCreateShader(GL_FRAGMENT_SHADER);
    glShaderSource(shader_ids[1], 1, &ps_content, nullptr);
    glCompileShader(shader_ids[1]);
    check_compile_error(shader_ids[1]);

    program_id = glCreateProgram();
    glAttachShader(program_id, shader_ids[0]);
    glAttachShader(program_id, shader_ids[1]);
    glLinkProgram(program_id);
    check_link_error(program_id);

    glUseProgram(program_id);
}

grbl::shader_program::~shader_program() {
    glDetachShader(program_id, shader_ids[0]);
    glDetachShader(program_id, shader_ids[1]);

    glDeleteShader(shader_ids[0]);
    glDeleteShader(shader_ids[1]);

    glDeleteProgram(program_id);
}

void grbl::shader_program::bind() const {
    glUseProgram(program_id);
}

void grbl::shader_program::unbind() {
    glUseProgram(0);
}

int grbl::shader_program::get_uniform(const char *name) {
    if (uniform_cache.count(name) == 0) {
        uniform_cache[name] = glGetUniformLocation(program_id, name);
    }
    return uniform_cache[name];
}

void grbl::shader_program::set_mat3(float *matrix, const char *uniform_name) {
    GLint location = get_uniform(uniform_name);
    if (location != -1)
        glUniformMatrix3fv(location, 1, false, matrix);
}

void grbl::shader_program::set_mat4(float *matrix, const char *uniform_name) {
    GLint location = get_uniform(uniform_name);
    if (location != -1)
        glUniformMatrix4fv(location, 1, false, matrix);
}

void grbl::shader_program::set_vec2(float *value, const char *uniform_name) {
    GLint location = get_uniform(uniform_name);
    if (location != -1)
        glUniform2fv(location, 1, value);
}

void grbl::shader_program::set_vec3(float *value, const char *uniform_name) {
    GLint location = get_uniform(uniform_name);
    if (location != -1)
        glUniform3fv(location, 1, value);
}

void grbl::shader_program::set_vec4(float *value, const char *uniform_name) {
    GLint location = get_uniform(uniform_name);
    if (location != -1)
        glUniform4fv(location, 1, value);
}

void grbl::shader_program::set_int(int value, const char *uniform_name) {
    GLint location = get_uniform(uniform_name);
    if (location != -1)
        glUniform1i(location, value);
}

void grbl::shader_program::set_float(float value, const char *uniform_name) {
    GLint location = get_uniform(uniform_name);
    if (location != -1)
        glUniform1f(location, value);
}