diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2a4ffaf..9f816dc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,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 render.h render.cpp)
+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 heightmap.h heightmap.cpp)
 target_link_libraries(sender nanogui GL gtest gtest_main)
\ No newline at end of file
diff --git a/docs/todo.txt b/docs/todo.txt
index c9d09b8..5f8b77e 100644
--- a/docs/todo.txt
+++ b/docs/todo.txt
@@ -1,14 +1,14 @@
 Heightmap probing:
     1. define the grid:
-        - from: x, y
-        - to: x, y
-        - step every: 5mm?
+DONE        - from: x, y
+DONE        - to: x, y
+DONE        - step every: 5mm?
         - clearance height: Z1.5
         - start probing at: Z0.5
         - max negative z: Z-0.5 (when to fail probing)
         - z final safety height: Z15
 
-        - from and to can be filled from current project bounding box
+DONE        - from and to can be filled from current project bounding box
 
     2. probing
         - machine moves at grid start position (x, y)
diff --git a/grbl_machine.cpp b/grbl_machine.cpp
index aae2e8c..fe6d974 100644
--- a/grbl_machine.cpp
+++ b/grbl_machine.cpp
@@ -16,6 +16,7 @@ grbl::machine::machine() {
     states[grbl_machine_state::idle] = new machine_state_idle;
     states[grbl_machine_state::check_program] = new machine_state_check_program;
     states[grbl_machine_state::run_program] = new machine_state_run_program;
+    states[grbl_machine_state::heightmap_probing] = new machine_state_heightmap_probing;
 
     switch_to_state(grbl_machine_state::disconnected);
 }
@@ -688,6 +689,12 @@ void grbl::machine::start_z_probe(float min_z, float feed_rate) {
     awaiting_responses++;
 }
 
+void grbl::machine::probe_heightmap(grbl::heightmap& grid) {
+    std::cout << "probing heightmap" << std::endl;
+    dynamic_cast<machine_state_heightmap_probing *>(states[grbl_machine_state::heightmap_probing])->grid = &grid;
+    switch_to_state(grbl_machine_state::heightmap_probing);
+}
+
 bool grbl::jog_state::no_jogging() const {
     return !(up_pressed || down_pressed || left_pressed || right_pressed || z_up_pressed || z_down_pressed);
 }
@@ -1017,3 +1024,146 @@ void grbl::machine_state_run_program::on_line_received(std::string line) {
         cnc->listener->on_alarm(std::stoi(line.substr(6)));
     }
 }
+
+// heightmap probing
+
+void grbl::machine_state_heightmap_probing::on_connected(grbl::machine *m) {
+
+}
+
+void grbl::machine_state_heightmap_probing::on_disconnected(grbl::machine *m) {
+
+}
+
+void grbl::machine_state_heightmap_probing::on_enter(grbl::machine *m) {
+    cnc = m;
+    stage = heightmap_probing_stage::start;
+    failed = false;
+    error = 0;
+    probed_locations = 0;
+    move_to_next_stage();
+}
+
+void grbl::machine_state_heightmap_probing::on_exit(grbl::machine *m) {
+
+}
+
+void grbl::machine_state_heightmap_probing::on_line_received(std::string line) {
+    if (starts_with(line, "ok")) {
+        if (cnc->awaiting_responses > 0) {
+            cnc->awaiting_responses--;
+        }
+        move_to_next_stage();
+    } else if (starts_with(line, "error")) {
+        if (cnc->awaiting_responses > 0) {
+            cnc->awaiting_responses--;
+        }
+        failed = true;
+        error = std::stoi(line.substr(6));
+        move_to_next_stage();
+    } else if (starts_with(line, "Grbl")) {
+        cnc->listener->on_banner(line);
+        cnc->reset_machine_state();
+    } else if (starts_with(line, "<")) {
+        cnc->last_report = parse_status_report(line, cnc->last_report);
+        cnc->listener->on_realtime_status_report(cnc->last_report);
+    } else if (starts_with(line, "[MSG:")) {
+        cnc->listener->on_message(line.substr(5, line.size() - 6));
+    } else if (starts_with(line, "ALARM:")) {
+        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+    } else if (starts_with(line, "[PRB")) {
+        std::cout << "received PRB" << std::endl;
+        line = line.substr(1, line.size() - 2);
+        auto pieces = split_string(line, ":");
+        cnc->parameters[pieces[0]] = pieces[1] + ":" + pieces[2];
+    }
+}
+
+bool grbl::machine_state_heightmap_probing::continue_program() {
+    return false;
+}
+
+void grbl::machine_state_heightmap_probing::move_to_next_stage() {
+    switch (stage) {
+        case heightmap_probing_stage::start:
+            cnc->pipe->send("G0X0Y0Z15");
+            stage = heightmap_probing_stage::goto_home;
+            break;
+        case heightmap_probing_stage::goto_home:
+            cnc->pipe->send("G38.2 Z-65 F100");
+            stage = heightmap_probing_stage::initial_probe_step_back;
+            break;
+        case heightmap_probing_stage::initial_probe_step_back:
+            // step back a bit (1mm, relative)
+            cnc->pipe->send("G91 G0 Z1");
+            stage = heightmap_probing_stage::initial_probe_fine_seek;
+            break;
+        case heightmap_probing_stage::initial_probe_fine_seek:
+            // probe again but finer
+            cnc->pipe->send(" G38.2 Z-5 F5");
+            stage = heightmap_probing_stage::initial_probe;
+            break;
+        case heightmap_probing_stage::initial_probe:
+            std::cout << "Initial probe: " << cnc->parameters["PRB"];
+
+            std::cout << ". Setting this as new zero" << std::endl;
+            cnc->pipe->send("G10 L20 P1 Z0");
+
+            {
+                auto pieces = split_string(cnc->parameters["PRB"], ":");
+                auto axes = split_string(pieces[0], ",");
+                z_zero_in_mpos = std::stof(axes[2]);
+                std::cout << "Z zero in mpos: " << z_zero_in_mpos << std::endl;
+                grid->vertices[probed_locations].z = 0;
+            }
+
+
+            stage = heightmap_probing_stage::goto_next_location;
+            break;
+        case heightmap_probing_stage::goto_next_location: {
+            auto pieces = split_string(cnc->parameters["PRB"], ":");
+            auto axes = split_string(pieces[0], ",");
+            auto current_z = std::stof(axes[2]);
+            if (probed_locations == 0) {
+                z_zero_in_mpos = current_z;
+                std::cout << "Z zero in mpos: " << z_zero_in_mpos << std::endl;
+                grid->vertices[probed_locations].z = 0;
+            } else {
+                auto delta_z = current_z - z_zero_in_mpos;
+                std::cout << "Z[" << probed_locations << "]  = " << delta_z << std::endl;
+                grid->vertices[probed_locations].z = delta_z;
+                cnc->listener->on_heightmap_probe_acquired(grid);
+            }
+        }
+
+            probed_locations++;
+            if (probed_locations == grid->vertices.size()) {
+                cnc->pipe->send("G0Z15"); // safe height
+                stage = heightmap_probing_stage::done;
+            } else {
+                cnc->pipe->send("G90 G0 Z1.5");
+                stage = heightmap_probing_stage::goto_next_location_move;
+            }
+            break;
+        case heightmap_probing_stage::goto_next_location_move:
+            cnc->pipe->send("G0 X" + std::to_string(grid->vertices[probed_locations].x) + " Y" +
+                            std::to_string(grid->vertices[probed_locations].y));
+            stage = heightmap_probing_stage::goto_start_probing_at;
+            break;
+        case heightmap_probing_stage::goto_start_probing_at:
+            cnc->pipe->send("G0 Z0.5"); // this appears to move Z upwards instead of downwards. why?
+            // faking the G0 Z0.5
+//            cnc->pipe->send("G91 G0 Z-1"); // 1.5 - 1 = 0.5.//
+            stage = heightmap_probing_stage::probing;
+            break;
+        case heightmap_probing_stage::probing:
+            cnc->pipe->send("G38.2 Z-5 F5");
+            stage = heightmap_probing_stage::goto_next_location;
+            break;
+        case heightmap_probing_stage::done:
+            cnc->switch_to_state(grbl_machine_state::idle);
+            break;
+        default:
+            break;
+    }
+}
diff --git a/grbl_machine.h b/grbl_machine.h
index 57639c8..d04bad0 100644
--- a/grbl_machine.h
+++ b/grbl_machine.h
@@ -4,6 +4,7 @@
 #include <unordered_map>
 #include "grbl_communication.h"
 #include "grbl.h"
+#include "heightmap.h"
 
 namespace grbl {
 
@@ -40,14 +41,14 @@ struct realtime_status_report {
     float actual_rpm = 0;
     union {
         struct {
-            bool x_limit : 1;
-            bool y_limit : 1;
-            bool z_limit : 1;
-            bool probe : 1;
-            bool door : 1;
-            bool hold : 1;
-            bool soft_reset : 1;
-            bool cycle_start : 1;
+            bool x_limit: 1;
+            bool y_limit: 1;
+            bool z_limit: 1;
+            bool probe: 1;
+            bool door: 1;
+            bool hold: 1;
+            bool soft_reset: 1;
+            bool cycle_start: 1;
         } bit;
         uint8_t value = 0;
     } signals;
@@ -81,6 +82,7 @@ enum class grbl_machine_state {
     run_program,
     check_program,
     idle,
+    heightmap_probing,
 };
 
 struct jog_state {
@@ -127,6 +129,7 @@ struct machine_listener {
     virtual void on_settings_reloaded() = 0;
     virtual void on_parameters_reloaded() = 0;
     virtual void on_probe_result(bool probe_touched, float probe_coords[3]) = 0;
+    virtual void on_heightmap_probe_acquired(heightmap* grid) = 0;
 };
 
 
@@ -149,6 +152,14 @@ enum class run_stage {
     run_program,
 };
 
+enum class heightmap_probing_stage {
+    start,
+    goto_home,
+    initial_probe,
+    goto_next_location,
+    probing, initial_probe_step_back, initial_probe_fine_seek, goto_next_location_move, goto_start_probing_at, done
+};
+
 struct machine;
 
 struct machine_state {
@@ -222,6 +233,25 @@ struct machine_state_run_program : public machine_state {
     size_t run_error;
 };
 
+struct machine_state_heightmap_probing : public machine_state {
+    void on_connected(machine *m) override;
+    void on_disconnected(machine *m) override;
+    void on_enter(machine *m) override;
+    void on_exit(machine *m) override;
+    void on_line_received(std::string line) override;
+
+    bool continue_program();
+    void move_to_next_stage();
+
+    machine *cnc;
+    heightmap_probing_stage stage = heightmap_probing_stage::start;
+    bool failed;
+    size_t error;
+    size_t probed_locations;
+    heightmap *grid;
+    float z_zero_in_mpos = 0;
+};
+
 
 struct settings_cmp {
     bool operator()(const std::string& a, const std::string& b) const {
@@ -280,6 +310,7 @@ struct machine : public transport_callbacks {
 
     void start_z_probe(float min_z, float feed_rate);
 
+    void probe_heightmap(grbl::heightmap& grid);
 protected:
     void on_connected(transport *transport) override;
     void on_disconnected(transport *transport) override;
@@ -292,6 +323,7 @@ protected:
     friend class machine_state_idle;
     friend class machine_state_check_program;
     friend class machine_state_run_program;
+    friend class machine_state_heightmap_probing;
 
     std::map<grbl_machine_state, machine_state *> states;
     std::map<std::string, std::string, settings_cmp> settings;
diff --git a/heightmap.cpp b/heightmap.cpp
new file mode 100644
index 0000000..af0623f
--- /dev/null
+++ b/heightmap.cpp
@@ -0,0 +1,29 @@
+#include "heightmap.h"
+
+grbl::heightmap grbl::heightmap::from_params(float from_x, float from_y, float to_x, float to_y, float resolution) {
+    heightmap result{};
+
+    result.from_x = from_x;
+    result.from_y = from_y;
+    result.to_x = to_x;
+    result.to_y = to_y;
+    result.resolution = resolution;
+
+    result.x_segments = static_cast<size_t>(ceilf((to_x - from_x) / resolution));
+    result.y_segments = static_cast<size_t>(ceilf((to_y - from_y) / resolution));
+
+    float y_pos = from_y;
+    for (int y = 0; y < (result.y_segments + 1); y++) {
+        float x_pos = from_x;
+        for (int x = 0; x < (result.x_segments + 1); x++) {
+            // add a bit of random wobble to make it visible [-10, +10]
+//            float z = ((rand() / (float) RAND_MAX) - 0.5f) * 2.0f * 1.0f;
+            float z = 0.0f;
+            result.vertices.emplace_back(x_pos, y_pos, z);
+            x_pos += resolution;
+        }
+        y_pos += resolution;
+    }
+
+    return result;
+}
diff --git a/heightmap.h b/heightmap.h
new file mode 100644
index 0000000..1d2c29b
--- /dev/null
+++ b/heightmap.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <cstddef>
+#include <cmath>
+#include <glm/vec3.hpp>
+#include <vector>
+
+namespace grbl {
+
+struct heightmap {
+    static heightmap from_params(float from_x, float from_y, float to_x, float to_y, float resolution);
+
+    float from_x, from_y;
+    float to_x, to_y;
+    float resolution;
+    size_t x_segments, y_segments;
+    std::vector<glm::vec3> vertices;
+};
+
+
+}
diff --git a/main.cpp b/main.cpp
index 33b949b..d68826f 100644
--- a/main.cpp
+++ b/main.cpp
@@ -34,6 +34,7 @@
 #include "glm/gtx/quaternion.hpp"
 #include "nanogui/nanogui.h"
 #include "glm/gtc/matrix_inverse.hpp"
+#include "heightmap.h"
 #include <glm/ext/quaternion_float.hpp>
 #include <glm/ext/quaternion_trigonometric.hpp>
 #include <glm/vec3.hpp> // glm::vec3
@@ -84,6 +85,7 @@ public:
 
     Button *btn_keyboard_jog;
     std::stringstream dro_ss;
+    grbl::heightmap heightmap_grid;
 
 
     SenderApp() : Screen(Vector2i(1024, 768), "GRBL Sender") {
@@ -128,6 +130,7 @@ public:
 
         m_render_pass = new RenderPass({this});
         m_render_pass->set_clear_color(0, Color(0.13f, 0.13f, 0.13f, 1.f));
+        m_render_pass->set_clear_depth(0.0);
         m_render_pass->set_depth_test(RenderPass::DepthTest::Always, true);
         m_render_pass->set_cull_mode(RenderPass::CullMode::Disabled);
     }
@@ -456,6 +459,7 @@ public:
     TextBox *txt_heightmap_from_x, *txt_heightmap_from_y;
     TextBox *txt_heightmap_to_x, *txt_heightmap_to_y;
     TextBox *txt_grid_res;
+    TextBox *txt_clearance_height, *txt_start_probing_at, *txt_max_negative_z, *txt_final_z_height;
 
     void add_heightmap_markup() {
         heightmap_layer->add<Label>("Grid definition", "sans-bold", 20);
@@ -524,6 +528,37 @@ public:
             return true;
         });
         grid_res_holder->add<Label>("mm", "sans-bold", 20);
+
+        // business params
+        auto heightmap_business_holder = heightmap_params_holder->add<Widget>();
+        heightmap_business_holder->set_layout(new GridLayout(nanogui::Orientation::Horizontal, 3, Alignment::Fill, 4, 4));
+
+        heightmap_business_holder->add<Label>("Clearance height Z", "sans-bold", 20);
+        txt_clearance_height = heightmap_business_holder->add<TextBox>("1.5");
+        txt_clearance_height->set_editable(true);
+        heightmap_business_holder->add<Label>("", "sans-bold");
+
+        heightmap_business_holder->add<Label>("Start probing at Z", "sans-bold", 20);
+        txt_start_probing_at = heightmap_business_holder->add<TextBox>("0.5");
+        txt_start_probing_at->set_editable(true);
+        heightmap_business_holder->add<Label>("", "sans-bold");
+
+        heightmap_business_holder->add<Label>("Max negative Z", "sans-bold", 20);
+        txt_max_negative_z = heightmap_business_holder->add<TextBox>("-1.0");
+        txt_max_negative_z->set_editable(true);
+        heightmap_business_holder->add<Label>("(when to fail probing)", "sans-bold");
+
+        heightmap_business_holder->add<Label>("Final Z safety height", "sans-bold", 20);
+        txt_final_z_height = heightmap_business_holder->add<TextBox>("15");
+        txt_final_z_height->set_editable(true);
+        heightmap_business_holder->add<Label>("", "sans-bold");
+
+        heightmap_business_holder->add<Label>("", "sans-bold");
+        auto btn_start_probing = heightmap_business_holder->add<Button>("Start Probing");
+        btn_start_probing->set_callback([&]() {
+            cnc.probe_heightmap(heightmap_grid);
+        });
+        heightmap_business_holder->add<Label>("", "sans-bold");
     }
 
     void fill_heightmap_from_model() {
@@ -540,8 +575,17 @@ public:
         auto from_y = std::stof(txt_heightmap_from_y->value());
         auto to_x = std::stof(txt_heightmap_to_x->value());
         auto to_y = std::stof(txt_heightmap_to_y->value());
-        auto res = std::stoi(txt_grid_res->value());
-        renderer.update_grid(from_x, from_y, to_x, to_y, res);
+        auto res = std::stof(txt_grid_res->value());
+
+        if (from_x != heightmap_grid.from_x ||
+            from_y != heightmap_grid.from_y ||
+            to_x != heightmap_grid.to_x ||
+            to_y != heightmap_grid.to_y ||
+            res != heightmap_grid.resolution) {
+            heightmap_grid = std::move(grbl::heightmap::from_params(from_x, from_y, to_x, to_y, res));
+            renderer.update_grid(heightmap_grid);
+        }
+
     }
 
     void add_pins_markup() {
@@ -773,6 +817,11 @@ public:
         new MessageDialog(this, MessageDialog::Type::Warning, "Probe result", ss.str());
     }
 
+    void on_heightmap_probe_acquired(grbl::heightmap *grid) override {
+        std::cout << "Updating grid" << std::endl;
+        renderer.update_grid(*grid);
+    }
+
     void on_check_completed(bool success, size_t failed_index, size_t error) override {
         btn_check_program->set_background_color(success ? colGreen : color_red);
         if (success) {
diff --git a/render.cpp b/render.cpp
index 0b89e46..1778d3c 100644
--- a/render.cpp
+++ b/render.cpp
@@ -145,6 +145,8 @@ static void add_triangle(std::vector<float>& buffer_data, glm::vec3 p1, glm::vec
 
 
 void grbl::program_renderer::render(glm::mat4 model, glm::mat4 view, glm::mat4 projection, glm::mat3 normal_mat, glm::vec2 viewport_size) {
+    if (shader == nullptr || heightmap_shader == nullptr) return;
+
     // draw heightmap
     heightmap_shader->bind();
     heightmap_shader->set_mat4(glm::value_ptr(model), "mmtx");
@@ -422,43 +424,39 @@ GLsizei grbl::program_renderer::update_model_vbo(const grbl::program& pgm) {
 }
 
 
-void grbl::program_renderer::update_grid(float from_x, float from_y, float to_x, float to_y, int resolution) {
-
-    int x_segments = ceil((to_x - from_x) / resolution);
-    int y_segments = ceil((to_y - from_y) / resolution);
+void grbl::program_renderer::update_grid(const grbl::heightmap& grid) {
 
     glm::vec4 color = {0.5, 0.3, 0, 1};
 
-    std::vector<glm::vec3> vertices;
-
-    float y_pos = from_y;
-    for (int y = 0; y < (y_segments + 1); y++) {
-        float x_pos = from_x;
-        for (int x = 0; x < (x_segments + 1); x++) {
-            vertices.emplace_back(x_pos, y_pos, 0);
-            x_pos += resolution;
-        }
-        y_pos += resolution;
-    }
-
     std::vector<float> buffer_data;
 
-    for (int y = 0; y < y_segments; y++) {
-        for (int x = 0; x < x_segments; x++) {
-            int current = x + y * (x_segments + 1);
+    for (int y = 0; y < grid.y_segments; y++) {
+        for (int x = 0; x < grid.x_segments; x++) {
+            int current = x + y * (grid.x_segments + 1);
             int next = current + 1;
-            int bottom = next + x_segments;
+            int bottom = next + grid.x_segments;
             int bottom_next = bottom + 1;
 
-            auto p1 = vertices[current];
-            auto p2 = vertices[bottom];
-            auto p3 = vertices[next];
+            auto p1 = grid.vertices[current];
+            auto p2 = grid.vertices[bottom];
+            auto p3 = grid.vertices[next];
+
+            // exaggerate Z
+            auto exaggeration_factor = 1000.0f;
+            p1.z *= exaggeration_factor;
+            p2.z *= exaggeration_factor;
+            p3.z *= exaggeration_factor;
+
             glm::vec3 normal = glm::normalize(glm::cross(p1 - p2, p3 - p1));
             add_triangle(buffer_data, p1, p2, p3, normal, color);
 
-            p1 = vertices[next];
-            p2 = vertices[bottom];
-            p3 = vertices[bottom_next];
+            p1 = grid.vertices[next];
+            p2 = grid.vertices[bottom];
+            p3 = grid.vertices[bottom_next];
+            p1.z *= exaggeration_factor;
+            p2.z *= exaggeration_factor;
+            p3.z *= exaggeration_factor;
+
             normal = glm::normalize(glm::cross(p1 - p2, p3 - p1));
             add_triangle(buffer_data, p1, p2, p3, normal, color);
         }
diff --git a/render.h b/render.h
index 32dd3cb..2883b78 100644
--- a/render.h
+++ b/render.h
@@ -3,6 +3,7 @@
 #include "grbl.h"
 #include "grbl_machine.h"
 #include "glm/ext/matrix_float4x4.hpp"
+#include "heightmap.h"
 #include <nanogui/opengl.h>
 
 namespace grbl {
@@ -18,7 +19,7 @@ public:
 
     glm::vec3 get_extents_max() const { return max_pos; };
 
-    void update_grid(float from_x, float from_y, float to_x, float to_y, int resolution);
+    void update_grid(const grbl::heightmap& grid);
 private:
     GLsizei update_model_vbo(const grbl::program& pgm);