From ad0a80eb858df1308e4a81b77c05d9da21f47ec5 Mon Sep 17 00:00:00 2001
From: Adrian Scripca <benishor@gmail.com>
Date: Wed, 3 May 2023 12:41:54 +0300
Subject: [PATCH] Added glm. Updated widget to expose mouse focus. Added custom
 renderer with stippled line.

---
 .gitmodules    |   5 +-
 CMakeLists.txt |   6 +-
 grbl.cpp       |   2 +-
 libs/glm       |   1 +
 libs/nanogui   |   2 +-
 main.cpp       | 256 +++++++++++++++++------------------
 program.nc     |   2 +-
 render.cpp     | 354 +++++++++++++++++++++++++++++++++++++++++++++++++
 render.h       |  58 ++++++++
 9 files changed, 546 insertions(+), 140 deletions(-)
 create mode 160000 libs/glm
 create mode 100644 render.cpp
 create mode 100644 render.h

diff --git a/.gitmodules b/.gitmodules
index bed7096..abbd48c 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,6 +1,9 @@
 [submodule "libs/nanogui"]
 	path = libs/nanogui
-	url = https://github.com/mitsuba-renderer/nanogui.git
+	url = https://github.com/benishor/nanogui.git
 [submodule "libs/gtest"]
 	path = libs/gtest
 	url = https://github.com/google/googletest.git
+[submodule "libs/glm"]
+	path = libs/glm
+	url = https://github.com/g-truc/glm.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 62b662d..2a4ffaf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,7 +13,9 @@ set(NANOGUI_INSTALL OFF CACHE BOOL " " FORCE)
 # Add the configurations from nanogui
 add_subdirectory(libs/nanogui)
 add_subdirectory(libs/gtest)
+add_subdirectory(libs/glm)
 include_directories(libs/gtest/googletest/include)
+include_directories(libs/glm)
 
 # For reliability of parallel build, make the NanoGUI targets dependencies
 set_property(TARGET glfw glfw_objects nanogui PROPERTY FOLDER "dependencies")
@@ -23,5 +25,5 @@ set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_VISIBILITY_PRESET hidden)
 
 
-add_executable(sender main.cpp grbl.h grbl.cpp grbl_test.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h)
-target_link_libraries(sender nanogui gtest gtest_main)
\ No newline at end of file
+add_executable(sender main.cpp grbl.h grbl.cpp grbl_test.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp)
+target_link_libraries(sender nanogui GL gtest gtest_main)
\ No newline at end of file
diff --git a/grbl.cpp b/grbl.cpp
index 71737bb..c5ef1f6 100644
--- a/grbl.cpp
+++ b/grbl.cpp
@@ -36,7 +36,7 @@ grbl::program::program(std::string filename) {
 }
 
 static auto comment_re = std::regex(R"(\(([^)]*)\))");
-static auto gcode_re = std::regex(R"(([a-zA-Z0-9\s.]+).*(\(([^)]*)\))?)");
+static auto gcode_re = std::regex(R"(([a-zA-Z0-9\s.\-]+)\s*(\(([^)]*)\))?)");
 
 bool grbl::program::load_from_stream(std::istream& in) {
     instructions.clear();
diff --git a/libs/glm b/libs/glm
new file mode 160000
index 0000000..efec5db
--- /dev/null
+++ b/libs/glm
@@ -0,0 +1 @@
+Subproject commit efec5db081e3aad807d0731e172ac597f6a39447
diff --git a/libs/nanogui b/libs/nanogui
index 2ee903c..449aa33 160000
--- a/libs/nanogui
+++ b/libs/nanogui
@@ -1 +1 @@
-Subproject commit 2ee903c96480d4aee54542ea3c340c13cc06dc32
+Subproject commit 449aa332f0b438090d8da2e4ccf5241a28931728
diff --git a/main.cpp b/main.cpp
index 3356985..dfeeafb 100644
--- a/main.cpp
+++ b/main.cpp
@@ -51,9 +51,18 @@
 #include <stb_image.h>
 #include "grbl.h"
 #include <gtest/gtest.h>
+#include <regex>
 #include "grbl_communication.h"
 #include "grbl_machine.h"
 #include "string_utils.h"
+#include "render.h"
+
+#include <glm/vec3.hpp> // glm::vec3
+#include <glm/vec4.hpp> // glm::vec4
+#include <glm/mat4x4.hpp> // glm::mat4
+#include <glm/ext/matrix_transform.hpp> // glm::translate, glm::rotate, glm::scale
+#include <glm/ext/matrix_clip_space.hpp> // glm::perspective
+#include <glm/ext/scalar_constants.hpp> // glm::pi
 
 using namespace nanogui;
 
@@ -74,6 +83,13 @@ public:
     grbl::program pgm;
     Button *btnLoadProgram, *btnCheckProgram, *btnRunProgram;
 
+
+    grbl::program_renderer renderer;
+
+    glm::vec3 cam_target = glm::vec3(0);
+    glm::vec3 cam_src = glm::vec3(0);
+    glm::vec2 cam_rotate = {0, 0};
+
     SenderApp() : Screen(Vector2i(1024, 768), "GRBL Sender") {
         inc_ref();
         window = new Window(this, "Machine status");
@@ -150,8 +166,9 @@ public:
         btnLoadProgram = new Button(pgm_actions, "Load");
         btnLoadProgram->set_callback([&] {
             auto path = file_dialog(
-                    {{"nc",  "G-Code files"},
-                     {"ngc", "G-Code files"}}, false);
+                    {{"gcode", "G-Code files"},
+                     {"nc",    "G-Code files"},
+                     {"ngc",   "G-Code files"}}, true);
 
             btnCheckProgram->set_background_color(colBg);
             btnRunProgram->set_background_color(colBg);
@@ -159,6 +176,16 @@ public:
             if (pgm.load_from_file(path)) {
                 btnCheckProgram->set_enabled(true);
 //                btnRunProgram->set_enabled(true);
+                renderer.update(pgm, cnc);
+
+                auto max_pos = renderer.get_extents_max();
+                auto min_pos = renderer.get_extents_min();
+
+                cam_target = (max_pos - min_pos) / 2.0f;
+                cam_src = cam_target;
+                cam_src.z = (max_pos.x - min_pos.x);
+
+                cam_rotate = {0, 0};
             } else {
                 btnCheckProgram->set_enabled(false);
                 btnRunProgram->set_enabled(false);
@@ -180,51 +207,51 @@ public:
         });
         btnRunProgram->set_tooltip("Execute program");
 
-        // Alternative construction notation using variadic template
-        btnLoadProgram = window->add<Button>("Styled", FA_ROCKET);
-        btnLoadProgram->set_background_color(Color(0, 0, 255, 25));
-        btnLoadProgram->set_callback([] { std::cout << "pushed!" << std::endl; });
-        btnLoadProgram->set_tooltip("This button has a fairly long tooltip. It is so long, in "
-                                    "fact, that the shown text will span several lines.");
-
-        new Label(window, "Toggle buttons", "sans-bold");
-        btnLoadProgram = new Button(window, "Toggle me");
-        btnLoadProgram->set_flags(Button::ToggleButton);
-        btnLoadProgram->set_change_callback([](bool state) { std::cout << "Toggle button state: " << state << std::endl; });
-
-        new Label(window, "Radio buttons", "sans-bold");
-        btnLoadProgram = new Button(window, "Radio button 1");
-        btnLoadProgram->set_flags(Button::RadioButton);
-        btnLoadProgram = new Button(window, "Radio button 2");
-        btnLoadProgram->set_flags(Button::RadioButton);
-
-        new Label(window, "A tool palette", "sans-bold");
-        Widget *tools = new Widget(window);
-        tools->set_layout(new BoxLayout(Orientation::Horizontal,
-                                        Alignment::Middle, 0, 6));
-
-        btnLoadProgram = new ToolButton(tools, FA_CLOUD);
-        btnLoadProgram = new ToolButton(tools, FA_FAST_FORWARD);
-        btnLoadProgram = new ToolButton(tools, FA_COMPASS);
-        btnLoadProgram = new ToolButton(tools, FA_UTENSILS);
-
-        new Label(window, "Popup buttons", "sans-bold");
-        PopupButton *popup_btn = new PopupButton(window, "Popup", FA_FLASK);
-        Popup *popup = popup_btn->popup();
-        popup->set_layout(new GroupLayout());
-        new Label(popup, "Arbitrary widgets can be placed here");
-        new CheckBox(popup, "A check box");
-        // popup right
-        popup_btn = new PopupButton(popup, "Recursive popup", FA_CHART_PIE);
-        Popup *popup_right = popup_btn->popup();
-        popup_right->set_layout(new GroupLayout());
-        new CheckBox(popup_right, "Another check box");
-        // popup left
-        popup_btn = new PopupButton(popup, "Recursive popup", FA_DNA);
-        popup_btn->set_side(Popup::Side::Left);
-        Popup *popup_left = popup_btn->popup();
-        popup_left->set_layout(new GroupLayout());
-        new CheckBox(popup_left, "Another check box");
+//        // Alternative construction notation using variadic template
+//        btnLoadProgram = window->add<Button>("Styled", FA_ROCKET);
+//        btnLoadProgram->set_background_color(Color(0, 0, 255, 25));
+//        btnLoadProgram->set_callback([] { std::cout << "pushed!" << std::endl; });
+//        btnLoadProgram->set_tooltip("This button has a fairly long tooltip. It is so long, in "
+//                                    "fact, that the shown text will span several lines.");
+//
+//        new Label(window, "Toggle buttons", "sans-bold");
+//        btnLoadProgram = new Button(window, "Toggle me");
+//        btnLoadProgram->set_flags(Button::ToggleButton);
+//        btnLoadProgram->set_change_callback([](bool state) { std::cout << "Toggle button state: " << state << std::endl; });
+//
+//        new Label(window, "Radio buttons", "sans-bold");
+//        btnLoadProgram = new Button(window, "Radio button 1");
+//        btnLoadProgram->set_flags(Button::RadioButton);
+//        btnLoadProgram = new Button(window, "Radio button 2");
+//        btnLoadProgram->set_flags(Button::RadioButton);
+//
+//        new Label(window, "A tool palette", "sans-bold");
+//        Widget *tools = new Widget(window);
+//        tools->set_layout(new BoxLayout(Orientation::Horizontal,
+//                                        Alignment::Middle, 0, 6));
+//
+//        btnLoadProgram = new ToolButton(tools, FA_CLOUD);
+//        btnLoadProgram = new ToolButton(tools, FA_FAST_FORWARD);
+//        btnLoadProgram = new ToolButton(tools, FA_COMPASS);
+//        btnLoadProgram = new ToolButton(tools, FA_UTENSILS);
+//
+//        new Label(window, "Popup buttons", "sans-bold");
+//        PopupButton *popup_btn = new PopupButton(window, "Popup", FA_FLASK);
+//        Popup *popup = popup_btn->popup();
+//        popup->set_layout(new GroupLayout());
+//        new Label(popup, "Arbitrary widgets can be placed here");
+//        new CheckBox(popup, "A check box");
+//        // popup right
+//        popup_btn = new PopupButton(popup, "Recursive popup", FA_CHART_PIE);
+//        Popup *popup_right = popup_btn->popup();
+//        popup_right->set_layout(new GroupLayout());
+//        new CheckBox(popup_right, "Another check box");
+//        // popup left
+//        popup_btn = new PopupButton(popup, "Recursive popup", FA_DNA);
+//        popup_btn->set_side(Popup::Side::Left);
+//        Popup *popup_left = popup_btn->popup();
+//        popup_left->set_layout(new GroupLayout());
+//        new CheckBox(popup_left, "Another check box");
 
         perform_layout();
 
@@ -237,81 +264,8 @@ public:
 
         m_render_pass = new RenderPass({this});
         m_render_pass->set_clear_color(0, Color(0.3f, 0.3f, 0.32f, 1.f));
-
-        m_shader = new Shader(
-                m_render_pass,
-
-                /* An identifying name */
-                "a_simple_shader",
-
-#if defined(NANOGUI_USE_OPENGL)
-                R"(/* Vertex shader */
-            #version 330
-            uniform mat4 mvp;
-            in vec3 position;
-            void main() {
-                gl_Position = mvp * vec4(position, 1.0);
-            })",
-
-                /* Fragment shader */
-                R"(#version 330
-            out vec4 color;
-            uniform float intensity;
-            void main() {
-                color = vec4(vec3(intensity), 1.0);
-            })"
-#elif defined(NANOGUI_USE_GLES)
-                R"(/* Vertex shader */
-            precision highp float;
-            uniform mat4 mvp;
-            attribute vec3 position;
-            void main() {
-                gl_Position = mvp * vec4(position, 1.0);
-            })",
-
-                /* Fragment shader */
-                R"(precision highp float;
-            uniform float intensity;
-            void main() {
-                gl_FragColor = vec4(vec3(intensity), 1.0);
-            })"
-#elif defined(NANOGUI_USE_METAL)
-                R"(using namespace metal;
-            struct VertexOut {
-                float4 position [[position]];
-            };
-
-            vertex VertexOut vertex_main(const device packed_float3 *position,
-                                         constant float4x4 &mvp,
-                                         uint id [[vertex_id]]) {
-                VertexOut vert;
-                vert.position = mvp * float4(position[id], 1.f);
-                return vert;
-            })",
-
-                /* Fragment shader */
-                R"(using namespace metal;
-            fragment float4 fragment_main(const constant float &intensity) {
-                return float4(intensity);
-            })"
-#endif
-        );
-
-        uint32_t indices[3 * 2] = {
-                0, 1, 2,
-                2, 3, 0
-        };
-
-        float positions[3 * 4] = {
-                -1.f, -1.f, 0.f,
-                1.f, -1.f, 0.f,
-                1.f, 1.f, 0.f,
-                -1.f, 1.f, 0.f
-        };
-
-        m_shader->set_buffer("indices", VariableType::UInt32, {3 * 2}, indices);
-        m_shader->set_buffer("position", VariableType::Float32, {4, 3}, positions);
-        m_shader->set_uniform("intensity", 0.5f);
+        m_render_pass->set_depth_test(RenderPass::DepthTest::Always, true);
+        m_render_pass->set_cull_mode(RenderPass::CullMode::Disabled);
     }
 
     bool resize_event(const Vector2i& size) override {
@@ -392,10 +346,11 @@ public:
     bool keyboard_event(int key, int scancode, int action, int modifiers) override {
         if (Screen::keyboard_event(key, scancode, action, modifiers))
             return true;
-        if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) {
-            set_visible(false);
-            return true;
-        }
+
+//        if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) {
+//            set_visible(false);
+//            return true;
+//        }
 
         auto new_jog = jog;
 
@@ -471,6 +426,34 @@ public:
         return false;
     }
 
+    bool mouse_motion_event(const Vector2i& p, const Vector2i& rel, int button, int modifiers) override {
+        auto result = Widget::mouse_motion_event(p, rel, button, modifiers);
+        if (window->mouse_focused()) {
+            return result;
+        } else {
+            std::cout << "Mouse motion p:" << p << ", rel:" << rel << ", button:" << button << ", .modifiers:" << modifiers << std::endl;
+            if (button == 1) {
+                cam_rotate += glm::vec2((float) rel.x() / 100.0f, (float) rel.y() / 100.0f);
+            } else if (button == 2) {
+                cam_src.x += (float) rel.x() / 10.0f;
+                cam_src.y += (float) rel.y() / 10.0f;
+            }
+            return true;
+        }
+    }
+
+    bool scroll_event(const Vector2i& p, const Vector2f& rel) override {
+        if (window->mouse_focused()) {
+            return Widget::scroll_event(p, rel);
+        } else {
+            std::cout << "Scroll event: p:" << p << ", rel:" << rel << std::endl;
+            cam_src.z += rel.y();
+            cam_src.z = std::max(cam_src.z, 0.1f);
+
+            return true;
+        }
+    }
+
     virtual void draw(NVGcontext *ctx) {
         // Animate the scrollbar
 //        m_progress->set_value(std::fmod((float) glfwGetTime() / 10, 1.0f));
@@ -479,26 +462,31 @@ public:
         Screen::draw(ctx);
     }
 
+
     virtual void draw_contents() {
-        Matrix4f mvp = Matrix4f::scale(Vector3f(
-                (float) m_size.y() / (float) m_size.x() * 0.25f, 0.25f, 0.25f)) *
-                       Matrix4f::rotate(Vector3f(0, 0, 1), (float) glfwGetTime());
+        auto fb_size = framebuffer_size();
 
-        m_shader->set_uniform("mvp", mvp);
+        renderer.update(pgm, cnc);
 
+        // compute mvp
+        glm::mat4 projection = glm::perspective(45.0f * glm::pi<float>() / 180.0f, (float) fb_size.x() / (float) fb_size.y(), 0.1f, 1000.f);
+        glm::mat4 view = glm::lookAt(cam_src, cam_target, glm::vec3{0.0f, 1.0f, 0.0f});
+        view = glm::rotate(view, cam_rotate.y, glm::vec3(-1.0f, 0.0f, 0.0f));
+        view = glm::rotate(view, cam_rotate.x, glm::vec3(0.0f, 1.0f, 0.0f));
+        glm::mat4 model = glm::mat4(1.0f);
+        auto mvp = projection * view * model;
+
+        // start rendering
         m_render_pass->resize(framebuffer_size());
         m_render_pass->begin();
 
-        m_shader->begin();
-        m_shader->draw_array(Shader::PrimitiveType::Triangle, 0, 6, true);
-        m_shader->end();
+        renderer.render(mvp, glm::vec2(fb_size.x(), fb_size.y()));
 
         m_render_pass->end();
     }
 
 private:
     ProgressBar *m_progress;
-    ref<Shader> m_shader;
     ref<RenderPass> m_render_pass;
 
     using ImageHolder = std::unique_ptr<uint8_t[], void (*)(void *)>;
diff --git a/program.nc b/program.nc
index 80e5883..4ce4449 100644
--- a/program.nc
+++ b/program.nc
@@ -15,7 +15,7 @@ G0 Z10
 (Text: MATEI)
 ( ---------- cut-here ---------- )
 g0 z3
-g0 x2.7539 y21.8701
+g0 x 2.7539 y 21.8701
 g1 z0.1 f500
 g1 f1200
 g1 x2.7539 y21.8701
diff --git a/render.cpp b/render.cpp
new file mode 100644
index 0000000..87fad15
--- /dev/null
+++ b/render.cpp
@@ -0,0 +1,354 @@
+#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);
+
+    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);
+
+        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);
+
+        initialized = true;
+    }
+
+    // update program with machine status
+    // build vbo and vao
+    vertices_count = build_vbo(pgm);
+}
+
+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;
+
+    std::vector<float> buffer_data;
+
+    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;
+    glBindBuffer(GL_ARRAY_BUFFER, vbo_id);
+    glBufferData(GL_ARRAY_BUFFER, sizeOfVertexInBytes * buffer_data.size(), buffer_data.data(), GL_DYNAMIC_DRAW);
+
+    auto number_of_vertices = buffer_data.size() * sizeof(float) / sizeOfVertexInBytes;
+    return number_of_vertices;
+}
+
+void check_compile_error(GLuint shader_id) {
+    GLint isCompiled = 0;
+    glGetShaderiv(shader_id, GL_COMPILE_STATUS, &isCompiled);
+    if (!isCompiled) {
+        GLint maxLength = 0;
+        glGetShaderiv(shader_id, GL_INFO_LOG_LENGTH, &maxLength);
+
+        //The maxLength includes the NULL character
+        char errorLog[maxLength];
+        glGetShaderInfoLog(shader_id, maxLength, &maxLength, &errorLog[0]);
+
+        std::cerr << "Shader compile error: " << "(id: " << shader_id << ", code: " << isCompiled << ", bytes: "
+                  << maxLength << ") - " << std::string(errorLog, maxLength);
+
+        //Provide the infolog in whatever manor you deem best.
+        //Exit with failure.
+        glDeleteShader(shader_id); //Don't leak the shader.
+    } else {
+        std::cout << "Shader " << shader_id << " successfully compiled";
+    }
+}
+
+void check_link_error(GLuint program_id) {
+    GLint is_linked = 0;
+    glGetShaderiv(program_id, GL_LINK_STATUS, &is_linked);
+    if (!is_linked) {
+        GLint maxLength = 0;
+        glGetShaderiv(program_id, GL_INFO_LOG_LENGTH, &maxLength);
+
+        //The maxLength includes the NULL character
+        char errorLog[maxLength];
+        glGetShaderInfoLog(program_id, maxLength, &maxLength, &errorLog[0]);
+
+        std::cerr << "Shader program link error: " << "(id: " << program_id << ", code: " << is_linked << ", bytes: "
+                  << maxLength << ") - " << std::string(errorLog, maxLength);
+
+        //Provide the info log in whatever manner you deem best.
+        //Exit with failure.
+        glDeleteShader(program_id); //Don't leak the program shader.
+    } else {
+        std::cout << "Shader program " << program_id << " successfully linked";
+    }
+}
+
+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, NULL);
+    glCompileShader(shader_ids[0]);
+    int success;
+    char infoLog[512];
+    glGetShaderiv(shader_ids[0], GL_COMPILE_STATUS, &success);
+    if (!success) {
+        glGetShaderInfoLog(shader_ids[0], 512, NULL, infoLog);
+        std::cout << "ERROR::SHADER::VERTEX::COMPILATION_FAILED\n" << infoLog << std::endl;
+    }
+
+    shader_ids[1] = glCreateShader(GL_FRAGMENT_SHADER);
+    glShaderSource(shader_ids[1], 1, &ps_content, NULL);
+    glCompileShader(shader_ids[1]);
+    glGetShaderiv(shader_ids[1], GL_COMPILE_STATUS, &success);
+    if (!success) {
+        glGetShaderInfoLog(shader_ids[1], 512, NULL, infoLog);
+        std::cout << "ERROR::SHADER::FRAGMENT::COMPILATION_FAILED\n" << infoLog << std::endl;
+    }
+
+    program_id = glCreateProgram();
+    glAttachShader(program_id, shader_ids[0]);
+    glAttachShader(program_id, shader_ids[1]);
+    glLinkProgram(program_id);
+
+    glGetProgramiv(program_id, GL_LINK_STATUS, &success);
+    if (!success) {
+        glGetProgramInfoLog(program_id, 512, NULL, infoLog);
+        std::cout << "ERROR::SHADER::PROGRAM::COMPILATION_FAILED\n" << infoLog << std::endl;
+    }
+
+    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);
+}
diff --git a/render.h b/render.h
new file mode 100644
index 0000000..742e3e9
--- /dev/null
+++ b/render.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include "grbl.h"
+#include "grbl_machine.h"
+#include "glm/ext/matrix_float4x4.hpp"
+#include <nanogui/opengl.h>
+
+namespace grbl {
+
+class shader_program;
+
+class program_renderer {
+public:
+    void update(const program& pgm, const machine& cnc);
+    void render(glm::mat4 mvp, glm::vec2 viewport_size);
+
+    glm::vec3 get_extents_min() const { return min_pos; };
+    glm::vec3 get_extents_max() const { return max_pos; };
+
+private:
+    GLsizei build_vbo(const grbl::program& pgm);
+
+    GLuint vbo_id;
+    GLuint vao_id;
+    shader_program *shader = nullptr;
+    bool initialized = false;
+    glm::vec3 min_pos, max_pos;
+    GLsizei vertices_count;
+};
+
+
+class shader_program {
+public:
+    shader_program(const char *vs_content, const char *ps_content);
+    virtual ~shader_program();
+
+    void bind() const;
+    static void unbind();
+
+    GLuint get_id() const { return program_id; }
+
+    int get_uniform(const char *name);
+
+    void set_mat3(float *value, const char *uniform_name);
+    void set_mat4(float *value, const char *uniform_name);
+    void set_vec2(float *value, const char *uniform_name);
+    void set_vec3(float *value, const char *uniform_name);
+    void set_vec4(float *value, const char *uniform_name);
+    void set_float(float value, const char *uniform_name);
+    void set_int(int value, const char *uniform_name);
+
+private:
+    GLuint program_id;
+    GLuint shader_ids[2];
+    std::map<const char *, int> uniform_cache;
+};
+
+}