Exposed exaggeration factor when rendering heightmap.

Added checkbox to select whether to run autoleveled program or not.
Heightmap can handle non integer from_x and to_x.

CMakeLists.txt

@@ -24,6 +24,9 @@ set_property(TARGET glfw glfw_objects nanogui PROPERTY FOLDER "dependencies")
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_VISIBILITY_PRESET hidden)
 
+set(TESTS grbl_test.cpp heightmap_test.cpp)
+set(TERJE terje/gcode.cpp terje/gcode.h terje/gcode_parser.h terje/gcode_parser.cpp terje/machine.h terje/machine.cpp terje/grbl.h terje/grbl.cpp terje/comms.h terje/comms.cpp terje/telnet.h terje/telnet.cpp terje/measure_view_model.h terje/measure_view_model.cpp)
+set(SENDER_GRBL_SRC grbl.h grbl.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp gcode_parser.h gcode_parser.cpp gcode_commands.h gcode_commands.cpp sender_app.h sender_app.cpp gcode_file.h gcode_file.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)
+add_executable(sender main.cpp ${TESTS} ${TERJE} ${SENDER_GRBL_SRC})
 target_link_libraries(sender nanogui GL gtest gtest_main)

docs/todo.txt

@@ -0,0 +1,64 @@
+Heightmap probing:
+    1. define the grid:
+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
+
+DONE        - from and to can be filled from current project bounding box
+DONE    2. probing
+DONE        - machine moves at grid start position (x, y)
+DONE            G0X0Y0
+DONE        - gets down for initial probing
+DONE            G38.2 Z-65 F100
+DONE        - step back a bit (1mm, relative)
+DONE            G91 G0 Z1
+DONE        - seek again with lower feed rate
+DONE            G38.2 Z-5 F5
+DONE        - after first probe ever, this Z becomes reference 0
+DONE            G10 L20 P0 Z0
+DONE        - all subsequent probes will be relative to this
+DONE        - for each probing location:
+DONE            - G0 Z"clearance height"
+DONE            - G0 XxxYyyy (next point location)
+DONE            - G0 Z"start probing at"
+DONE            - G38.2 Z-5 F5
+DONE            - store Z offset and place it in the heightmap
+
+        3. modify loaded program with heightmap data
+        - foreach line in program
+            - if line contains a Z coordinate, update the Z according to the X and Y
+            - if line does not contain a Z coordinate, add a Z coordinate according to the X and Y
+
+DONE -Let's get rid of the pesky inter-thread communication issue which is causing a lot of confusion.
+DONE - Solve bug in which the probing data does not get rendered properly as it gets probed.
+
+Bug: query coordinate systems after first z probe.
+Refactor: see if we can unify machine state line handling and remove duplication
+
+Edge finding
+    - prerequisites
+        - which edge to find? -X/+X/-Y/+Y
+        - bit diameter used
+        - put the  
+
+Corner finding
+    - which corner are we probing: up left, up right, bottom left, bottom right
+    - need to know the bit diameter
+    - use G38.2 to probe X edge and set that as current X - diam/2
+
+Render arcs
+Render quadratic splines
+Render bezier splines
+Synchronize executing program with render (show executed paths with different colors)
+Add aggressive buffer execution policy and make it selectable.
+Show a progress bar when executing programs.
+Show a progress bar when probing.
+
+DONE - Show program extents
+
+DONE - Render program extents
+DONE    - show a bounding box for the program extents

gcode_commands.cpp

@@ -0,0 +1,121 @@
+#include "gcode_commands.h"
+#include "glm/geometric.hpp"
+
+glm::vec<3, double> grbl::roll_components(glm::vec<3, double> v, int turns) {
+    glm::vec<3, double> roll{};
+
+    for (int i = 0; i < 3; i++) {
+        roll[i] = v[(i - turns + 300) % 3];
+    }
+
+    return roll;
+}
+
+grbl::mcode_cmd::mcode_cmd(int code, int line)
+        : code(code) {
+    command::line_number = line;
+}
+
+grbl::spindle_cmd::spindle_cmd(double s, int line)
+        : speed(s) {
+    command::line_number = line;
+}
+
+grbl::dwell_cmd::dwell_cmd(double s, int line)
+        : seconds(s) {
+    command::line_number = line;
+}
+
+double grbl::line_motion_cmd::length() const {
+    if (!start_valid || !position_valid[0] || !position_valid[1] || !position_valid[2])
+        return 0;
+
+    return glm::length(delta());
+}
+
+glm::vec<3, double> grbl::line_motion_cmd::interpolate(double ratio) {
+    return start + delta() * ratio;
+}
+
+std::vector<grbl::motion_cmd*> grbl::line_motion_cmd::split(double lngth) {
+    //don't split up rapid or not fully defined motions
+    if (rapid || !start_valid || !position_valid[0] || !position_valid[1] || !position_valid[2]) {
+        return {this};
+    }
+
+    std::vector<motion_cmd*> result;
+    int divisions = (int) std::ceil(length() / lngth);
+
+    if (divisions < 1) {
+        divisions = 1;
+    }
+
+    glm::vec3 last_end = start;
+
+    for (int i = 1; i <= divisions; i++) {
+        glm::vec3 end = interpolate(((double) i) / divisions);
+
+        auto immediate = new line_motion_cmd;
+        immediate->start = last_end;
+        immediate->end = end;
+        immediate->feed = feed;
+        immediate->position_valid[0] = immediate->position_valid[1] = immediate->position_valid[2] = true;
+        immediate->start_valid = true;
+        result.push_back(immediate);
+
+        last_end = end;
+    }
+    return result;
+}
+
+double grbl::arc_motion_cmd::length() const {
+    return abs(angle_span() * radius());
+}
+
+glm::vec<3, double> grbl::arc_motion_cmd::interpolate(double ratio) {
+    double angle = start_angle() + angle_span() * ratio;
+
+    glm::vec<3, double> onPlane = {
+            u + (radius() * cos(angle)),
+            v + (radius() * sin(angle)),
+            0
+    };
+
+    auto d = delta() * ratio;
+    double helix = grbl::roll_components(start + d, -(int) plane).z;
+
+    onPlane.z = helix;
+
+    glm::vec3 interpolation = roll_components(onPlane, (int) plane);
+    return interpolation;
+}
+
+std::vector<grbl::motion_cmd*> grbl::arc_motion_cmd::split(double lngth) {
+    int divisions = (int) ceil(length() / lngth);
+
+    if (divisions < 1)
+        divisions = 1;
+
+    glm::vec3 lastEnd = start;
+
+
+    std::vector<grbl::motion_cmd*> result;
+
+    for (int i = 1; i <= divisions; i++) {
+        glm::vec<3, double> end = interpolate(((double) i) / divisions);
+
+        auto immediate = new arc_motion_cmd;
+        immediate->start = lastEnd;
+        immediate->end = end;
+        immediate->feed = feed;
+        immediate->direction = direction;
+        immediate->plane = plane;
+        immediate->u = u;
+        immediate->v = v;
+
+        result.push_back(immediate);
+
+        lastEnd = end;
+    }
+    return result;
+}

gcode_commands.h

@@ -0,0 +1,123 @@
+#pragma once
+
+#include <glm/vec3.hpp>
+#include <vector>
+#include <memory>
+#include <cmath>
+#include "glm/ext/scalar_constants.hpp"
+
+namespace grbl {
+
+    glm::vec<3, double> roll_components(glm::vec<3, double> v, int turns);
+
+    enum class arc_plane {
+        xy = 0,
+        yz = 1,
+        zx = 2
+    };
+
+    enum class arc_direction {
+        cw,
+        ccw
+    };
+
+    struct command {
+        virtual ~command() = default;
+        int line_number = 1;
+    };
+
+    struct mcode_cmd : public command {
+        mcode_cmd(int code, int line_number);
+
+        int code;
+    };
+
+    struct spindle_cmd : public command {
+        spindle_cmd(double speed, int line_number);
+        double speed;
+    };
+
+    struct dwell_cmd : public command {
+        dwell_cmd(double seconds, int line_number);
+        double seconds;
+    };
+
+    struct motion_cmd : public command {
+        glm::vec<3, double> start = glm::vec3(0);
+        glm::vec<3, double> end = glm::vec3(0);
+        double feed = 0;
+
+        glm::vec<3, double> delta() const { return end - start; }
+
+        // Total travel distance of tool
+        virtual double length() const = 0;
+        virtual glm::vec<3, double> interpolate(double ratio) = 0;
+        virtual std::vector<motion_cmd*> split(double length) = 0;
+    };
+
+    struct line_motion_cmd : public motion_cmd {
+
+        bool rapid = false;
+        // PositionValid[i] is true if the corresponding coordinate of the end position was defined in the file.
+        // eg. for a file with "G0 Z15" as the first line, X and Y would still be false
+        bool position_valid[3] = {false, false, false};
+        bool start_valid = false;
+
+        double length() const override;
+        glm::vec<3, double> interpolate(double ratio) override;
+        std::vector<motion_cmd*> split(double length) override;
+    };
+
+    struct arc_motion_cmd : public motion_cmd {
+        arc_plane plane;
+        arc_direction direction;
+        double u;    //absolute position of center in first axis of plane
+        double v;    //absolute position of center in second axis of plane
+
+        double start_angle() const {
+            glm::vec<3, double> start_in_plane = roll_components(start, -(int) plane);
+            double X = start_in_plane.x - u;
+            double Y = start_in_plane.y - v;
+            return atan2(Y, X);
+        }
+
+        double end_angle() const {
+            glm::vec<3, double> end_in_plane = roll_components(end, -(int) plane);
+            double X = end_in_plane.x - u;
+            double Y = end_in_plane.y - v;
+            return atan2(Y, X);
+        }
+
+        double angle_span() const {
+            double span = end_angle() - start_angle();
+            if (direction == arc_direction::cw) {
+                if (span >= 0)
+                    span -= 2 * glm::pi<double>();
+            } else {
+                if (span <= 0)
+                    span += 2 * glm::pi<double>();
+            }
+
+            return span;
+        }
+
+        double radius() const {
+            glm::vec<3, double> start_plane = roll_components(start, -(int) plane);
+            glm::vec<3, double> end_plane = roll_components(end, -(int) plane);
+
+            return (
+                           sqrt(pow(start_plane.x - u, 2) + pow(start_plane.y - v, 2)) +
+                           sqrt(pow(end_plane.x - u, 2) + pow(end_plane.y - v, 2))
+                   ) / 2;
+        }
+
+        glm::vec<3, double> interpolate(double ratio) override;
+        std::vector<motion_cmd*> split(double length) override;
+
+        double length() const override;
+
+    };
+
+
+}
+

gcode_file.cpp

@@ -0,0 +1,157 @@
+#include "gcode_file.h"
+#include "gcode_parser.h"
+#include <fstream>
+#include <iomanip>
+#include <utility>
+
+grbl::grbl_file grbl::grbl_file::load(std::string path) {
+    grbl::grbl_parser parser;
+
+    grbl::grbl_file result{};
+
+    std::ifstream in_file{path};
+    if (in_file) {
+        parser.parse(in_file);
+        result = grbl_file(parser.commands);
+    }
+
+    return result;
+}
+
+grbl::grbl_file::grbl_file(std::vector<grbl::command*> cmd)
+        : commands(cmd) {
+
+}
+
+std::vector<std::string> grbl::grbl_file::get_gcode() {
+    std::vector<std::string> result;
+
+    result.emplace_back("G90 G91.1 G21 G17");
+
+    grbl::parser_state state;
+    auto xyz = "XYZ";
+
+    std::stringstream ss;
+    ss << std::fixed << std::setprecision(3);
+
+    for (auto &c: commands) {
+        auto *motion = dynamic_cast<motion_cmd *>(c);
+        bool is_motion = motion != nullptr;
+
+        auto *line = dynamic_cast<line_motion_cmd *>(c);
+        auto *arc = dynamic_cast<arc_motion_cmd *>(c);
+        auto *mcode = dynamic_cast<mcode_cmd *>(c);
+        auto *spindle = dynamic_cast<spindle_cmd *>(c);
+        auto *dwell = dynamic_cast<dwell_cmd *>(c);
+
+        if (is_motion) {
+            if (motion->feed != state.feed) {
+                ss.str("");
+                ss << "F" << motion->feed;
+                result.push_back(ss.str());
+                state.feed = motion->feed;
+            }
+        }
+
+        if (line != nullptr) {
+            std::string code = line->rapid ? "G0" : "G1";
+
+            for (int i = 0; i < 3; i++) {
+                if (!line->position_valid[i])
+                    continue;
+                if (!line->start_valid || state.position[i] != line->end[i]) {
+                    ss.str("");
+                    ss << xyz[i] << line->end[i];
+                    code += ss.str();
+                }
+            }
+
+            result.push_back(code);
+
+            state.position = line->end;
+            continue;
+        }
+
+        if (arc != nullptr) {
+            if (state.plane != arc->plane) {
+                switch (arc->plane) {
+                    case arc_plane::xy:
+                        result.emplace_back("G17");
+                        break;
+                    case arc_plane::yz:
+                        result.emplace_back("G19");
+                        break;
+                    case arc_plane::zx:
+                        result.emplace_back("G18");
+                        break;
+                }
+                state.plane = arc->plane;
+            }
+
+            std::string code = arc->direction == arc_direction::cw ? "G2" : "G3";
+            if (state.position.x != arc->end.x) {
+                ss.str("");
+                ss << "X" << arc->end.x;
+                code += ss.str();
+            }
+            if (state.position.y != arc->end.y) {
+                ss.str("");
+                ss << "Y" << arc->end.y;
+                code += ss.str();
+            }
+            if (state.position.z != arc->end.z) {
+                ss.str("");
+                ss << "Z" << arc->end.z;
+                code += ss.str();
+            }
+
+            glm::vec3 center = roll_components({arc->u, arc->v, 0}, (int) arc->plane) - state.position;
+
+            if (center.x != 0 && arc->plane != arc_plane::yz) {
+                ss.str("");
+                ss << " I" << center.x;
+                code += ss.str();
+            }
+            if (center.y != 0 && arc->plane != arc_plane::zx) {
+                ss.str("");
+                ss << " J" << center.y;
+                code += ss.str();
+            }
+            if (center.z != 0 && arc->plane != arc_plane::xy) {
+                ss.str("");
+                ss << " K" << center.z;
+                code += ss.str();
+            }
+
+            result.push_back(code);
+            state.position = arc->end;
+            continue;
+        }
+
+        if (mcode != nullptr) {
+            int code = mcode->code;
+            if (code == 2 || code == 30)
+                continue;
+
+            result.push_back("M" + std::to_string(code));
+            continue;
+        }
+
+        if (spindle != nullptr) {
+            ss.str("");
+            ss << "S" << spindle->speed;
+            result.push_back(ss.str());
+            continue;
+        }
+
+        if (dwell != nullptr) {
+            ss.str("");
+            ss << "G4 P" << dwell->seconds;
+            result.push_back(ss.str());
+            continue;
+        }
+    }
+
+    return result;
+}
+

gcode_file.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <string>
+#include "gcode_commands.h"
+
+namespace grbl {
+
+    struct grbl_file {
+
+        grbl_file() = default;
+        grbl_file(std::vector<grbl::command *> commands);
+        static grbl_file load(std::string path);
+
+        std::vector<std::string> get_gcode();
+
+        std::vector<grbl::command *> commands;
+    };
+}

gcode_parser.cpp

@@ -0,0 +1,529 @@
+#include "gcode_parser.h"
+
+grbl::parse_exception::parse_exception(const std::string &reason, size_t lineNumber)
+        : reason(reason), line_number(lineNumber) {}
+
+const char *grbl::parse_exception::what() const noexcept {
+    return reason.c_str();
+}
+
+struct word {
+    char command;
+    double parameter;
+
+    std::string to_string() const {
+        std::string result;
+        result += command;
+        result += std::to_string(parameter);
+        return result;
+    }
+};
+
+static std::array<double, 4> motion_commands = {0, 1, 2, 3};
+static std::string valid_words = "GMXYZIJKFRSP";
+static std::string ignore_axes = "ABC";
+
+std::string grbl::grbl_parser::cleanup_line(std::string line, int line_number) {
+    auto comment_index = line.rfind(';');
+    if (comment_index != std::string::npos) {
+        line = line.substr(0, comment_index);
+    }
+
+    size_t start;
+    while ((start = line.find('(')) != std::string::npos) {
+        auto end = line.find(')');
+        if (end == std::string::npos || end < start)
+            throw parse_exception("mismatched parentheses", line_number);
+
+        line.erase(start, end - start + 1);
+    }
+
+    return line;
+}
+
+void grbl::grbl_parser::parse(std::istream &in) {
+    int i = 0;
+    for (std::string line; std::getline(in, line);) {
+        i++;
+        line = trim(cleanup_line(line, i));
+        if (line.empty()) continue;
+
+        parse(to_upper(line), i);
+    }
+}
+
+bool grbl::grbl_parser::is_motion_command(double param) {
+    for (auto &p: motion_commands) {
+        if (p == param)
+            return true;
+    }
+    return false;
+}
+
+void grbl::grbl_parser::parse(std::string line, int line_number) {
+    static auto gcode_re = std::regex(R"([a-zA-Z]\s*\-?\d*\.?\d*)");
+
+    std::vector<word> words;
+
+    std::smatch matches;
+    std::string::const_iterator start(line.cbegin());
+    while (regex_search(start, line.cend(), matches, gcode_re)) {
+        auto token = remove_spaces(matches[0]);
+
+        words.push_back(
+                word{
+                        .command= token[0],
+                        .parameter = std::stod(token.substr(1))
+                }
+        );
+
+        start = matches.suffix().first;
+    }
+
+    auto it = words.begin();
+    while (it != words.end()) {
+        if ((*it).command == 'N') {
+            it = words.erase(it);
+            continue;
+        }
+
+        bool ignore_additional_axes = true;
+        if (ignore_axes.find((*it).command) != std::string::npos && ignore_additional_axes) {
+            it = words.erase(it);
+            continue;
+        }
+
+        if (valid_words.find((*it).command) == std::string::npos) {
+            warnings.push_back("ignoring unknown word (letter): \"" + (*it).to_string() + "\". (line " +
+                               std::to_string(line_number) + ")");
+            it = words.erase(it);
+            continue;
+        }
+
+        if ((*it).command != 'F') {
+            it++;
+            continue;
+        }
+
+        state.feed = (*it).parameter;
+        if (state.unit == parse_unit::imperial)
+            state.feed *= 25.4;
+
+        it = words.erase(it);
+    }
+
+
+    it = words.begin();
+    while (it != words.end()) {
+        if ((*it).command == 'M') {
+            int param = (int) (*it).parameter;
+            if (param != (*it).parameter || param < 0) {
+                throw parse_exception("M code can only have positive integer parameters", line_number);
+            }
+
+            commands.push_back(new mcode_cmd{param, line_number});
+            it = words.erase(it);
+            continue;
+        }
+
+        if ((*it).command == 'S') {
+            double param = (*it).parameter;
+            if (param < 0) {
+                warnings.push_back("spindle speed must be positive. (line " + std::to_string(line_number) + ")");
+            }
+
+            commands.push_back(new spindle_cmd{std::abs(param), line_number});
+            it = words.erase(it);
+            continue;
+        }
+
+
+        if ((*it).command == 'G' && !is_motion_command((*it).parameter)) {
+            auto param = (*it).parameter;
+
+            if (param == 90) {
+                state.distance_mode = parse_distance_mode::absolute;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 91) {
+                state.distance_mode = parse_distance_mode::incremental;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 90.1) {
+                state.arc_distance_mode = parse_distance_mode::absolute;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 91.1) {
+                state.arc_distance_mode = parse_distance_mode::incremental;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 21) {
+                state.unit = parse_unit::metric;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 20) {
+                state.unit = parse_unit::imperial;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 17) {
+                state.plane = arc_plane::xy;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 18) {
+                state.plane = arc_plane::zx;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 19) {
+                state.plane = arc_plane::yz;
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 4) {
+                auto next = it;
+                next++;
+                if (next != words.end() && (*next).command == 'P') {
+                    if ((*next).parameter < 0) {
+                        warnings.push_back("dwell time must be positive. (line " + std::to_string(line_number) + ")");
+                    }
+
+                    commands.push_back(new dwell_cmd{std::abs((*next).parameter), line_number});
+                    it = words.erase(it);
+                    it = words.erase(it);
+                    continue;
+                }
+            }
+
+            // we do not support G64, only G61
+            if (param == 64) {
+                warnings.push_back("G64 not supported. Ignoring. (line " + std::to_string(line_number) + ")");
+                auto next = it;
+                next++;
+                if (next != words.end() && (*next).command == 'P') {
+                    it = words.erase(it);
+                }
+                it = words.erase(it);
+                continue;
+            }
+
+            if (param == 94 || param == 93 || param == 95) {
+                // G93 sets feed rate mode to inverse time
+                // G94 sets feed rate mode to units per minute (mm/min or inch/min)
+                // G95 sets feed rate mode to units per revolution
+                it = words.erase(it);
+                continue;
+            }
+
+            // coordinate system offset
+            if (param == 54 || param == 55 || param == 56 || param == 57) {
+                it = words.erase(it);
+                continue;
+            }
+
+
+            warnings.push_back(
+                    "ignoring unknown command " + (*it).to_string() + ". (line " + std::to_string(line_number) + ")");
+            it = words.erase(it);
+        }
+
+        if (words.empty())
+            return;
+
+        int MotionMode = state.last_motion_mode;
+        if (words.begin()->command == 'G') {
+            MotionMode = (int) words.begin()->parameter;
+            state.last_motion_mode = MotionMode;
+            words.erase(words.begin());
+        }
+
+        if (MotionMode < 0) {
+            throw parse_exception("no motion mode active", line_number);
+        }
+
+        double unit_multiplier = (state.unit == parse_unit::metric) ? 1 : 25.4;
+        glm::vec<3, double> end_pos = state.position;
+        auto StartValid = state.position_valid[0] && state.position_valid[1] && state.position_valid[2];
+        if (state.distance_mode == parse_distance_mode::incremental && !StartValid) {
+            throw parse_exception(
+                    "incremental motion is only allowed after an absolute position has been established (eg. with \"G90 G0 X0 Y0 Z5\")",
+                    line_number);
+        }
+
+        if ((MotionMode == 2 || MotionMode == 3) && !StartValid) {
+            throw parse_exception(
+                    "arcs (G2/G3) are only allowed after an absolute position has been established (eg. with \"G90 G0 X0 Y0 Z5\")",
+                    line_number);
+        }
+
+        {
+            float incremental = (state.distance_mode == parse_distance_mode::incremental) ? 1 : 0;
+
+            it = words.begin();
+            while (it != words.end()) {
+                if ((*it).command != 'X') {
+                    it++;
+                    continue;
+                }
+
+                end_pos.x = (*it).parameter * unit_multiplier + incremental * end_pos.x;
+                it = words.erase(it);
+                state.position_valid[0] = true;
+                break;
+            }
+
+            it = words.begin();
+            while (it != words.end()) {
+                if ((*it).command != 'Y') {
+                    it++;
+                    continue;
+                }
+
+                end_pos.y = (*it).parameter * unit_multiplier + incremental * end_pos.y;
+                it = words.erase(it);
+                state.position_valid[1] = true;
+                break;
+            }
+
+            it = words.begin();
+            while (it != words.end()) {
+                if ((*it).command != 'Z') {
+                    it++;
+                    continue;
+                }
+
+                end_pos.z = (*it).parameter * unit_multiplier + incremental * end_pos.z;
+                it = words.erase(it);
+                state.position_valid[2] = true;
+                break;
+            }
+        }
+
+        if (MotionMode != 0 && state.feed <= 0) {
+            throw parse_exception("feed rate undefined", line_number);
+        }
+
+        if (MotionMode == 1 && !StartValid) {
+            warnings.push_back(
+                    "a feed move is used before an absolute position is established, height maps will not be applied to this motion. (line " +
+                    std::to_string(line_number) + ")");
+        }
+
+        if (MotionMode <= 1) {
+            if (!words.empty()) {
+                warnings.push_back(
+                        "motion command must be last in line (ignoring unused words (TOOD: show serialized words)in block). (line " +
+                        std::to_string(line_number) + ")");
+            }
+
+            auto l = new line_motion_cmd;
+            l->start = state.position;
+            l->end = end_pos;
+            l->feed = state.feed;
+            l->rapid = MotionMode == 0;
+            l->line_number = line_number;
+            l->start_valid = StartValid;
+
+            std::memcpy(l->position_valid, state.position_valid, sizeof(bool) * 3);
+
+            commands.push_back(l);
+            state.position = end_pos;
+            return;
+        }
+
+        double U, V;
+        bool ijk_used = false;
+
+        switch (state.plane) {
+            case arc_plane::yz:
+                U = state.position.y;
+                V = state.position.z;
+                break;
+            case arc_plane::zx:
+                U = state.position.z;
+                V = state.position.x;
+                break;
+            default:
+                U = state.position.x;
+                V = state.position.y;
+                break;
+        }
+
+        // find IJK
+        {
+            float arc_incremental = (state.arc_distance_mode == parse_distance_mode::incremental) ? 1 : 0;
+
+            it = words.begin();
+            while (it != words.end()) {
+                if ((*it).command != 'I') {
+                    it++;
+                    continue;
+                }
+
+                switch (state.plane) {
+                    case arc_plane::xy:
+                        U = (*it).parameter * unit_multiplier + arc_incremental * state.position.x;
+                        break;
+                    case arc_plane::yz:
+                        throw parse_exception("current plane is YZ, I word is invalid", line_number);
+                    case arc_plane::zx:
+                        V = (*it).parameter * unit_multiplier + arc_incremental * state.position.x;
+                        break;
+                }
+
+                ijk_used = true;
+                it = words.erase(it);
+                break;
+            }
+
+            it = words.begin();
+            while (it != words.end()) {
+                if ((*it).command != 'J') {
+                    it++;
+                    continue;
+                }
+
+                switch (state.plane) {
+                    case arc_plane::xy:
+                        V = (*it).parameter * unit_multiplier + arc_incremental * state.position.y;
+                        break;
+                    case arc_plane::yz:
+                        U = (*it).parameter * unit_multiplier + arc_incremental * state.position.y;
+                        break;
+                    case arc_plane::zx:
+                        throw parse_exception("current plane is ZX, J word is invalid", line_number);
+                }
+
+                ijk_used = true;
+                it = words.erase(it);
+                break;
+            }
+
+            it = words.begin();
+            while (it != words.end()) {
+                if ((*it).command != 'K') {
+                    it++;
+                    continue;
+                }
+
+                switch (state.plane) {
+                    case arc_plane::xy:
+                        throw parse_exception("current plane is XY, K word is invalid", line_number);
+                        break;
+                    case arc_plane::yz:
+                        V = (*it).parameter * unit_multiplier + arc_incremental * state.position.z;
+                        break;
+                    case arc_plane::zx:
+                        U = (*it).parameter * unit_multiplier + arc_incremental * state.position.z;
+                        break;
+                }
+
+                ijk_used = true;
+                it = words.erase(it);
+                break;
+            }
+
+            // resolve radius
+            it = words.begin();
+            while (it != words.end()) {
+                if ((*it).command != 'R') {
+                    it++;
+                    continue;
+                }
+
+                if (ijk_used) {
+                    throw parse_exception("both IJK and R notation used", line_number);
+                }
+
+                if (state.position == end_pos) {
+                    throw parse_exception("arcs in R-notation must have non-coincident start and end points",
+                                          line_number);
+                }
+
+                double radius = (*it).parameter * unit_multiplier;
+                if (radius == 0)
+                    throw parse_exception("radius can't be zero", line_number);
+
+                double A, B;
+                switch (state.plane) {
+                    case arc_plane::yz:
+                        A = end_pos.y;
+                        B = end_pos.z;
+                        break;
+                    case arc_plane::zx:
+                        A = end_pos.z;
+                        B = end_pos.x;
+                        break;
+                    default:
+                        A = end_pos.x;
+                        B = end_pos.y;
+                        break;
+                }
+
+                A -= U;     //(AB) = vector from start to end of arc along the axes of the current plane
+                B -= V;
+
+                // see grbl/gcode.c
+                double h_x2_div_d = 4.0 * (radius * radius) - (A * A + B * B);
+                if (h_x2_div_d < 0) {
+                    throw parse_exception("arc radius too small to reach both ends", line_number);
+                }
+
+                h_x2_div_d = -sqrt(h_x2_div_d) / sqrt(A * A + B * B);
+                if (MotionMode == 3 ^ radius < 0) {
+                    h_x2_div_d = -h_x2_div_d;
+                }
+
+                U += 0.5 * (A - (B * h_x2_div_d));
+                V += 0.5 * (B + (A * h_x2_div_d));
+
+                it = words.erase(it);
+                break;
+            }
+
+            if (!words.empty()) {
+                warnings.push_back(
+                        "motion command must be last in line (ignoring unused words (add serialized words here) in block). (line " +
+                        std::to_string(line_number) + ")");
+            }
+
+            auto a = new arc_motion_cmd;
+            a->start = state.position;
+            a->end = end_pos;
+            a->feed = state.feed;
+            a->direction = (MotionMode == 2) ? arc_direction::cw : arc_direction::ccw;
+            a->u = U;
+            a->v = V;
+            a->line_number = line_number;
+            a->plane = state.plane;
+
+            commands.push_back(a);
+            state.position = end_pos;
+            return;
+        }
+
+    }
+}
+
+grbl::grbl_parser::grbl_parser() {
+    reset();
+}
+
+void grbl::grbl_parser::reset() {
+    state = parser_state{};
+}

gcode_parser.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include <string>
+#include <vector>
+#include <memory>
+#include "glm/vec3.hpp"
+#include "string_utils.h"
+#include <istream>
+#include <regex>
+#include <iostream>
+#include <array>
+#include <cstring>
+#include <cmath>
+#include "gcode_commands.h"
+
+namespace grbl {
+
+    // direct translation from OpenCNCPilot
+
+    enum class parse_distance_mode {
+        absolute,
+        incremental
+    };
+
+    enum class parse_unit {
+        metric,
+        imperial
+    };
+
+    struct parser_state {
+        glm::vec<3, double> position = {0, 0, 0};
+
+        // true if the position for this coordinate was previously specified in absolute terms, to prevent the start point of (0, 0, 0) to influence the output file
+        bool position_valid[3] = {false, false, false};
+
+        arc_plane plane = arc_plane::xy;
+        double feed = 0;
+        parse_distance_mode distance_mode = parse_distance_mode::absolute;
+        parse_distance_mode arc_distance_mode = parse_distance_mode::incremental;
+        parse_unit unit = parse_unit::metric;
+        int last_motion_mode = -1;
+    };
+
+
+    struct parse_exception : public std::exception {
+        parse_exception(const std::string &reason, size_t lineNumber);
+        const char *what() const noexcept override;
+
+        std::string reason;
+        size_t line_number;
+    };
+
+    struct grbl_parser {
+        parser_state state;
+        std::vector<command *> commands;
+        std::vector<std::string> warnings;
+
+        grbl_parser();
+        void reset();
+        void parse(std::istream &in);
+        void parse(std::string line, int line_number);
+
+    private:
+        static std::string cleanup_line(std::string line, int line_number);
+        static bool is_motion_command(double param);
+    };
+
+}

grbl.cpp

@@ -1,5 +1,7 @@
 #include "grbl.h"
 #include "string_utils.h"
+#include "gcode_parser.h"
+#include "gcode_file.h"
 
 #include <utility>
 #include <fstream>
@@ -38,7 +40,7 @@ grbl::program::program(std::string filename) {
 static auto comment_re = std::regex(R"(^\s*\(([^)]*)\)\s*$)");
 static auto gcode_re = std::regex(R"(([a-zA-Z0-9\s.\-]+)\s*(;.*|\(([^)]*)\))?)");
 
-bool grbl::program::load_from_stream(std::istream& in) {
+bool grbl::program::load_from_stream(std::istream &in) {
     instructions.clear();
 
     is_loaded = true;
@@ -74,7 +76,7 @@ bool grbl::program::load_from_stream(std::istream& in) {
     return is_loaded;
 }
 
-bool grbl::program::load_from_string(const std::string& content) {
+bool grbl::program::load_from_string(const std::string &content) {
     std::stringstream in_stream;
     in_stream << content;
 
@@ -95,7 +97,7 @@ bool grbl::program::load_from_file(std::string path) {
     return is_loaded;
 }
 
-std::ostream& operator<<(std::ostream& out, const grbl::instruction_type& t) {
+std::ostream &operator<<(std::ostream &out, const grbl::instruction_type &t) {
     switch (t) {
         case grbl::instruction_type::gcode:
             out << "gcode";
@@ -110,13 +112,118 @@ std::ostream& operator<<(std::ostream& out, const grbl::instruction_type& t) {
     return out;
 }
 
-std::ostream& operator<<(std::ostream& out, const grbl::instruction& i) {
+std::ostream &operator<<(std::ostream &out, const grbl::instruction &i) {
-    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment << " }";
+    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment
+        << " }";
     return out;
 }
 
-void grbl::program::dump(std::ostream& out) {
+void grbl::program::dump(std::ostream &out) {
-    for (auto& i: instructions) {
+    for (auto &i: instructions) {
         out << i << std::endl;
     }
 }
+
+grbl::program grbl::program::apply_heightmap(grbl::heightmap& grid) {
+
+    double segmentLength = grid.resolution;
+
+    grbl::program result;
+
+    std::vector<command*> new_commands;
+
+    grbl::grbl_parser parser;
+    std::ifstream in_file{filename};
+    if (in_file) {
+        parser.parse(in_file);
+
+        for (auto &c: parser.commands) {
+            auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
+            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
+            if (line != nullptr) {
+
+                // do not split up or modify any lines that are rapid or not fully defined
+                if (!line->start_valid ||
+                    (!line->position_valid[0] || !line->position_valid[1] || !line->position_valid[2]) || line->rapid) {
+                    new_commands.push_back(line);
+                    continue;
+                }
+
+                for (auto &m: line->split(segmentLength)) {
+                    m->start.z += grid.get_z_at(m->start.x, m->start.y);
+                    m->end.z += grid.get_z_at(m->end.x, m->end.y);
+
+                    new_commands.push_back(m);
+                }
+
+            } else if (arc != nullptr) {
+                if (arc->plane != arc_plane::xy) {
+                    throw std::runtime_error(
+                            "GCode contains arcs in YZ or XZ plane (G18/19), can't apply height map. Use 'Arcs to Lines' if you really need this.");
+                }
+
+                for (auto &m: arc->split(segmentLength)) {
+                    m->start.z += grid.get_z_at(m->start.x, m->start.y);
+                    m->end.z += grid.get_z_at(m->end.x, m->end.y);
+
+                    new_commands.push_back(m);
+                }
+
+            } else {
+                new_commands.push_back(c);
+            }
+        }
+
+        grbl::grbl_file file(new_commands);
+        auto new_lines = file.get_gcode();
+        result.load_from_lines(new_lines);
+    }
+
+    return result;
+
+}
+
+bool grbl::program::load_from_lines(std::vector<std::string> lines) {
+    is_loaded = true;
+    size_t line_number = 0;
+    for (auto &line: lines) {
+        line_number++;
+        if (!line.empty()) {
+
+            std::smatch sm{};
+            if (std::regex_match(line, sm, comment_re)) {
+                auto comment = sm.str(1);
+                instructions.emplace_back(instruction::new_comment(line_number, comment));
+            } else if (std::regex_match(line, sm, gcode_re)) {
+                auto command = trim(sm.str(1));
+                auto comment = trim(sm.str(3));
+                instructions.emplace_back(instruction::new_gcode(line_number, command, comment));
+            } else {
+                std::cerr << "Failed to parse line " << line << std::endl;
+                is_loaded = false;
+            }
+        }
+    }
+
+    return is_loaded;
+}
+
+void grbl::program::save(std::string path) {
+    std::ofstream out_file{path};
+    if (out_file) {
+        for (auto &i: instructions) {
+            switch (i.type) {
+                case instruction_type::gcode:
+                    out_file << i.command;
+                    if (!i.comment.empty())
+                        out_file << " (" << i.comment << ")";
+                    out_file << std::endl;
+                    break;
+                default:
+                    break;
+            }
+
+        }
+    }
+
+}

grbl.h

@@ -3,6 +3,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include "heightmap.h"
 
 namespace grbl {
 
@@ -32,6 +33,7 @@ struct program {
     bool load_from_file(std::string filename);
     bool load_from_stream(std::istream& in);
     bool load_from_string(const std::string& content);
+    bool load_from_lines(std::vector<std::string> lines);
 
     void dump(std::ostream& out);
 
@@ -42,10 +44,11 @@ struct program {
     instruction instruction_at(size_t index) {
         return instructions.at(index);
     }
-
     bool is_loaded = false;
     std::string filename;
     std::vector<instruction> instructions{};
+    grbl::program apply_heightmap(grbl::heightmap& grid);
+    void save(std::string path);
 };
 
 }

grbl_machine.cpp

@@ -5,10 +5,6 @@
 #include "grbl_machine.h"
 #include "string_utils.h"
 
-static bool starts_with(const std::string& line, const std::string& prefix) {
-    return line.rfind(prefix, 0) == 0;
-}
-
 grbl::machine::machine() {
     pipe = new tcp_transport("192.168.5.39", 23);
     states[grbl_machine_state::disconnected] = new machine_state_connect;
@@ -16,6 +12,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);
 }
@@ -41,10 +38,15 @@ void grbl::machine::on_disconnected(grbl::transport *transport) {
     switch_to_state(grbl_machine_state::disconnected);
 }
 
-grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::realtime_status_report& result) {
+grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::realtime_status_report &result) {
 //    grbl::realtime_status_report result;
 
-    auto l = line.substr(1, -1);
+    // pin values are always reset when a report arrives
+    // if there is no value in the Pn: field then it means
+    // no pin is active
+    result.signals.value = 0;
+
+    auto l = line.substr(1, line.size() - 2);
     auto pieces = split_string(l, "|");
     for (auto i = 0; i < pieces.size(); i++) {
         if (i == 0) {
@@ -68,6 +70,38 @@ grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::r
                 auto p = split_string(elements[1], ",");
                 result.buffers_free = std::stoi(p[0]);
                 result.rx_chars_free = std::stoi(p[1]);
+            } else if (elements[0] == "Pn") {
+                for (auto &c: elements[1]) {
+                    switch (c) {
+                        case 'P':
+                            result.signals.bit.probe = true;
+                            break;
+                        case 'X':
+                            result.signals.bit.x_limit = true;
+                            break;
+                        case 'Y':
+                            result.signals.bit.y_limit = true;
+                            break;
+                        case 'Z':
+                            result.signals.bit.z_limit = true;
+                            break;
+                        case 'D':
+                            result.signals.bit.door = true;
+                            break;
+                        case 'H':
+                            result.signals.bit.hold = true;
+                            break;
+                        case 'R':
+                            result.signals.bit.soft_reset = true;
+                            break;
+                        case 'S':
+                            result.signals.bit.cycle_start = true;
+                            break;
+                        default:
+                            std::cerr << "Unknown pin value [" << c << "] when parsing status report" << std::endl;
+                            break;
+                    }
+                }
             } else {
                 // not implemented
             }
@@ -77,7 +111,7 @@ grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::r
 }
 
 
-grbl::machine_status grbl::status_from_string(const std::string& status) {
+grbl::machine_status grbl::status_from_string(const std::string &status) {
     if (status == "Idle") return machine_status::idle;
     if (status == "Run") return machine_status::run;
     if (status == "Hold") return machine_status::hold;
@@ -91,9 +125,10 @@ grbl::machine_status grbl::status_from_string(const std::string& status) {
     return machine_status::unknown;
 }
 
-void grbl::machine::check_program(const grbl::program& pgm) {
+void grbl::machine::check_program(const grbl::program &pgm) {
     running_program = pgm;
-    std::cout << "checking program (" << running_program.filename << ") with " << running_program.number_of_instructions()
+    std::cout << "checking program (" << running_program.filename << ") with "
+              << running_program.number_of_instructions()
               << " instructions" << std::endl;
     switch_to_state(grbl_machine_state::check_program);
 }
@@ -115,9 +150,10 @@ void grbl::machine::set_work_offset(std::string work_offset) {
 
 }
 
-void grbl::machine::run_program(const grbl::program& pgm, const std::string& work_offset) {
+void grbl::machine::run_program(const grbl::program &pgm, const std::string &work_offset) {
     running_program = pgm;
-    std::cout << "running program (" << running_program.filename << ") with " << running_program.number_of_instructions() << " instructions"
+    std::cout << "running program (" << running_program.filename << ") with "
+              << running_program.number_of_instructions() << " instructions"
               << " on work offset " << work_offset << std::endl;
 
     set_work_offset(work_offset);
@@ -125,7 +161,7 @@ void grbl::machine::run_program(const grbl::program& pgm, const std::string& wor
     switch_to_state(grbl_machine_state::run_program);
 }
 
-std::string grbl::status_to_string(const grbl::machine_status& status) {
+std::string grbl::status_to_string(const grbl::machine_status &status) {
     switch (status) {
         case machine_status::idle:
             return "Idle";
@@ -522,10 +558,6 @@ void grbl::machine::cancel_jog() const {
     pipe->send_single_char_command(0x85);
 }
 
-void grbl::machine::set_listener(grbl::machine_listener *l) {
-    listener = l;
-}
-
 void grbl::machine::request_unlock() {
     pipe->send("$X");
 }
@@ -560,11 +592,11 @@ void grbl::machine::switch_to_state(grbl::grbl_machine_state new_state) {
     states[state]->on_enter(this);
 }
 
-const std::map<std::string, std::string, grbl::settings_cmp>& grbl::machine::get_settings() const {
+const std::map<std::string, std::string, grbl::settings_cmp> &grbl::machine::get_settings() const {
     return settings;
 }
 
-const std::map<std::string, std::string, grbl::parameters_cmp>& grbl::machine::get_parameters() const {
+const std::map<std::string, std::string, grbl::parameters_cmp> &grbl::machine::get_parameters() const {
     return parameters;
 }
 
@@ -577,7 +609,8 @@ void grbl::machine::zero_offset(int which) {
     pipe->send("$#");
     awaiting_responses++;
     while (awaiting_responses > 0);
-    listener->on_parameters_reloaded();
+
+    push_event(std::make_shared<machine_event_parameters_reloaded>());
 }
 
 void grbl::machine::zero_offset_axis(int offset_index, int axis) {
@@ -589,7 +622,8 @@ void grbl::machine::zero_offset_axis(int offset_index, int axis) {
     pipe->send("$#");
     awaiting_responses++;
     while (awaiting_responses > 0);
-    listener->on_parameters_reloaded();
+
+    push_event(std::make_shared<machine_event_parameters_reloaded>());
 }
 
 void grbl::machine::go_to_zero(bool x, bool y, bool z) {
@@ -645,6 +679,35 @@ void grbl::machine::request_jog_fixed(grbl::jog_direction dir, float distance, f
     awaiting_responses++;
 }
 
+void grbl::machine::start_z_probe(float min_z, float feed_rate) {
+    std::string command = "G38.2 Z" + std::to_string(min_z) + "F" + std::to_string(feed_rate);
+    pipe->send(command);
+    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);
+}
+
+void grbl::machine::push_event(std::shared_ptr<machine_event> event) {
+    std::scoped_lock<std::mutex> lock(event_mutex);
+    events.push_back(event);
+}
+
+std::shared_ptr<grbl::machine_event> grbl::machine::pop_event() {
+    // TODO: make this more efficient
+    std::scoped_lock<std::mutex> lock(event_mutex);
+    if (events.empty())
+        return nullptr;
+    else {
+        auto result = events.front();
+        events.pop_front();
+        return result;
+    }
+}
+
 bool grbl::jog_state::no_jogging() const {
     return !(up_pressed || down_pressed || left_pressed || right_pressed || z_up_pressed || z_down_pressed);
 }
@@ -669,7 +732,7 @@ void grbl::machine_state_connect::on_exit(grbl::machine *m) {
 void grbl::machine_state_connect::on_line_received(std::string line) {
 //    std::cerr << "Should not get content while connecting!" << std::endl;
     if (starts_with(line, "Grbl")) {
-        cnc->listener->on_banner(line);
+        cnc->push_event(std::make_shared<machine_event_banner>(line));
         cnc->switch_to_state(grbl_machine_state::init);
     }
 }
@@ -682,7 +745,7 @@ void grbl::machine_state_init::on_enter(grbl::machine *m) {
 }
 
 void grbl::machine_state_init::on_exit(grbl::machine *m) {
-    cnc->listener->on_init_completed();
+    cnc->push_event(std::make_shared<machine_event_init_completed>());
 }
 
 void grbl::machine_state_init::on_line_received(std::string line) {
@@ -695,7 +758,8 @@ void grbl::machine_state_init::on_line_received(std::string line) {
         if (starts_with(line, "$")) {
             auto pieces = split_string(line, "=");
             cnc->settings[pieces[0]] = pieces[1];
-        } else if (starts_with(line, "[G") || starts_with(line, "[H") || starts_with(line, "[T") || starts_with(line, "[P")) {
+        } else if (starts_with(line, "[G") || starts_with(line, "[H") || starts_with(line, "[T") ||
+                   starts_with(line, "[P")) {
             line = line.substr(1, line.size() - 2);
             // TODO: some parameters have more than two :
             auto pieces = split_string(line, ":");
@@ -714,7 +778,7 @@ void grbl::machine_state_init::move_to_next_init_stage() {
     switch (init_state) {
         case init_stage::start:
             init_state = init_stage::set_work_pos;
-            cnc->pipe->send("$10=1"); // machine pos in report, please
+            cnc->pipe->send("$10=511"); // machine pos in report, please. also sensors and buffers info
             break;
         case init_stage::set_work_pos:
             init_state = init_stage::fetch_settings;
@@ -758,23 +822,38 @@ void grbl::machine_state_idle::on_line_received(std::string line) {
             cnc->awaiting_responses--;
         }
     } else if (starts_with(line, "Grbl")) {
-        cnc->listener->on_banner(line);
+        cnc->push_event(std::make_shared<machine_event_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);
+        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
     } else if (starts_with(line, "[MSG:")) {
-        cnc->listener->on_message(line.substr(5, line.size() - 6));
+        auto message = line.substr(5, line.size() - 6);
+        cnc->push_event(std::make_shared<machine_event_message>(message));
     } else if (starts_with(line, "ALARM:")) {
-        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+        auto alarm = std::stoi(line.substr(6));
+        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
     } else if (starts_with(line, "$")) {
         auto pieces = split_string(line, "=");
         cnc->settings[pieces[0]] = pieces[1];
-    } else if (starts_with(line, "[G") || starts_with(line, "[H") || starts_with(line, "[T") || starts_with(line, "[P")) {
+    } else if (starts_with(line, "[G") || starts_with(line, "[H") || starts_with(line, "[T") ||
+               starts_with(line, "[P")) {
         line = line.substr(1, line.size() - 2);
         // TODO: some parameters have more than two :
         auto pieces = split_string(line, ":");
         cnc->parameters[pieces[0]] = pieces[1];
+
+        if (starts_with(line, "PRB")) {
+            auto pieces = split_string(line, ":");
+            auto coords_as_string = split_string(pieces[1], ",");
+
+            float probe_coords[3];
+            for (auto i = 0; i < 3; i++) {
+                probe_coords[i] = std::stof(coords_as_string[i]);
+            }
+            bool probe_touched = pieces[2] == "1";
+            cnc->push_event(std::make_shared<machine_event_probe_result>(probe_touched, probe_coords));
+        }
     }
 }
 
@@ -822,9 +901,15 @@ void grbl::machine_state_check_program::move_to_next_check_stage() {
             break;
         case check_stage::disable_check_mode:
             if (check_failed) {
-                cnc->listener->on_check_completed(false, cnc->executed_instructions - 1, check_error);
+                bool success = false;
+                size_t failed_idx = cnc->executed_instructions - 1;
+                size_t error = check_error;
+                cnc->push_event(std::make_shared<machine_event_check_completed>(success, failed_idx, error));
             } else {
-                cnc->listener->on_check_completed(true, 0, 0);
+                bool success = true;
+                size_t failed_idx = 0;
+                size_t error = 0;
+                cnc->push_event(std::make_shared<machine_event_check_completed>(success, failed_idx, error));
             }
             cnc->switch_to_state(grbl_machine_state::idle);
             break;
@@ -846,15 +931,17 @@ void grbl::machine_state_check_program::on_line_received(std::string line) {
         move_to_next_check_stage();
 
     } else if (starts_with(line, "Grbl")) {
-        cnc->listener->on_banner(line);
+        cnc->push_event(std::make_shared<machine_event_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);
+        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
     } else if (starts_with(line, "[MSG:")) {
-        cnc->listener->on_message(line.substr(5, line.size() - 6));
+        auto message = line.substr(5, line.size() - 6);
+        cnc->push_event(std::make_shared<machine_event_message>(message));
     } else if (starts_with(line, "ALARM:")) {
-        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+        auto alarm = std::stoi(line.substr(6));
+        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
     }
 }
 
@@ -865,7 +952,8 @@ bool grbl::machine_state_check_program::continue_program() {
     instruction to_send;
     do {
         to_send = cnc->running_program.instruction_at(cnc->executed_instructions++);
-    } while (to_send.type != instruction_type::gcode && cnc->executed_instructions < cnc->running_program.number_of_instructions());
+    } while (to_send.type != instruction_type::gcode &&
+             cnc->executed_instructions < cnc->running_program.number_of_instructions());
 
     if (to_send.type == instruction_type::gcode) {
         cnc->pipe->send(to_send.command);
@@ -908,9 +996,15 @@ void grbl::machine_state_run_program::move_to_next_run_stage() {
         case run_stage::run_program:
             if (run_failed || !continue_program()) {
                 if (run_failed) {
-                    cnc->listener->on_run_completed(false, cnc->executed_instructions - 1, run_error);
+                    bool success = false;
+                    size_t failed_index = cnc->executed_instructions - 1;
+                    size_t error = run_error;
+                    cnc->push_event(std::make_shared<machine_event_run_completed>(success, failed_index, error));
                 } else {
-                    cnc->listener->on_run_completed(true, 0, 0);
+                    bool success = true;
+                    size_t failed_index = 0;
+                    size_t error = 0;
+                    cnc->push_event(std::make_shared<machine_event_run_completed>(success, failed_index, error));
                 }
                 cnc->switch_to_state(grbl_machine_state::idle);
             }
@@ -925,7 +1019,8 @@ bool grbl::machine_state_run_program::continue_program() {
     instruction to_send;
     do {
         to_send = cnc->running_program.instruction_at(cnc->executed_instructions++);
-    } while (to_send.type != instruction_type::gcode && cnc->executed_instructions < cnc->running_program.number_of_instructions());
+    } while (to_send.type != instruction_type::gcode &&
+             cnc->executed_instructions < cnc->running_program.number_of_instructions());
 
     if (to_send.type == instruction_type::gcode) {
         cnc->pipe->send(to_send.command);
@@ -951,14 +1046,230 @@ void grbl::machine_state_run_program::on_line_received(std::string line) {
         move_to_next_run_stage();
 
     } else if (starts_with(line, "Grbl")) {
-        cnc->listener->on_banner(line);
+        cnc->push_event(std::make_shared<machine_event_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);
+        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
     } else if (starts_with(line, "[MSG:")) {
-        cnc->listener->on_message(line.substr(5, line.size() - 6));
+        auto message = line.substr(5, line.size() - 6);
+        cnc->push_event(std::make_shared<machine_event_message>(message));
     } else if (starts_with(line, "ALARM:")) {
-        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+        auto alarm = std::stoi(line.substr(6));
+        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
     }
 }
+
+// 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->push_event(std::make_shared<machine_event_banner>(line));
+        cnc->reset_machine_state();
+    } else if (starts_with(line, "<")) {
+        cnc->last_report = parse_status_report(line, cnc->last_report);
+        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
+    } else if (starts_with(line, "[MSG:")) {
+        auto message = line.substr(5, line.size() - 6);
+        cnc->push_event(std::make_shared<machine_event_message>(message));
+    } else if (starts_with(line, "ALARM:")) {
+        auto alarm = std::stoi(line.substr(6));
+        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
+    } 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("G0 X" + std::to_string(grid->vertices[0].x) + " Y" +
+                            std::to_string(grid->vertices[0].y) + "Z0.5");
+
+            stage = heightmap_probing_stage::goto_home;
+            break;
+        case heightmap_probing_stage::goto_home:
+            cnc->pipe->send("G38.2 Z-65 F5");
+            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 Z0.3");
+            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-1 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->push_event(std::make_shared<machine_event_heightmap_probe_acquired>(grid, probed_locations));
+            }
+        }
+
+            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");
+                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.3"); // 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-1 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;
+    }
+}
+
+
+grbl::machine_event_connect::machine_event_connect() {
+    machine_event::type = machine_event_type::connected;
+}
+
+grbl::machine_event_disconnect::machine_event_disconnect() {
+    machine_event::type = machine_event_type::disconnected;
+}
+
+grbl::machine_event_report_received::machine_event_report_received(const grbl::realtime_status_report &r)
+        : report(r) {
+    machine_event::type = machine_event_type::report_received;
+}
+
+grbl::machine_event_banner::machine_event_banner(const std::string &b)
+        : banner{b} {
+    machine_event::type = machine_event_type::banner;
+}
+
+grbl::machine_event_message::machine_event_message(const std::string &m)
+        : message{m} {
+    machine_event::type = machine_event_type::message;
+}
+
+grbl::machine_event_alarm::machine_event_alarm(int code)
+        : alarm(code) {
+    machine_event::type = machine_event_type::alarm;
+}
+
+grbl::machine_event_init_completed::machine_event_init_completed() {
+    machine_event::type = machine_event_type::init_completed;
+}
+
+grbl::machine_event_run_completed::machine_event_run_completed(bool success, size_t failed_index, size_t error)
+        : success(success),
+          failed_index(failed_index),
+          error(error) {
+    machine_event::type = machine_event_type::run_completed;
+}
+
+grbl::machine_event_check_completed::machine_event_check_completed(bool success, size_t failed_idx, size_t error)
+        : success(success),
+          failed_index(failed_idx),
+          error(error) {
+    machine_event::type = machine_event_type::check_completed;
+}
+
+grbl::machine_event_settings_reloaded::machine_event_settings_reloaded() {
+    machine_event::type = machine_event_type::settings_reloaded;
+}
+
+grbl::machine_event_parameters_reloaded::machine_event_parameters_reloaded() {
+    machine_event::type = machine_event_type::parameters_reloaded;
+}
+
+grbl::machine_event_probe_result::machine_event_probe_result(bool touched, const float *coords)
+        : probe_touched(touched),
+          probe_coords{coords[0], coords[1], coords[2]} {
+    machine_event::type = machine_event_type::probe_result;
+}
+
+grbl::machine_event_heightmap_probe_acquired::machine_event_heightmap_probe_acquired(grbl::heightmap *g,
+                                                                                     size_t location)
+        : grid(g),
+          probed_location(location) {
+    machine_event::type = machine_event_type::heightmap_probe_acquired;
+}

grbl_machine.h

@@ -2,8 +2,11 @@
 
 #include <map>
 #include <unordered_map>
+#include <memory>
+#include <mutex>
 #include "grbl_communication.h"
 #include "grbl.h"
+#include "heightmap.h"
 
 namespace grbl {
 
@@ -38,7 +41,19 @@ struct realtime_status_report {
     float feed_rate = 0;
     float programmed_rpm = 0;
     float actual_rpm = 0;
-    std::string signals;
+    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;
+        } bit;
+        uint8_t value = 0;
+    } signals;
     float axis_offsets[3] = {0};
     std::string coordinate_system;
     std::string overrides;
@@ -69,6 +84,7 @@ enum class grbl_machine_state {
     run_program,
     check_program,
     idle,
+    heightmap_probing,
 };
 
 struct jog_state {
@@ -102,18 +118,92 @@ static bool operator!=(const jog_state& a, const jog_state& b) {
     return !(a == b);
 }
 
-struct machine_listener {
+enum class machine_event_type {
-    virtual void on_connected() = 0;
+    connected,
-    virtual void on_disconnected() = 0;
+    disconnected,
-    virtual void on_realtime_status_report(realtime_status_report) = 0;
+    report_received,
-    virtual void on_banner(std::string line) = 0;
+    banner,
-    virtual void on_message(std::string message) = 0;
+    message,
-    virtual void on_alarm(int alarm) = 0;
+    alarm,
-    virtual void on_init_completed() = 0;
+    init_completed,
-    virtual void on_run_completed(bool success, size_t failed_index, size_t error) = 0;
+    run_completed,
-    virtual void on_check_completed(bool success, size_t failed_index, size_t error) = 0;
+    check_completed,
-    virtual void on_settings_reloaded() = 0;
+    settings_reloaded,
-    virtual void on_parameters_reloaded() = 0;
+    parameters_reloaded,
+    probe_result,
+    heightmap_probe_acquired,
+};
+
+struct machine_event {
+    machine_event_type type;
+    // TODO: ewwww
+    virtual void omg() {}
+};
+
+struct machine_event_connect : public machine_event {
+    machine_event_connect();
+};
+
+struct machine_event_disconnect : public machine_event {
+    machine_event_disconnect();
+};
+
+struct machine_event_report_received : public machine_event {
+    explicit machine_event_report_received(const realtime_status_report& report);
+    realtime_status_report report;
+};
+
+struct machine_event_banner : public machine_event {
+    explicit machine_event_banner(const std::string& banner);
+    std::string banner;
+};
+
+struct machine_event_message : public machine_event {
+    explicit machine_event_message(const std::string& message);
+    std::string message;
+};
+
+struct machine_event_alarm : public machine_event {
+    explicit machine_event_alarm(int code);
+    int alarm;
+};
+
+struct machine_event_init_completed : public machine_event {
+    machine_event_init_completed();
+};
+
+struct machine_event_run_completed : public machine_event {
+    machine_event_run_completed(bool success, size_t failed_index, size_t error);
+    bool success;
+    size_t failed_index;
+    size_t error;
+};
+
+struct machine_event_check_completed : public machine_event {
+    machine_event_check_completed(bool success, size_t failed_idx, size_t error);
+    bool success;
+    size_t failed_index;
+    size_t error;
+};
+
+struct machine_event_settings_reloaded : public machine_event {
+    machine_event_settings_reloaded();
+};
+
+struct machine_event_parameters_reloaded : public machine_event {
+    machine_event_parameters_reloaded();
+};
+
+struct machine_event_probe_result : public machine_event {
+    machine_event_probe_result(bool touched, const float *coords);
+    bool probe_touched;
+    float probe_coords[3];
+};
+
+struct machine_event_heightmap_probe_acquired : public machine_event {
+    machine_event_heightmap_probe_acquired(heightmap *g, size_t location);
+    grbl::heightmap *grid;
+    size_t probed_location;
 };
 
 
@@ -136,6 +226,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 {
@@ -209,6 +307,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 {
@@ -235,7 +352,6 @@ struct machine : public transport_callbacks {
     machine();
     ~machine();
 
-    void set_listener(grbl::machine_listener *listener);
     void connect();
 
     void set_work_offset(std::string work_offset);
@@ -265,7 +381,21 @@ struct machine : public transport_callbacks {
     // 0 = G54, 1 = G55, etc axis 0=X, 1=Y, 2=Z
     void zero_offset_axis(int offset_index, int axis);
 
+    void start_z_probe(float min_z, float feed_rate);
+
+    void probe_heightmap(grbl::heightmap& grid);
+
+    void push_event(std::shared_ptr<machine_event> event);
+
+    // pops nullptr if queue is empty
+    std::shared_ptr<machine_event> pop_event();
+
+
 protected:
+
+    std::mutex event_mutex;
+    std::deque<std::shared_ptr<machine_event>> events;
+
     void on_connected(transport *transport) override;
     void on_disconnected(transport *transport) override;
     void on_line_received(std::string line, transport *transport) override;
@@ -277,6 +407,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;
@@ -290,7 +421,6 @@ protected:
 
     volatile size_t awaiting_responses = 0;
     realtime_status_report last_report{};
-    machine_listener *listener = nullptr;
     transport *pipe = nullptr;
     grbl_machine_state state = grbl_machine_state::disconnected;
     program running_program;

grbl_test.cpp

@@ -2,6 +2,8 @@
 
 #include "grbl.h"
 #include "grbl_machine.h"
+#include "gcode_parser.h"
+#include "glm/gtx/string_cast.hpp"
 
 TEST(grbl_program, default_state) {
     grbl::program pgm;
@@ -44,7 +46,55 @@ TEST(grbl_status_report, parse) {
     EXPECT_EQ(35, r.buffers_free);
 
     //
-    grbl::parse_status_report("<Home|MPos:0.000,0.000,0.000|Bf:35,1022|FS:0,0|Pn:Z|Ov:100,100,100>", r);
+    grbl::parse_status_report("<Home|MPos:0.000,0.000,0.000|Bf:35,1022|FS:0,0|Pn:PZ|Ov:100,100,100>", r);
     EXPECT_EQ(grbl::machine_status::home, r.status);
     EXPECT_EQ(1022, r.rx_chars_free);
+
+    EXPECT_EQ(true, r.signals.bit.probe);
+
+    EXPECT_EQ(false, r.signals.bit.x_limit);
+    EXPECT_EQ(false, r.signals.bit.y_limit);
+    EXPECT_EQ(true, r.signals.bit.z_limit);
+}
+
+TEST(grbl_parser, parse) {
+    std::string content = R"(G17 G21 G90 G94 G54
+G0 Z0.25
+X-0.5 Y0.
+Z0.1
+G01 Z0. F5.
+G02 X0. Y0.5 I0.5 J0. F2.5
+X0.5 Y0. I0. J-0.5
+X0. Y-0.5 I-0.5 J0.
+X-0.5 Y0. I0. J0.5
+G01 Z0.1 F5.
+G00 X0. Y0. Z0.25
+)";
+    std::stringstream ss;
+    ss << content;
+
+    grbl::grbl_parser parser;
+    try {
+        parser.parse(ss);
+    } catch (std::exception &e) {
+        std::cerr << e.what();
+    }
+
+    for (auto &c: parser.commands) {
+        auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
+        if (line != nullptr) {
+            std::cout << "Line from " << glm::to_string(line->start) << " to " << glm::to_string(line->end)
+                      << std::endl;
+            continue;
+        }
+
+        auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
+        if (arc != nullptr) {
+            std::cout << "Arc from " << glm::to_string(arc->start) << " to " << glm::to_string(arc->end) << std::endl;
+            continue;
+        }
+
+    }
+
+//    EXPECT_EQ(true, false);
 }

heightmap.cpp

@@ -0,0 +1,68 @@
+#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;
+}
+
+size_t grbl::heightmap::index_from_coords(float x, float y) const {
+    size_t result = 0;
+    for (auto &v: vertices) {
+        if (float_equal(v.x, x) && float_equal(v.y, y)) {
+            return result;
+        }
+        result++;
+    }
+    return 0;
+}
+
+// bi-linear interpolation
+float grbl::heightmap::get_z_at(float x, float y) const {
+
+    // TODO: make faster by precalculating indices
+
+    double x1 = x - fmod(x - from_x, resolution);
+    double x2 = x1 + resolution;
+    double y1 = y - fmod(y - from_y, resolution);
+    double y2 = y1 + resolution;
+    double z11 = vertices[index_from_coords(x1, y1)].z;
+    double z12 = vertices[index_from_coords(x1, y2)].z;
+    double z21 = vertices[index_from_coords(x2, y1)].z;
+    double z22 = vertices[index_from_coords(x2, y2)].z;
+
+    double alpha_x = (x - x1) / (double) resolution;
+    double a = z11 + (z21 - z11) * alpha_x;
+    double b = z12 + (z22 - z12) * alpha_x;
+
+    double alpha_y = (y - y1) / (double) resolution;
+    double c = a + (b - a) * alpha_y;
+
+    return (float) c;
+}
+
+bool grbl::float_equal(float a, float b) {
+    return fabs(a - b) < 0.0001;
+}

heightmap.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <cstddef>
+#include <cmath>
+#include <glm/vec3.hpp>
+#include <vector>
+
+namespace grbl {
+
+    bool float_equal(float a, float b);
+
+    struct heightmap {
+        static heightmap from_params(float from_x, float from_y, float to_x, float to_y, float resolution);
+        float get_z_at(float x, float y) const;
+        size_t index_from_coords(float x, float y) const;
+
+        float from_x, from_y;
+        float to_x, to_y;
+        float resolution;
+        size_t x_segments, y_segments;
+        std::vector<glm::vec3> vertices;
+    };
+
+}

heightmap_test.cpp

@@ -0,0 +1,49 @@
+#include <gtest/gtest.h>
+
+#include "heightmap.h"
+
+TEST(heightmap, get_z_at) {
+    auto grid = grbl::heightmap::from_params(0, 0, 1, 1, 1);
+    grid.vertices[0].z = 0;
+    grid.vertices[1].z = 1;
+    grid.vertices[2].z = 2;
+    grid.vertices[3].z = 3;
+
+    auto z = grid.get_z_at(0.5f, 0.5f);
+    EXPECT_EQ(1.5, z);
+
+    z = grid.get_z_at(0.5f, 0);
+    EXPECT_EQ(0.5, z);
+
+    z = grid.get_z_at(0.5f, 1);
+    EXPECT_EQ(2.5, z);
+
+    z = grid.get_z_at(0, 0.5f);
+    EXPECT_EQ(1, z);
+
+    z = grid.get_z_at(1, 0.5f);
+    EXPECT_EQ(2, z);
+}
+
+TEST(heightmap, get_z_at_non_zero_origin) {
+    auto grid = grbl::heightmap::from_params(0 - 0.123, 0 + 0.234, 1 - 0.123, 1 + 0.234, 1);
+    grid.vertices[0].z = 0;
+    grid.vertices[1].z = 1;
+    grid.vertices[2].z = 2;
+    grid.vertices[3].z = 3;
+
+    auto z = grid.get_z_at(0.5f - 0.123f, 0.5f + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(1.5, z));
+
+    z = grid.get_z_at(0.5f - 0.123f, 0 + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(0.5, z));
+
+    z = grid.get_z_at(0.5f - 0.123f, 1 + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(2.5, z));
+
+    z = grid.get_z_at(0 - 0.123f, 0.5f + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(1, z));
+
+    z = grid.get_z_at(1 - 0.123f, 0.5f + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(2, z));
+}

main.cpp

@@ -1,727 +1,7 @@
-#include <nanogui/opengl.h>
+#include "sender_app.h"
-#include <nanogui/screen.h>
-#include <nanogui/window.h>
-#include <nanogui/layout.h>
-#include <nanogui/label.h>
-#include <nanogui/button.h>
-#include <nanogui/popupbutton.h>
-#include <nanogui/progressbar.h>
-#include <nanogui/messagedialog.h>
-#include <nanogui/texture.h>
-#include <nanogui/textarea.h>
-#include <nanogui/textbox.h>
-#include <nanogui/tabwidget.h>
-#include <nanogui/shader.h>
-#include <nanogui/renderpass.h>
-#include <iostream>
-#include <memory>
-
-#define STB_IMAGE_STATIC
-#define STB_IMAGE_IMPLEMENTATION
-#if defined(_MSC_VER)
-#  pragma warning (disable: 4505) // don't warn about dead code in stb_image.h
-#elif defined(__GNUC__)
-#   pragma GCC diagnostic ignored "-Wunused-function"
-#endif
-
-#include <stb_image.h>
-#include "grbl.h"
-#include <gtest/gtest.h>
-#include <regex>
-#include "grbl_machine.h"
-#include "string_utils.h"
-#include "render.h"
-#include "glm/gtx/quaternion.hpp"
-#include "nanogui/nanogui.h"
-#include <glm/ext/quaternion_float.hpp>
-#include <glm/ext/quaternion_trigonometric.hpp>
-#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;
-
-grbl::machine cnc{};
-
-class SenderApp : public Screen, public grbl::machine_listener {
-public:
-
-    Window *window;
-    grbl::jog_state jog;
-    TextBox *txtStatus;
-    TextBox *txtMessage;
-    nanogui::Color colRed = nanogui::Color(255, 0, 0, 255);
-    nanogui::Color colGreen = nanogui::Color(0, 255, 0, 255);
-    nanogui::Color colBg;
-    int last_alarm = 0;
-    grbl::program pgm;
-    Button *btnLoadProgram, *btnCheckProgram, *btnRunProgram;
-
-    std::vector<std::string> jog_distances = {"0.01", "0.1", "1", "10"};
-    std::vector<std::string> jog_feed_rates = {"5", "100", "500", "1000"};
-
-    grbl::program_renderer renderer;
-
-    glm::vec3 cam_target = glm::vec3(0);
-    float cam_zoom = 0;
-    glm::quat cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0); // identity quaternion
-
-    TabWidget *tab_widget;
-    VScrollPanel *settings_vscroll;
-    Widget *settings_layer;
-
-    VScrollPanel *parameters_vscroll;
-    Widget *parameters_layer;
-    TextBox *mpos_x_text, *mpos_y_text, *mpos_z_text;
-    ComboBox *cbo_work_offset, *cbo_tool, *cbo_jog_feed_rates, *cbo_jog_distance;
-    Button *btn_keyboard_jog;
-
-    SenderApp() : Screen(Vector2i(1024, 768), "GRBL Sender") {
-        inc_ref();
-
-        // save regular button color
-        colBg = theme()->m_button_gradient_bot_focused;
-
-        // create main window
-        window = new Window(this, "Machine status");
-//        window->set_fixed_height((Screen::size().y() - 40) / 2);
-        window->set_position(Vector2i(0, 0));
-        window->set_layout(new BoxLayout(nanogui::Orientation::Vertical));
-
-        tab_widget = window->add<TabWidget>();
-        tab_widget->set_callback([&](int index) {
-            tab_widget->set_selected_index(index);
-        });
-
-        tab_widget->set_fixed_height((this->height() - 80) / 2);
-
-        Widget *layer = new Widget(tab_widget);
-        layer->set_layout(new GroupLayout(10, 20, 30, 0));
-        tab_widget->append_tab("Info", layer);
-
-        layer->add<Label>("");
-        auto status_holder = layer->add<Widget>();
-        status_holder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 0, 0));
-
-        auto status_text_holder = status_holder->add<Widget>();
-        status_text_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, nanogui::Alignment::Fill, 0, 0));
-
-        status_text_holder->add<Label>("Status", "sans-bold", 20);
-
-        txtStatus = status_text_holder->add<TextBox>();
-        txtStatus->set_editable(false);
-        txtStatus->set_fixed_width(300);
-
-        auto *btnReset = status_text_holder->add<Button>("Reset");
-        btnReset->set_background_color(colRed);
-        btnReset->set_callback([&] {
-            cnc.request_reset();
-        });
-
-        //
-        status_text_holder->add<Label>("");
-
-        txtMessage = status_text_holder->add<TextBox>("");
-        txtMessage->set_fixed_width(300);
-
-        status_text_holder->add<Label>("");
-
-        // buttons
-        auto status_btn_holder = status_holder->add<Widget>();
-        status_btn_holder->set_layout(new GridLayout(Orientation::Horizontal, 2, Alignment::Fill, 6, 6));
-
-        auto *btnHome = new Button(status_btn_holder, "Home", FA_HOME);
-        btnHome->set_callback([&] {
-            cnc.request_home();
-        });
-
-        auto *btnUnlock = new Button(status_btn_holder, "Unlock", FA_UNLOCK);
-        btnUnlock->set_callback([&] {
-            cnc.request_unlock();
-        });
-
-        // buttons to change state
-        auto *btnCycleStart = status_btn_holder->add<Button>("Cycle Start", FA_PLAY);
-        btnCycleStart->set_callback([&] {
-            cnc.request_cycle_start();
-        });
-        auto *btnFeedHold = status_btn_holder->add<Button>("Feed Hold", FA_PAUSE);
-        btnFeedHold->set_callback([&] {
-            cnc.request_feed_hold();
-        });
-
-        // DRO
-        layer->add<Label>("DRO", "sans-bold", 20);
-        Widget *mpos = new Widget(layer);
-        mpos->set_layout(new GridLayout(Orientation::Horizontal, 1, Alignment::Middle, 0, 6));
-
-        auto x_holder = mpos->add<Widget>();
-        x_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 6));
-        auto lbl = x_holder->add<Label>("X", "sans-bold", 20);
-        lbl->set_fixed_width(30);
-        mpos_x_text = x_holder->add<TextBox>(std::to_string(cnc.get_status().machine_pos[0]));
-        mpos_x_text->set_fixed_width(200);
-        auto zero_btn = x_holder->add<ToolButton>(FA_BAN);
-        zero_btn->set_flags(Button::Flags::NormalButton);
-        zero_btn->set_tooltip("Zero axis");
-        zero_btn->set_callback([&]() {
-            cnc.zero_offset_axis(cbo_work_offset->selected_index(), 0);
-        });
-
-        auto y_holder = mpos->add<Widget>();
-        y_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 6));
-        lbl = y_holder->add<Label>("Y", "sans-bold", 20);
-        lbl->set_fixed_width(30);
-        mpos_y_text = y_holder->add<TextBox>(std::to_string(cnc.get_status().machine_pos[1]));
-        mpos_y_text->set_fixed_width(200);
-        zero_btn = y_holder->add<ToolButton>(FA_BAN);
-        zero_btn->set_flags(Button::Flags::NormalButton);
-        zero_btn->set_tooltip("Zero axis");
-        zero_btn->set_callback([&]() {
-            cnc.zero_offset_axis(cbo_work_offset->selected_index(), 1);
-        });
-
-        auto z_holder = mpos->add<Widget>();
-        z_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 6));
-        lbl = z_holder->add<Label>("Z", "sans-bold", 20);
-        lbl->set_fixed_width(30);
-        mpos_z_text = z_holder->add<TextBox>(std::to_string(cnc.get_status().machine_pos[0]));
-        mpos_z_text->set_fixed_width(200);
-        zero_btn = z_holder->add<ToolButton>(FA_BAN);
-        zero_btn->set_flags(Button::Flags::NormalButton);
-        zero_btn->set_tooltip("Zero axis");
-        zero_btn->set_callback([&]() {
-            cnc.zero_offset_axis(cbo_work_offset->selected_index(), 2);
-        });
-
-        auto axis_btns_holder = mpos->add<Widget>();
-        axis_btns_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 6));
-
-        auto zero_all_btn = axis_btns_holder->add<Button>("Zero all");
-        zero_all_btn->set_callback([&]() {
-            cnc.zero_offset(cbo_work_offset->selected_index());
-        });
-
-        auto goto_zero_btn = axis_btns_holder->add<Button>("Goto 0");
-        goto_zero_btn->set_callback([&]() {
-            cnc.go_to_zero(true, true, true);
-        });
-
-        // jogging
-        layer->add<Label>("Jogging", "sans-bold", 20);
-        auto holder = new Widget(layer);
-
-        holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 0));
-
-        // feed and distance
-        auto jogParamsHolder = holder->add<Widget>();
-        jogParamsHolder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 4));
-
-        jogParamsHolder->add<Label>("Feed", "sans-bold");
-        cbo_jog_feed_rates = jogParamsHolder->add<ComboBox>(jog_feed_rates);
-        cbo_jog_feed_rates->set_selected_index(2);
-        jogParamsHolder->add<Label>("mm/min");
-
-        jogParamsHolder->add<Label>("Distance", "sans-bold");
-        cbo_jog_distance = jogParamsHolder->add<ComboBox>(jog_distances);
-        cbo_jog_distance->set_selected_index(2);
-        jogParamsHolder->add<Label>("mm");
-
-        jogParamsHolder->add<Label>("Keyboard Jog", "sans-bold");
-        btn_keyboard_jog = jogParamsHolder->add<Button>("", FA_KEYBOARD);
-        btn_keyboard_jog->set_flags(Button::Flags::ToggleButton);
-        jogParamsHolder->add<Label>("");
-
-        // X and Y
-        auto jogBtnsHolder = holder->add<Widget>();
-        jogBtnsHolder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 6));
-
-        jogBtnsHolder->add<Label>("");
-        auto jog_btn = jogBtnsHolder->add<Button>("", FA_ARROW_ALT_CIRCLE_UP);
-        jog_btn->set_callback([&]() {
-            // FIXME: get rid of std::stof
-            cnc.request_jog_fixed(grbl::jog_direction::y_up,
-                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
-                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
-        });
-        jogBtnsHolder->add<Label>("");
-
-        jog_btn = jogBtnsHolder->add<Button>("", FA_ARROW_ALT_CIRCLE_LEFT);
-        jog_btn->set_callback([&]() {
-            // FIXME: get rid of std::stof
-            cnc.request_jog_fixed(grbl::jog_direction::x_down,
-                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
-                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
-        });
-
-        jog_btn = jogBtnsHolder->add<Button>("", FA_HOME);
-        jog_btn->set_callback([&]() {
-            cnc.go_to_zero(true, true, false);
-        });
-
-
-        jog_btn = jogBtnsHolder->add<Button>("", FA_ARROW_ALT_CIRCLE_RIGHT);
-        jog_btn->set_callback([&]() {
-            // FIXME: get rid of std::stof
-            cnc.request_jog_fixed(grbl::jog_direction::x_up,
-                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
-                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
-        });
-
-        jogBtnsHolder->add<Label>("");
-        jog_btn = jogBtnsHolder->add<Button>("", FA_ARROW_ALT_CIRCLE_DOWN);
-        jog_btn->set_callback([&]() {
-            // FIXME: get rid of std::stof
-            cnc.request_jog_fixed(grbl::jog_direction::y_down,
-                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
-                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
-        });
-        jogBtnsHolder->add<Label>("");
-
-
-        // z up/down
-        auto jogZBtnsHolder = holder->add<Widget>();
-        jogZBtnsHolder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 0, 6));
-
-        jog_btn = jogZBtnsHolder->add<Button>("", FA_CHEVRON_UP);
-        jog_btn->set_callback([&]() {
-            // FIXME: get rid of std::stof
-            cnc.request_jog_fixed(grbl::jog_direction::z_up,
-                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
-                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
-        });
-
-        jog_btn = jogZBtnsHolder->add<Button>("", FA_HOME);
-        jog_btn->set_callback([&]() {
-            cnc.go_to_zero(0, 0, true);
-        });
-
-        jog_btn = jogZBtnsHolder->add<Button>("", FA_CHEVRON_DOWN);
-        jog_btn->set_callback([&]() {
-            // FIXME: get rid of std::stof
-            cnc.request_jog_fixed(grbl::jog_direction::z_down,
-                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
-                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
-        });
-
-        // work parameters
-        layer->add<Label>("Work parameters", "sans-bold", 20);
-        auto work_holder = layer->add<Widget>();
-        work_holder->set_layout(new BoxLayout(nanogui::Orientation::Horizontal, Alignment::Fill, 6, 6));
-
-        auto work_data_holder = work_holder->add<Widget>();
-        work_data_holder->set_layout(new GridLayout(Orientation::Horizontal, 2, Alignment::Fill, 6, 6));
-
-        work_data_holder->add<Label>("Offset", "sans-bold");
-        std::vector<std::string> offset_items = {"G54", "G55", "G56", "G57", "G58", "G59"};
-        cbo_work_offset = work_data_holder->add<ComboBox>(offset_items);
-        cbo_work_offset->set_callback([&](int idx) {
-            refresh_offset();
-            update_dro();
-        });
-
-        work_data_holder->add<Label>("Tool", "sans-bold");
-        std::vector<std::string> tool_items = {"None"};
-        cbo_tool = work_data_holder->add<ComboBox>(tool_items);
-
-        auto work_btns_holder = work_holder->add<Widget>();
-        work_btns_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 6));
-
-
-        btnLoadProgram = work_btns_holder->add<Button>("Load");
-        btnLoadProgram->set_callback([&] {
-            auto path = file_dialog(
-                    {{"gcode", "G-Code files"},
-                     {"nc",    "G-Code files"},
-                     {"ngc",   "G-Code files"}}, true);
-
-//            btnCheckProgram->set_background_color(colBg);
-//            btnRunProgram->set_background_color(colBg);
-
-            if (pgm.load_from_file(path)) {
-//                btnCheckProgram->set_enabled(true);
-//                btnRunProgram->set_enabled(true);
-                this->init_program_geometry();
-            } else {
-//                btnCheckProgram->set_enabled(false);
-//                btnRunProgram->set_enabled(false);
-            }
-        });
-        btnLoadProgram->set_tooltip("Load program");
-
-        btnCheckProgram = work_btns_holder->add<Button>("Check");
-//        btnCheckProgram->set_enabled(false);
-        btnCheckProgram->set_callback([&] {
-            cnc.check_program(pgm);
-        });
-        btnCheckProgram->set_tooltip("Check program");
-
-        btnRunProgram = work_btns_holder->add<Button>("Run");
-//        btnRunProgram->set_enabled(false);
-        btnRunProgram->set_callback([&] {
-            cnc.run_program(pgm, "G" + std::to_string(cbo_work_offset->selected_index() + 54));
-        });
-        btnRunProgram->set_tooltip("Execute program");
-
-        perform_layout();
-
-        m_render_pass = new RenderPass({this});
-        m_render_pass->set_clear_color(0, Color(0.3f, 0.3f, 0.32f, 1.f));
-        m_render_pass->set_depth_test(RenderPass::DepthTest::Always, true);
-        m_render_pass->set_cull_mode(RenderPass::CullMode::Disabled);
-    }
-
-    void refresh_offset() const {
-        auto offset_name = "G" + std::to_string(cbo_work_offset->selected_index() + 54);
-        cnc.set_work_offset(offset_name);
-    }
-
-    void fill_in_parameters() {
-        parameters_vscroll = new VScrollPanel(tab_widget);
-        tab_widget->append_tab("Parameters", parameters_vscroll);
-
-        parameters_layer = new Widget(parameters_vscroll);
-        parameters_layer->set_layout(new GridLayout(Orientation::Horizontal, 1, Alignment::Middle));
-
-        auto& parameters = cnc.get_parameters();
-
-        for (auto& entry: parameters) {
-            auto w = parameters_layer->add<Widget>();
-            w->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 2, 2));
-            auto x = w->add<Label>(entry.first, "sans-bold", 20);
-            x->set_fixed_width(50);
-
-            auto y = w->add<TextBox>(entry.second);
-            y->set_editable(true);
-            y->set_fixed_width(200);
-
-            auto z = w->add<ToolButton>(FA_SAVE);
-            z->set_flags(Button::Flags::NormalButton); // no toggle, please
-            z->set_tooltip("save");
-        }
-    }
-
-
-    void fill_in_settings() {
-        settings_vscroll = new VScrollPanel(tab_widget);
-        tab_widget->append_tab("Settings", settings_vscroll);
-
-        settings_layer = new Widget(settings_vscroll);
-        settings_layer->set_layout(new GridLayout(Orientation::Horizontal, 1, Alignment::Middle));
-
-        auto& settings = cnc.get_settings();
-
-        for (auto& s: settings) {
-            auto w = settings_layer->add<Widget>();
-            w->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 2, 2));
-            auto x = w->add<Label>(s.first, "sans-bold", 20);
-            x->set_fixed_width(40);
-
-            auto desc = grbl::setting_description(std::stoi(s.first.substr(1)));
-
-            auto y = w->add<TextBox>(s.second);
-            y->set_editable(true);
-            y->set_tooltip(desc);
-            y->set_fixed_width(150);
-
-            auto z = w->add<ToolButton>(FA_SAVE);
-            z->set_flags(Button::Flags::NormalButton); // no toggle, please
-            z->set_tooltip("save");
-
-            auto t = w->add<TextArea>();
-            t->set_fixed_width(200);
-            t->set_fixed_height(20);
-            t->clear();
-            t->append(desc.substr(0, desc.find_first_of('\n')));
-        }
-    }
-
-    void init_program_geometry() {
-        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_zoom = (max_pos.x - min_pos.x);
-        cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0);
-    }
-
-    bool resize_event(const Vector2i& size) override {
-        return Screen::resize_event(size);
-    }
-
-    void on_connected() override {
-    }
-
-    void on_disconnected() override {
-    }
-
-    void on_settings_reloaded() override {
-    }
-
-    void on_parameters_reloaded() override {
-        refresh_offset();
-        update_dro();
-    }
-
-
-    grbl::realtime_status_report last_report;
-
-    volatile bool machine_initialized = false;
-
-    void on_init_completed() override {
-        machine_initialized = true;
-        // need to do UI related things in UI thread
-    }
-
-    void on_realtime_status_report(grbl::realtime_status_report report) override {
-//        if (report == last_report) return;
-
-//        if (last_report.status != report.status) {
-        txtStatus->set_value(grbl::status_to_string(cnc.get_status().status));
-
-        if (cnc.get_status().status == grbl::machine_status::alarm) {
-            txtStatus->set_tooltip(grbl::alarm_to_string(last_alarm));
-        } else {
-            txtStatus->set_tooltip(cnc.get_status().sub_status);
-        }
-//        }
-
-        update_dro();
-        last_report = report;
-    }
-
-    void update_dro() {
-        auto work_pos = cnc.get_work_pos();
-        mpos_x_text->set_value(std::to_string(work_pos[0]));
-        mpos_y_text->set_value(std::to_string(work_pos[1]));
-        mpos_z_text->set_value(std::to_string(work_pos[2]));
-    }
-
-    void on_banner(std::string line) override {
-    }
-
-    void on_message(std::string message) override {
-        txtMessage->set_value(message);
-        set_caption(message);
-    }
-
-    void on_alarm(int alarm) override {
-        last_alarm = alarm;
-    }
-
-    void on_check_completed(bool success, size_t failed_index, size_t error) override {
-        btnCheckProgram->set_background_color(success ? colGreen : colRed);
-        if (success) {
-            btnRunProgram->set_enabled(true);
-            new MessageDialog(this, MessageDialog::Type::Information, "Check result", "Program checked successfully");
-        } else {
-            btnRunProgram->set_enabled(false);
-            std::stringstream ss;
-            auto i = pgm.instruction_at(failed_index);
-            ss << "Program check failed at line " << i.line << " (" << i.command << "). Error: " << error << ", "
-               << grbl::error_to_string(error);
-            new MessageDialog(this, MessageDialog::Type::Warning, "Check result", ss.str());
-        }
-    }
-
-    void on_run_completed(bool success, size_t failed_index, size_t error) override {
-        btnRunProgram->set_background_color(success ? colGreen : colRed);
-        if (success) {
-            new MessageDialog(this, MessageDialog::Type::Information, "Run result", "Program executed successfully");
-        } else {
-            std::stringstream ss;
-            auto i = pgm.instruction_at(failed_index);
-            ss << "Program execution failed at line " << i.line << " (" << i.command << "). Error: " << error << ", "
-               << grbl::error_to_string(error);
-            new MessageDialog(this, MessageDialog::Type::Warning, "Run result", ss.str());
-        }
-    }
-
-
-    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_SPACE && action == GLFW_PRESS && !window->mouse_focused()) {
-            // reset trackball rotation
-            cam_src_rotation = glm::quat(1, 0, 0, 0);
-        }
-
-        auto new_jog = jog;
-
-        if (key == GLFW_KEY_LEFT_SHIFT) {
-            if (action == GLFW_PRESS) {
-                new_jog.speed_fast_pressed = true;
-            } else if (action == GLFW_RELEASE) {
-                new_jog.speed_fast_pressed = false;
-            }
-        }
-
-        if (key == GLFW_KEY_LEFT_CONTROL) {
-            if (action == GLFW_PRESS) {
-                new_jog.speed_slow_pressed = true;
-            } else if (action == GLFW_RELEASE) {
-                new_jog.speed_slow_pressed = false;
-            }
-        }
-
-        if (key == GLFW_KEY_UP) {
-            if (action == GLFW_PRESS) {
-                new_jog.up_pressed = true;
-            } else if (action == GLFW_RELEASE) {
-                new_jog.up_pressed = false;
-            }
-        }
-
-        if (key == GLFW_KEY_DOWN) {
-            if (action == GLFW_PRESS) {
-                new_jog.down_pressed = true;
-            } else if (action == GLFW_RELEASE) {
-                new_jog.down_pressed = false;
-            }
-        }
-
-        if (key == GLFW_KEY_LEFT) {
-            if (action == GLFW_PRESS) {
-                new_jog.left_pressed = true;
-            } else if (action == GLFW_RELEASE) {
-                new_jog.left_pressed = false;
-            }
-        }
-
-        if (key == GLFW_KEY_RIGHT) {
-            if (action == GLFW_PRESS) {
-                new_jog.right_pressed = true;
-            } else if (action == GLFW_RELEASE) {
-                new_jog.right_pressed = false;
-            }
-        }
-
-        if (key == GLFW_KEY_PAGE_UP) {
-            if (action == GLFW_PRESS) {
-                new_jog.z_up_pressed = true;
-            } else if (action == GLFW_RELEASE) {
-                new_jog.z_up_pressed = false;
-            }
-        }
-
-        if (key == GLFW_KEY_PAGE_DOWN) {
-            if (action == GLFW_PRESS) {
-                new_jog.z_down_pressed = true;
-            } else if (action == GLFW_RELEASE) {
-                new_jog.z_down_pressed = false;
-            }
-        }
-
-        if (btn_keyboard_jog->pushed() && jog != new_jog) {
-            cnc.request_jog(new_jog);
-            jog = new_jog;
-        }
-
-        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;
-            bool is_rotating = button == 1;
-            bool is_panning = button == 2;
-            if (is_rotating) {
-                if (abs(rel.x()) > abs(rel.y())) {
-                    cam_src_rotation *= glm::angleAxis(rel.x() / 100.0f, glm::vec3(0, 1, 0));
-                } else {
-                    cam_src_rotation *= glm::angleAxis(rel.y() / 100.0f, glm::vec3(1, 0, 0));
-                }
-
-            } else if (is_panning) {
-//                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_zoom -= rel.y() * cam_zoom / 10.0f;
-            cam_zoom = std::max(cam_zoom, 0.1f);
-
-            return true;
-        }
-    }
-
-    void draw(NVGcontext *ctx) override {
-        // not pretty but need to do this in UI thread
-        if (machine_initialized) {
-            fill_in_settings();
-            fill_in_parameters();
-
-            perform_layout();
-
-            refresh_offset();
-            update_dro();
-            machine_initialized = false;
-        }
-
-        Widget::draw(ctx);
-    }
-
-    virtual void draw_contents() {
-        auto fb_size = framebuffer_size();
-
-        // start rendering
-        m_render_pass->resize(framebuffer_size());
-        m_render_pass->begin();
-
-        if (pgm.is_loaded) {
-            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,
-                                                    10000.f);
-
-            glm::mat4 view = glm::translate(glm::mat4(1.0f), glm::vec3(0, 0, -cam_zoom)) *
-                             glm::toMat4(cam_src_rotation) *
-                             glm::translate(glm::mat4(1.0f), -cam_target);
-
-            glm::mat4 model = glm::mat4(1.0f);
-            auto mvp = projection * view * model;
-
-            renderer.render(mvp, glm::vec2(fb_size.x(), fb_size.y()));
-        }
-
-        m_render_pass->end();
-    }
-
-private:
-    ProgressBar *m_progress;
-    ref<RenderPass> m_render_pass;
-
-    using ImageHolder = std::unique_ptr<uint8_t[], void (*)(void *)>;
-    std::vector<std::pair<ref<Texture>, ImageHolder>> m_images;
-    int m_current_image;
-};
 
 
 int main(int argc, char **argv) {
-
     testing::InitGoogleTest(&argc, argv);
     auto result = RUN_ALL_TESTS();
     if (result) {
@@ -733,19 +13,16 @@ int main(int argc, char **argv) {
 
         // scoped variables
         {
-            ref<SenderApp> app = new SenderApp();
+            nanogui::ref<SenderApp> app = new SenderApp();
             app->dec_ref();
             app->draw_all();
             app->set_visible(true);
 
-            cnc.set_listener(app);
-            cnc.connect();
-
             nanogui::mainloop(1 / 60.f * 1000);
         }
 
         nanogui::shutdown();
-    } catch (const std::exception& e) {
+    } catch (const std::exception &e) {
         std::string error_msg = std::string("Caught a fatal error: ") + std::string(e.what());
         std::cerr << error_msg << std::endl;
         return -1;

render.cpp

@@ -4,7 +4,8 @@
 #include "glm/vec3.hpp"
 #include "glm/vec4.hpp"
 #include "glm/gtc/type_ptr.hpp"
-//#include <GL/glext.h>
+#include "gcode_parser.h"
+#include <fstream>
 
 static const char *vs_code = R"(
     #version 330
@@ -54,8 +55,118 @@ static const char *ps_code = R"(
     }
 )";
 
+static const char *vs_heightmap_code = R"(
+    #version 330
 
-void grbl::program_renderer::render(glm::mat4 mvp, glm::vec2 viewport_size) {
+    layout (location = 0) in vec3 position;
+    layout (location = 1) in vec3 normal;
+    layout (location = 2) in vec4 color;
+
+    uniform mat4 mmtx;
+    uniform mat4 vmtx;
+    uniform mat4 pmtx;
+    uniform mat3 nmtx;
+
+    out vec3 eye_space_normal;
+    out vec3 eye_space_position;
+    out vec4 diffuse_color;
+
+    void main() {
+        mat4 mvp = pmtx * vmtx * mmtx;
+        gl_Position = mvp * vec4(position, 1.0);
+
+        eye_space_position = (vmtx * mmtx * vec4(position, 1)).xyz;
+        eye_space_normal = normalize(nmtx * normal);
+
+        diffuse_color = color;
+    }
+)";
+
+
+static const char *ps_heightmap_code = R"(
+    #version 330
+
+    in vec3 eye_space_normal;
+    in vec3 eye_space_position;
+    in vec4 diffuse_color;
+
+
+    out vec4 color;
+
+    void main() {
+        vec3 normal = normalize(eye_space_normal);
+        vec3 light_dir = normalize(vec3(0, 0, 1));
+        vec4 ambient_color = vec4(0.1, 0.1, 0.1, 1);
+
+        float n_dot_l = max(dot(normal, light_dir), 0.0);
+        color = n_dot_l * diffuse_color + ambient_color;
+    }
+)";
+
+static void add_vertex(std::vector<float> &buffer_data, glm::vec3 v, glm::vec3 n, glm::vec4 col) {
+    buffer_data.push_back(v.x);
+    buffer_data.push_back(v.y);
+    buffer_data.push_back(v.z);
+
+    buffer_data.push_back(n.x);
+    buffer_data.push_back(n.y);
+    buffer_data.push_back(n.z);
+
+    buffer_data.push_back(col.r);
+    buffer_data.push_back(col.g);
+    buffer_data.push_back(col.b);
+    buffer_data.push_back(col.a);
+}
+
+static void add_vertex(std::vector<float> &buffer_data, glm::vec3 v, glm::vec4 col) {
+    buffer_data.push_back(v.x);
+    buffer_data.push_back(v.y);
+    buffer_data.push_back(v.z);
+
+    buffer_data.push_back(col.r);
+    buffer_data.push_back(col.g);
+    buffer_data.push_back(col.b);
+    buffer_data.push_back(col.a);
+}
+
+static void
+add_line(std::vector<float> &buffer_data, glm::vec3 from, glm::vec4 from_col, glm::vec3 to, glm::vec4 to_col) {
+    add_vertex(buffer_data, from, from_col);
+    add_vertex(buffer_data, to, to_col);
+}
+
+static void add_line(std::vector<float> &buffer_data, glm::vec3 from, glm::vec3 to, glm::vec4 col) {
+    add_vertex(buffer_data, from, col);
+    add_vertex(buffer_data, to, col);
+}
+
+static void
+add_triangle(std::vector<float> &buffer_data, glm::vec3 p1, glm::vec3 p2, glm::vec3 p3, glm::vec3 n, glm::vec4 col) {
+    add_vertex(buffer_data, p1, n, col);
+    add_vertex(buffer_data, p2, n, col);
+    add_vertex(buffer_data, p3, n, col);
+}
+
+
+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");
+    heightmap_shader->set_mat4(glm::value_ptr(view), "vmtx");
+    heightmap_shader->set_mat4(glm::value_ptr(projection), "pmtx");
+    heightmap_shader->set_mat3(glm::value_ptr(normal_mat), "nmtx");
+
+    glBindVertexArray(heightmap_vao_id);
+    glDrawArrays(GL_TRIANGLES, 0, heightmap_vertices_count);
+
+    heightmap_shader->unbind();
+
+    auto mvp = projection * view * model;
+
+    // draw model
     shader->bind();
     shader->set_mat4(glm::value_ptr(mvp), "mvp");
     shader->set_vec2(glm::value_ptr(viewport_size), "u_resolution");
@@ -63,7 +174,7 @@ void grbl::program_renderer::render(glm::mat4 mvp, glm::vec2 viewport_size) {
     glBindVertexArray(vao_id);
     glDrawArrays(GL_LINES, 0, vertices_count);
 
-
+    // draw spindle
     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");
@@ -71,91 +182,144 @@ void grbl::program_renderer::render(glm::mat4 mvp, glm::vec2 viewport_size) {
     glBindVertexArray(spindle_vao_id);
     glDrawArrays(GL_LINES, 0, spindle_vertices_count);
 
-    shader->unbind();
+    // draw bounding box
+    auto bbox_size = max_pos - min_pos;
 
-    // draw bit location
+    auto bbox_translation = glm::translate(glm::mat4(1.0f), min_pos);
+    auto bbox_scale = glm::scale(glm::mat4(1.0f), bbox_size);
+    auto bbox_view = mvp * bbox_translation * bbox_scale;
+    shader->set_mat4(glm::value_ptr(bbox_view), "mvp");
+
+    glBindVertexArray(extents_vao_id);
+    glDrawArrays(GL_LINES, 0, extents_vertices_count);
+
+    shader->unbind();
 }
 
-void grbl::program_renderer::update(const grbl::program& pgm, const grbl::machine& cnc) {
+void grbl::program_renderer::update(const grbl::program &pgm, const grbl::machine &cnc) {
     if (!initialized) {
-
         shader = new shader_program(vs_code, ps_code);
+        heightmap_shader = new shader_program(vs_heightmap_code, ps_heightmap_code);
 
         initialize_spindle_buffers();
         initialize_program_buffers();
+        initialize_extents_buffers();
+        initialize_heightmap_buffers();
 
         initialized = true;
     }
 
     // update program with machine status
     // build vbo and vao
-    vertices_count = build_vbo(pgm);
+    vertices_count = update_model_vbo(pgm);
 
-    auto work_pos = cnc.get_status().machine_pos;
+    auto machine_pos = glm::vec3{cnc.get_status().machine_pos[0],
-    auto offsets = cnc.get_current_work_offset_values();
+                                 cnc.get_status().machine_pos[1],
-    for (int i = 0; i < 3; i++) {
+                                 cnc.get_status().machine_pos[2]};
-        work_pos[i] -= offsets[i];
+    auto offsets = glm::vec3{cnc.get_current_work_offset_values()[0],
-    }
+                             cnc.get_current_work_offset_values()[1],
-
+                             cnc.get_current_work_offset_values()[2]};
-    spindle_pos = glm::vec3(work_pos[0], work_pos[1], work_pos[2]);
+    spindle_pos = machine_pos - offsets;
 }
 
+
 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;
+    const GLsizei size_of_vertex_in_bytes = 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);
+    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) nullptr);
 
     glEnableVertexAttribArray(1); // vertex colors on stream 1
-    glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, sizeOfVertexInBytes, (void *) (sizeof(float) * 3));
+    glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) (sizeof(float) * 3));
 
     // unbind vao
     glBindVertexArray(0);
 
     // construct spindle model
-    glm::vec4 col(1, 1, 0, 1); // yellow
+    glm::vec4 col(1, 1, 0, 1);
 
     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 x = sinf((i / (float) cone_granularity) * 2.0f * glm::pi<float>());
-        float y = cosf((i / (float) cone_granularity) * 2.0f * M_PI);
+        float y = cosf((i / (float) cone_granularity) * 2.0f * glm::pi<float>());
 
-        // from
+        add_line(buffer_data, {0, 0, 0}, {x, y, spindle_length}, col);
-        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;
+    spindle_vertices_count = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
+}
+
+void grbl::program_renderer::initialize_extents_buffers() {
+    glGenBuffers(1, &extents_vbo_id);
+    glGenVertexArrays(1, &extents_vao_id);
+
+    // vertex format: x, y, z, r, g, b, a
+    // stride: 28 bytes
+    const GLsizei size_of_vertex_in_bytes = 28;
+
+    // we're going to draw a simple box for the extents
+    // box is made up of 8 vertices and 12 lines
+
+    glBindVertexArray(extents_vao_id);
+    glBindBuffer(GL_ARRAY_BUFFER, extents_vbo_id);
+
+    glEnableVertexAttribArray(0); // vertices on stream 0
+    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) 0);
+
+    glEnableVertexAttribArray(1); // vertex colors on stream 1
+    glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) (sizeof(float) * 3));
+
+    // unbind vao
+    glBindVertexArray(0);
+
+    // we're going to make the box as unit box and scale it when rendering
+    // since we're going to reuse the same box for all loaded programs.
+    // or at least we'll try to.
+
+    // construct box
+    glm::vec4 col(1, 0.7, 0.3, 1);
+
+
+    // box will range from [0,0,0] to [1,1,1] and we'll use
+    // translation and scaling afterward if needed to place it
+
+    std::vector<float> buffer_data;
+    // bottom plane
+    add_line(buffer_data, {0, 0, 0}, {1, 0, 0}, col);
+    add_line(buffer_data, {1, 0, 0}, {1, 1, 0}, col);
+    add_line(buffer_data, {1, 1, 0}, {0, 1, 0}, col);
+    add_line(buffer_data, {0, 1, 0}, {0, 0, 0}, col);
+
+    // top plane
+    add_line(buffer_data, {0, 0, 1}, {1, 0, 1}, col);
+    add_line(buffer_data, {1, 0, 1}, {1, 1, 1}, col);
+    add_line(buffer_data, {1, 1, 1}, {0, 1, 1}, col);
+    add_line(buffer_data, {0, 1, 1}, {0, 0, 1}, col);
+
+    // add vertical lines connecting the planes
+    add_line(buffer_data, {0, 0, 0}, {0, 0, 1}, col);
+    add_line(buffer_data, {1, 0, 0}, {1, 0, 1}, col);
+    add_line(buffer_data, {1, 1, 0}, {1, 1, 1}, col);
+    add_line(buffer_data, {0, 1, 0}, {0, 1, 1}, col);
+
+    glBindBuffer(GL_ARRAY_BUFFER, extents_vbo_id);
+    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
+
+    extents_vertices_count = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
 }
 
 void grbl::program_renderer::initialize_program_buffers() {
@@ -179,104 +343,51 @@ void grbl::program_renderer::initialize_program_buffers() {
     glBindVertexArray(0);
 }
 
-GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
+GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) {
-    static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))");
+    if (!pgm.is_loaded) return 0;
-    bool is_tool_on = false;
 
+    min_pos = glm::vec3(std::numeric_limits<float>::max());
+    max_pos = glm::vec3(std::numeric_limits<float>::min());
 
-    buffer_data.clear();
+    std::vector<float> buffer_data;
 
-    glm::vec3 tool_pos;
+    grbl::grbl_parser parser;
-    min_pos = max_pos = tool_pos = glm::vec3(0);
+    std::ifstream in_file{pgm.filename};
+    if (in_file) {
+        parser.parse(in_file);
 
-    for (auto& i: pgm.instructions) {
+        auto rapid_color = glm::vec4(1.0f);
-        if (i.type == grbl::instruction_type::gcode) {
+        auto feed_color = glm::vec4(1.0f, 0, 0, 1);
 
-            std::vector<std::string> tokens;
+        for (auto &c: parser.commands) {
-            std::smatch res;
+            auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
-            std::string::const_iterator start(i.command.cbegin());
+            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
-            while (std::regex_search(start, i.command.cend(), res, movement_re)) {
+            if (line != nullptr) {
-                auto str = res[0].str();
+                if (line->rapid) {
+                    add_line(buffer_data, line->start, rapid_color, line->end, rapid_color);
+                } else {
+                    add_line(buffer_data, line->start, feed_color, line->end, feed_color);
 
-                // make upper case
+                    update_model_extents(line->start);
-                std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+                    update_model_extents(line->end);
-
-                // 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) {
+            } else if (arc != nullptr) {
-                    if (t[0] == 'X') {
+                auto pieces = arc->split(0.1);
-                        new_pos.x = std::stof(t.substr(1));
+                for (auto &p: pieces) {
-                    } else if (t[0] == 'Y') {
+                    // transform arc to line
-                        new_pos.y = std::stof(t.substr(1));
+                    add_line(buffer_data, p->start, feed_color, p->end, feed_color);
-                    } else if (t[0] == 'Z') {
+
-                        new_pos.z = std::stof(t.substr(1));
+                    update_model_extents(p->start);
+                    update_model_extents(p->end);
                 }
             }
-
-                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;
+    const GLsizei size_of_vertex_in_bytes = 28;
-    auto number_of_vertices = buffer_data.size() * sizeof(float) / sizeOfVertexInBytes;
+    auto number_of_vertices = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
 
     glBindBuffer(GL_ARRAY_BUFFER, vbo_id);
     glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
@@ -284,6 +395,92 @@ GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
     return number_of_vertices;
 }
 
+
+void grbl::program_renderer::update_grid(const grbl::heightmap &grid, float exaggeration_factor) {
+
+    glm::vec4 color = {0.5, 0.3, 0, 1};
+
+    // this should only be called whenever the grid changes
+    // therefor it should be called each and every time we
+    // probe a new location. let's check that.
+    std::vector<float> buffer_data;
+
+    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 + grid.x_segments;
+            int bottom_next = bottom + 1;
+
+            auto p1 = grid.vertices[current];
+            auto p2 = grid.vertices[bottom];
+            auto p3 = grid.vertices[next];
+
+            // exaggerate Z
+            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 = 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);
+        }
+    }
+
+    const GLsizei size_of_vertex_in_bytes = 40;
+    heightmap_vertices_count = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
+
+    glBindBuffer(GL_ARRAY_BUFFER, heightmap_vbo_id);
+    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_DYNAMIC_DRAW);
+}
+
+void grbl::program_renderer::initialize_heightmap_buffers() {
+    glGenBuffers(1, &heightmap_vbo_id);
+    glGenVertexArrays(1, &heightmap_vao_id);
+
+    // for height map we are going to use solid rendering (triangles)
+    // we also need to send normals so that we can better see the shape of the map through lighting
+
+    // vertex format: v.x, v.y, v.z, n.x, n.y, n.z, r, g, b, a
+    // stride: 10 * 4 bytes => 40 bytes
+    const GLsizei size_of_vertex_in_bytes = 40;
+
+    glBindVertexArray(heightmap_vao_id);
+    glBindBuffer(GL_ARRAY_BUFFER, heightmap_vbo_id);
+
+    glEnableVertexAttribArray(0); // vertices on stream 0
+    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) 0);
+
+    glEnableVertexAttribArray(1); // normals on stream 1
+    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) (sizeof(float) * 3));
+
+    glEnableVertexAttribArray(2); // vertex colors on stream 2
+    glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) (sizeof(float) * 6));
+
+    // unbind vao
+    glBindVertexArray(0);
+}
+
+void grbl::program_renderer::update_model_extents(glm::vec3 point) {
+    min_pos.x = std::min(min_pos.x, point.x);
+    min_pos.y = std::min(min_pos.y, point.y);
+    min_pos.z = std::min(min_pos.z, point.z);
+
+    max_pos.x = std::max(max_pos.x, point.x);
+    max_pos.y = std::max(max_pos.y, point.y);
+    max_pos.z = std::max(max_pos.z, point.z);
+
+}
+
 std::string get_shader_info_log(GLuint id) {
     GLint log_length = 0;
     glGetShaderiv(id, GL_INFO_LOG_LENGTH, &log_length);
@@ -291,6 +488,7 @@ std::string get_shader_info_log(GLuint id) {
     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);
 }
 
@@ -301,18 +499,19 @@ std::string get_program_info_log(GLuint id) {
     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);
+    return {error_log, (size_t) 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;
+    std::cerr << "Shader compile error: " << "(id: " << shader_id << ") - " << get_shader_info_log(shader_id)
+              << std::endl;
     glDeleteShader(shader_id);
+    exit(1);
     return false;
 }
 
@@ -322,8 +521,10 @@ bool check_link_error(GLuint program_id) {
     if (is_linked == GL_TRUE)
         return true;
 
-    std::cerr << "Shader program link error: " << "(id: " << program_id << ") - " << get_program_info_log(program_id) << std::endl;
+    std::cerr << "Shader program link error: " << "(id: " << program_id << ") - " << get_program_info_log(program_id)
+              << std::endl;
     glDeleteProgram(program_id);
+    exit(1);
     return false;
 }
 

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 {
@@ -12,14 +13,16 @@ class shader_program;
 class program_renderer {
 public:
     void update(const program& pgm, const machine& cnc);
-    void render(glm::mat4 mvp, glm::vec2 viewport_size);
+    void render(glm::mat4 model, glm::mat4 view, glm::mat4 projection, glm::mat3 normal_mat, glm::vec2 viewport_size);
 
     glm::vec3 get_extents_min() const { return min_pos; };
 
     glm::vec3 get_extents_max() const { return max_pos; };
 
+    void update_grid(const grbl::heightmap& grid, float exaggeration_factor);
 private:
-    GLsizei build_vbo(const grbl::program& pgm);
+    GLsizei update_model_vbo(const grbl::program& pgm);
+    void update_model_extents(glm::vec3 point);
 
     GLuint spindle_vbo_id;
     GLuint spindle_vao_id;
@@ -27,16 +30,24 @@ private:
     GLuint vbo_id;
     GLuint vao_id;
 
-    std::vector<float> buffer_data;
+    GLuint extents_vbo_id;
+    GLuint extents_vao_id;
+
+    GLuint heightmap_vbo_id;
+    GLuint heightmap_vao_id;
+
     shader_program *shader = nullptr;
+    shader_program *heightmap_shader = nullptr;
 
     bool initialized = false;
 
     glm::vec3 min_pos, max_pos, spindle_pos;
-    GLsizei vertices_count, spindle_vertices_count;
+    GLsizei vertices_count, spindle_vertices_count, extents_vertices_count, heightmap_vertices_count;
 
     void initialize_program_buffers();
     void initialize_spindle_buffers();
+    void initialize_extents_buffers();
+    void initialize_heightmap_buffers();
 };
 
 

sender_app.h

@@ -0,0 +1,152 @@
+#pragma once
+
+#include <nanogui/opengl.h>
+#include <nanogui/screen.h>
+#include <nanogui/window.h>
+#include <nanogui/layout.h>
+#include <nanogui/label.h>
+#include <nanogui/button.h>
+#include <nanogui/popupbutton.h>
+#include <nanogui/progressbar.h>
+#include <nanogui/messagedialog.h>
+#include <nanogui/texture.h>
+#include <nanogui/textarea.h>
+#include <nanogui/textbox.h>
+#include <nanogui/tabwidget.h>
+#include <nanogui/shader.h>
+#include <nanogui/renderpass.h>
+#include <iostream>
+#include <memory>
+#include "grbl_machine.h"
+#include "render.h"
+#include "glm/glm.hpp"
+
+#define STB_IMAGE_STATIC
+#define STB_IMAGE_IMPLEMENTATION
+#if defined(_MSC_VER)
+#  pragma warning (disable: 4505) // don't warn about dead code in stb_image.h
+#elif defined(__GNUC__)
+#   pragma GCC diagnostic ignored "-Wunused-function"
+#endif
+
+#include <stb_image.h>
+#include "grbl.h"
+#include <gtest/gtest.h>
+#include <regex>
+#include "grbl_machine.h"
+#include "string_utils.h"
+#include "render.h"
+#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
+#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
+#include <fstream>
+
+class SenderApp : public nanogui::Screen {
+public:
+    SenderApp();
+
+private:
+    void add_z_probe_markup();
+    void add_work_parameters_markup();
+    void add_status_markup();
+    void add_jogging_markup();
+    void add_dro_markup();
+    void add_program_extents();
+    void add_heightmap_markup();
+    void add_pins_markup();
+
+    static void save_heightmap(const grbl::heightmap &grid, std::string path);
+    grbl::heightmap load_heightmap(std::string path);
+
+    void update_dro();
+    void update_grid();
+    void refresh_offset();
+
+    void fill_heightmap_controls_from_grid(const grbl::heightmap &grid) const;
+    void fill_heightmap_from_model() const;
+    void fill_in_parameters();
+    void fill_in_settings();
+    void init_program_geometry();
+    void set_program_extents() const;
+
+    bool resize_event(const nanogui::Vector2i &size) override;
+    bool keyboard_event(int key, int scancode, int action, int modifiers) override;
+    bool mouse_motion_event(const nanogui::Vector2i &p,
+                            const nanogui::Vector2i &rel,
+                            int button,
+                            int modifiers) override;
+    bool scroll_event(const nanogui::Vector2i &p, const nanogui::Vector2f &rel) override;
+    void draw(NVGcontext *ctx) override;
+    void draw_contents() override;
+
+    grbl::machine cnc{};
+
+    nanogui::TextBox *txt_min_z = nullptr, *txt_feed = nullptr;
+    nanogui::TextBox *txt_heightmap_from_x = nullptr, *txt_heightmap_from_y = nullptr;
+    nanogui::TextBox *txt_heightmap_to_x = nullptr, *txt_heightmap_to_y = nullptr;
+    nanogui::TextBox *txt_heightmap_res = nullptr;
+    nanogui::TextBox *txt_clearance_height = nullptr, *txt_start_probing_at = nullptr, *txt_max_negative_z = nullptr, *txt_final_z_height = nullptr;
+    nanogui::Window *window = nullptr;
+    nanogui::TextBox *txt_status = nullptr;
+    nanogui::TextBox *txt_message = nullptr;
+    nanogui::Color color_red = nanogui::Color(255, 0, 0, 255);
+    nanogui::Color color_green = nanogui::Color(0, 255, 0, 255);
+    nanogui::Button *btn_load_program = nullptr, *btn_check_program = nullptr, *btn_run_program = nullptr;
+
+    nanogui::TabWidget *tab_widget = nullptr;
+
+    nanogui::VScrollPanel *info_vscroll = nullptr;
+    nanogui::Widget *info_layer = nullptr;
+
+    nanogui::Widget *heightmap_layer = nullptr;
+
+
+    nanogui::VScrollPanel *settings_vscroll = nullptr;
+    nanogui::Widget *settings_layer = nullptr;
+
+    nanogui::VScrollPanel *parameters_vscroll = nullptr;
+    nanogui::Widget *parameters_layer = nullptr;
+
+    nanogui::TextBox *mpos_x_text = nullptr, *mpos_y_text = nullptr, *mpos_z_text = nullptr;
+    nanogui::ComboBox *cbo_work_offset = nullptr, *cbo_tool = nullptr, *cbo_jog_feed_rates = nullptr, *cbo_jog_distance = nullptr, *cbo_exaggeration = nullptr;
+
+    nanogui::ToolButton *btn_pin_door = nullptr, *btn_pin_hold = nullptr, *btn_pin_reset = nullptr, *btn_pin_cycle_start = nullptr;
+    nanogui::ToolButton *btn_pin_limit_x = nullptr, *btn_pin_limit_y = nullptr, *btn_pin_limit_z = nullptr, *btn_pin_probe = nullptr;
+
+    nanogui::Button *btn_keyboard_jog = nullptr;
+    nanogui::Button *btn_run_autoleveled = nullptr;
+    std::stringstream dro_ss;
+    grbl::heightmap heightmap_grid;
+
+    nanogui::TextBox *extents_min_x = nullptr, *extents_max_x = nullptr;
+    nanogui::TextBox *extents_min_y = nullptr, *extents_max_y = nullptr;
+    nanogui::TextBox *extents_min_z = nullptr, *extents_max_z = nullptr;
+
+    nanogui::ProgressBar *m_progress = nullptr;
+    nanogui::ref<nanogui::RenderPass> m_render_pass;
+
+    using ImageHolder = std::unique_ptr<uint8_t[], void (*)(void *)>;
+    std::vector<std::pair<nanogui::ref<nanogui::Texture>, ImageHolder>> m_images;
+
+    std::vector<std::string> jog_distances = {"0.01", "0.1", "1", "10"};
+    std::vector<std::string> jog_feed_rates = {"5", "100", "500", "1000"};
+    std::vector<std::string> exaggeration_factors = {"0", "1", "5", "10", "20", "50", "100"};
+
+    int last_alarm = 0;
+
+    grbl::program pgm{};
+    grbl::realtime_status_report last_report;
+    grbl::jog_state jog;
+    grbl::program_renderer renderer{};
+    glm::vec3 cam_target = glm::vec3(0);
+    glm::vec3 cam_pan = glm::vec3(0);
+    float cam_zoom = 0;
+    glm::quat cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0); // identity quaternion
+};

string_utils.h

@@ -1,7 +1,9 @@
 #pragma once
 
+#include <utility>
 #include <vector>
 #include <string>
+#include <algorithm>
 
 inline std::vector<std::string> split_string(const std::string& in, const std::string& delimiter) {
     std::vector<std::string> result{};
@@ -41,3 +43,38 @@ inline std::string& trim(std::string& str, const std::string& chars = "\t\n\v\f\
 inline std::string trim(std::string&& str, const std::string& chars = "\t\n\v\f\r ") {
     return ltrim(rtrim(str, chars), chars);
 }
+
+inline std::string& to_upper(std::string& str) {
+    std::transform(str.begin(), str.end(), str.begin(),
+                   [](unsigned char c) { return std::toupper(c); });
+    return str;
+}
+
+inline std::string& to_lower(std::string& str) {
+    std::transform(str.begin(), str.end(), str.begin(),
+                   [](unsigned char c) { return std::tolower(c); });
+    return str;
+}
+
+inline std::string to_upper(std::string&& str) {
+    auto result = str;
+    return to_upper(result);
+}
+
+inline std::string to_lower(std::string&& str) {
+    auto result = str;
+    return to_lower(result);
+}
+
+inline bool string_contains(const std::string& str, const std::string& needle) {
+    return str.find(needle) != std::string::npos;
+}
+
+inline std::string remove_spaces(std::string str) {
+    str.erase(std::remove(str.begin(), str.end(), ' '), str.end());
+    return str;
+}
+
+inline bool starts_with(const std::string& line, const std::string& prefix) {
+    return line.rfind(prefix, 0) == 0;
+}

terje/comms.cpp

@@ -0,0 +1,11 @@
+#include "comms.h"
+
+static iosender::StreamComms *currentCommunicator = nullptr;
+
+void iosender::Comms::SetCom(iosender::StreamComms *com) {
+    currentCommunicator = com;
+}
+
+iosender::StreamComms *iosender::Comms::GetCom() {
+    return currentCommunicator;
+}

terje/comms.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+
+namespace iosender {
+
+class StreamComms;
+
+class Comms {
+public:
+
+    enum class State {
+        AwaitAck,
+        DataReceived,
+        ACK,
+        NAK
+    };
+
+    enum class ResetMode {
+        None,
+        DTR,
+        RTS
+    };
+
+    enum class StreamType {
+        Serial,
+        Telnet,
+        Websocket
+    };
+
+    const int TXBUFFERSIZE = 4096;
+    const int RXBUFFERSIZE = 1024;
+
+    static void SetCom(StreamComms *com);
+    static StreamComms *GetCom();
+};
+
+struct DataReceivedHandler {
+    virtual void DataReceived(std::string data) = 0;
+};
+
+struct ByteReceivedHandler {
+    virtual void ByteReceived(int b) = 0;
+};
+
+class StreamComms {
+public:
+
+    virtual ~StreamComms() = default;
+
+    virtual bool IsOpen() = 0;
+    virtual int OutCount() = 0;
+    virtual std::string Reply() = 0;
+    virtual Comms::StreamType StreamType() = 0;
+    virtual bool EventMode() = 0;
+    virtual void EventMode(bool mode) = 0;
+
+    virtual void Close() = 0;
+    virtual int ReadByte() = 0;
+    virtual void WriteByte(uint8_t data) = 0;
+    virtual void WriteBytes(uint8_t *bytes, int len) = 0;
+    virtual void WriteString(std::string data) = 0;
+    virtual void WriteCommand(std::string command) = 0;
+    virtual std::string GetReply(std::string command) = 0;
+    virtual void AwaitAck() = 0;
+    virtual void AwaitAck(std::string command) = 0;
+    virtual void AwaitResponse(std::string command) = 0;
+    virtual void AwaitResponse() = 0;
+    virtual void PurgeQueue() = 0;
+
+    Comms::State CommandState = Comms::State::NAK;
+    DataReceivedHandler *dataReceivedHandler = nullptr;
+    ByteReceivedHandler *byteReceivedHandler = nullptr;
+};
+
+
+}

terje/gcode.cpp

@@ -0,0 +1,75 @@
+#include "gcode.h"
+#include "../string_utils.h"
+
+std::vector<int> iosender::ToIndices(iosender::AxisFlags flags) {
+    std::vector<int> result;
+
+    int i = 0, j = (int) flags;
+    while (j != 0) {
+        if ((j & 0x01) != 0)
+            result.push_back(i);
+        i++;
+        j >>= 1;
+    }
+    return result;
+}
+
+std::vector<int> iosender::ToIndices(iosender::IJKFlags flags) {
+    std::vector<int> result;
+
+    int i = 0, j = (int) flags;
+    while (j != 0) {
+        if ((j & 0x01) != 0)
+            result.push_back(i);
+        i++;
+        j >>= 1;
+    }
+    return result;
+}
+
+std::vector<int> iosender::ToIndices(iosender::ThreadingFlags flags) {
+    std::vector<int> result;
+
+    int i = 0, j = (int) flags;
+    while (j != 0) {
+        if ((j & 0x01) != 0)
+            result.push_back(i);
+        i++;
+        j >>= 1;
+    }
+    return result;
+}
+
+
+std::string iosender::GCodeUtils::StripSpaces(std::string line) {
+    std::string s;
+    bool skip = true;
+
+    s = to_upper(line);
+    if (string_contains(s, "(MSG,")) {
+        s = "";
+        for (auto c: line) {
+            switch (c) {
+                case '(':
+                    s += c;
+                    skip = false;
+                    break;
+                case ')':
+                    skip = true;
+                    s += c;
+                    break;
+                case ' ':
+                    if (!skip)
+                        s += c;
+                    break;
+                default:
+                    s += c;
+                    break;
+            }
+        }
+    } else {
+        s = remove_spaces(line);
+    }
+
+    return s;
+}

terje/gcode.h

@@ -0,0 +1,369 @@
+#pragma once
+
+#include <vector>
+#include <string>
+
+namespace iosender {
+
+enum class Dialect {
+    Grbl,
+    GrblHAL,
+    LinuxCNC
+};
+
+enum class AxisFlags : int {
+    None = 0,
+    X = 1 << 0,
+    Y = 1 << 1,
+    Z = 1 << 2,
+    A = 1 << 3,
+    B = 1 << 4,
+    C = 1 << 5,
+    U = 1 << 6,
+    V = 1 << 7,
+    W = 1 << 8,
+    XY = 0x03,
+    XZ = 0x05,
+    XYZ = 0x07,
+    All = 0x3F
+};
+
+enum class IJKFlags : int {
+    None = 0,
+    I = 1 << 0,
+    J = 1 << 1,
+    K = 1 << 2,
+    All = 0x07
+};
+
+enum class ThreadingFlags : int {
+    None = 0,
+    R = 1 << 0,
+    Q = 1 << 1,
+    H = 1 << 2,
+    E = 1 << 3,
+    L = 1 << 4,
+    All = 0x1F
+};
+
+enum class Plane {
+    XY,
+    XZ,
+    YZ
+};
+
+enum class DistanceMode {
+    Absolute,
+    Incremental
+};
+
+enum class FeedRateMode {
+    InverseTime,    //G93
+    UnitsPerMin,    //G94 - default
+    UnitsPerRev     //G95
+};
+
+enum class MotionMode {
+    G0 = 0,
+    G1 = 10,
+    G2 = 20,
+    G3 = 30,
+    G5 = 50,
+    G5_1 = 51,
+    G5_2 = 52,
+    G33 = 330,
+    G38_2 = 382,
+    G38_3 = 383,
+    G38_4 = 384,
+    G38_5 = 385,
+    G73 = 730,
+    G76 = 760,
+    G80 = 800,
+    None = G80,
+    G81 = 810,
+    G82 = 820,
+    G83 = 830,
+    G84 = 840,
+    G85 = 850,
+    G86 = 860,
+    G87 = 870,
+    G88 = 880,
+    G89 = 890
+};
+
+enum class IJKMode {
+    Absolute,
+    Incremental
+};
+
+enum class Units {
+    Imperial = 0,
+    Metric = 1
+};
+
+enum class SpindleState : int {
+    Off = 1 << 0,
+    CW = 1 << 1,
+    CCW = 1 << 2
+};
+
+enum class CoolantState : int {
+    Off = 0,
+    Flood = 1 << 0,
+    Mist = 1 << 1,
+    Shower = 1 << 2
+};
+
+enum class ToolLengthOffset {
+    Cancel = 0,         // G49 (Default: Must be zero)
+    Enable = 1,         // G43
+    EnableDynamic = 2,  // G43.1
+    ApplyAdditional = 3 // G43.2
+};
+
+enum class ThreadTaper : int {
+    None = 0,
+    Entry = 1 << 0,
+    Exit = 1 << 1,
+    Both = Entry | Exit
+};
+
+enum class LatheMode : int {
+    Disabled = 0,
+    Diameter = 1, // Do not change
+    Radius = 2    // Do not change
+};
+
+enum class Direction {
+    Positive = 0,
+    Negative
+};
+
+enum class InputWaitMode {
+    Immediate = 0,
+    Rise,
+    Fall,
+    High,
+    Low
+};
+
+enum class Commands {
+    G0,
+    G1,
+    G2,
+    G3,
+    G4,
+    G5,
+    G5_1,
+    G7,
+    G8,
+    G10,
+    G17,
+    G18,
+    G19,
+    G20,
+    G21,
+    G28,
+    G28_1,
+    G30,
+    G30_1,
+    G33,
+    G38_2,
+    G38_3,
+    G38_4,
+    G38_5,
+    G40,
+    G43,
+    G43_1,
+    G43_2,
+    G49,
+    G50,
+    G51,
+    G53,
+    G54,
+    G55,
+    G56,
+    G57,
+    G58,
+    G59,
+    G59_1,
+    G59_2,
+    G59_3,
+    G61,
+    G61_1,
+    G64,
+    G73,
+    G76,
+    G80,
+    G81,
+    G82,
+    G83,
+    G85,
+    G86,
+    G89,
+    G90,
+    G90_1,
+    G91,
+    G91_1,
+    G92,
+    G92_1,
+    G92_2,
+    G92_3,
+    G93,
+    G94,
+    G95,
+    G96,
+    G97,
+    G98,
+    G99,
+    M0,
+    M1,
+    M2,
+    M3,
+    M4,
+    M5,
+    M6,
+    M7,
+    M8,
+    M9,
+    M30,
+    M48,
+    M49,
+    M50,
+    M51,
+    M52,
+    M53,
+    M56,
+    M61,
+    M62,
+    M63,
+    M64,
+    M65,
+    M66,
+    M67,
+    M68,
+    Feedrate,
+    SpindleRPM,
+    ToolSelect,
+    Comment,
+    UserMCommand,
+    Undefined
+};
+
+std::vector<int> ToIndices(AxisFlags flags);
+std::vector<int> ToIndices(IJKFlags flags);
+std::vector<int> ToIndices(ThreadingFlags flags);
+
+
+class GCodeUtils {
+public:
+    static std::string StripSpaces(std::string line);
+};
+
+struct Point3D {
+    union {
+        struct {
+            double X, Y, Z;
+        };
+        double value[3];
+    };
+
+    double& operator[](int index) {
+        return value[index];
+    }
+
+    double *Array() { return value; }
+};
+
+
+struct Point6D {
+    union {
+        struct {
+            double X, Y, Z, A, B, C, U, V, W;
+        };
+        double value[9];
+    };
+
+    double& operator[](int index) {
+        return value[index];
+    }
+
+    double *Array() { return value; }
+
+    explicit operator Point3D() { return Point3D{X, Y, Z}; }
+
+    void Set(double *values, AxisFlags axisFlags, bool relative = false) {
+        if (relative) {
+            Add(values, axisFlags);
+        } else {
+            for (auto i: ToIndices(axisFlags)) {
+                switch (i) {
+                    case 0:
+                        X = values[0];
+                        break;
+                    case 1:
+                        Y = values[1];
+                        break;
+                    case 2:
+                        Z = values[2];
+                        break;
+                    case 3:
+                        A = values[3];
+                        break;
+                    case 4:
+                        B = values[4];
+                        break;
+                    case 5:
+                        C = values[5];
+                        break;
+                    case 6:
+                        U = values[6];
+                        break;
+                    case 7:
+                        V = values[7];
+                        break;
+                    case 8:
+                        W = values[8];
+                        break;
+                }
+            }
+        }
+    }
+
+    void Add(const double values[], AxisFlags axisFlags) {
+        for (auto i: ToIndices(axisFlags)) {
+            switch (i) {
+                case 0:
+                    X += values[0];
+                    break;
+                case 1:
+                    Y += values[1];
+                    break;
+                case 2:
+                    Z += values[2];
+                    break;
+                case 3:
+                    A += values[3];
+                    break;
+                case 4:
+                    B += values[4];
+                    break;
+                case 5:
+                    C += values[5];
+                    break;
+                case 6:
+                    U += values[6];
+                    break;
+                case 7:
+                    V += values[7];
+                    break;
+                case 8:
+                    W += values[8];
+                    break;
+            }
+        }
+    }
+};
+
+
+}

terje/gcode_parser.cpp

@@ -0,0 +1 @@
+#include "gcode_parser.h"

terje/gcode_parser.h

@@ -0,0 +1,10 @@
+#pragma once
+
+namespace iosender {
+
+//class GCodeParser : public Machine {
+//public:
+//
+//};
+
+}

terje/grbl.cpp

@@ -0,0 +1,38 @@
+#include "grbl.h"
+#include <thread>
+#include "comms.h"
+
+void iosender::Grbl::Reset() {
+    Comms::GetCom()->WriteByte(GrblConstants::CMD_RESET);
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+}
+
+const std::string iosender::GrblConstants::CMD_STATUS_REPORT_LEGACY = "?";
+const std::string iosender::GrblConstants::CMD_CYCLE_START_LEGACY = "~";
+const std::string iosender::GrblConstants::CMD_FEED_HOLD_LEGACY = "!";
+const std::string iosender::GrblConstants::CMD_UNLOCK = "$X";
+const std::string iosender::GrblConstants::CMD_HOMING = "$H";
+const std::string iosender::GrblConstants::CMD_CHECK = "$C";
+const std::string iosender::GrblConstants::CMD_GETSETTINGS = "$$";
+const std::string iosender::GrblConstants::CMD_GETSETTINGS_ALL = "$+";
+const std::string iosender::GrblConstants::CMD_GETPARSERSTATE = "$G";
+const std::string iosender::GrblConstants::CMD_GETINFO = "$I";
+const std::string iosender::GrblConstants::CMD_GETINFO_EXTENDED = "$I+";
+const std::string iosender::GrblConstants::CMD_GETNGCPARAMETERS = "$#";
+const std::string iosender::GrblConstants::CMD_GETSTARTUPLINES = "$N";
+const std::string iosender::GrblConstants::CMD_GETSETTINGSDETAILS = "$ES";
+const std::string iosender::GrblConstants::CMD_GETSETTINGSGROUPS = "$EG";
+const std::string iosender::GrblConstants::CMD_GETALARMCODES = "$EA";
+const std::string iosender::GrblConstants::CMD_GETERRORCODES = "$EE";
+const std::string iosender::GrblConstants::CMD_PROGRAM_DEMARCATION = "%";
+const std::string iosender::GrblConstants::CMD_SDCARD_MOUNT = "$FM";
+const std::string iosender::GrblConstants::CMD_SDCARD_DIR = "$F";
+const std::string iosender::GrblConstants::CMD_SDCARD_DIR_ALL = "$F+";
+const std::string iosender::GrblConstants::CMD_SDCARD_REWIND = "$FR";
+const std::string iosender::GrblConstants::CMD_SDCARD_RUN = "$F=";
+const std::string iosender::GrblConstants::CMD_SDCARD_UNLINK = "$FD=";
+const std::string iosender::GrblConstants::CMD_SDCARD_DUMP = "$F<=";
+const std::string iosender::GrblConstants::FORMAT_METRIC = "###0.000";
+const std::string iosender::GrblConstants::FORMAT_IMPERIAL = "##0.0000";
+const std::string iosender::GrblConstants::NO_TOOL = "None";
+const std::string iosender::GrblConstants::THCSIGNALS = "AERTOVHDU"; // Keep in sync with THCSignals enum below!!

terje/grbl.h

@@ -0,0 +1,304 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+#include "grbl.h"
+
+namespace iosender {
+
+
+struct Color {
+    union {
+        float r, g, b, a;
+    };
+    float values[4];
+};
+
+class GrblConstants {
+public:
+
+    static const uint8_t CMD_EXIT = 0x03; // ctrl-C
+    static const uint8_t CMD_RESET = 0x18; // ctrl-X
+    static const uint8_t CMD_STOP = 0x19; // ctrl-Y
+    static const uint8_t CMD_STATUS_REPORT = 0x80;
+    static const uint8_t CMD_CYCLE_START = 0x81;
+    static const uint8_t CMD_FEED_HOLD = 0x82;
+    static const uint8_t CMD_GCODE_REPORT = 0x83;
+    static const uint8_t CMD_SAFETY_DOOR = 0x84;
+    static const uint8_t CMD_JOG_CANCEL = 0x85;
+    static const uint8_t CMD_STATUS_REPORT_ALL = 0x87;
+    static const uint8_t CMD_OPTIONAL_STOP_TOGGLE = 0x88;
+    static const uint8_t CMD_SINGLE_BLOCK_TOGGLE = 0x89;
+    static const uint8_t CMD_OVERRIDE_FAN0_TOGGLE = 0x8A;
+    static const uint8_t CMD_FEED_OVR_RESET = 0x90;
+    static const uint8_t CMD_FEED_OVR_COARSE_PLUS = 0x91;
+    static const uint8_t CMD_FEED_OVR_COARSE_MINUS = 0x92;
+    static const uint8_t CMD_FEED_OVR_FINE_PLUS = 0x93;
+    static const uint8_t CMD_FEED_OVR_FINE_MINUS = 0x94;
+    static const uint8_t CMD_RAPID_OVR_RESET = 0x95;
+    static const uint8_t CMD_RAPID_OVR_MEDIUM = 0x96;
+    static const uint8_t CMD_RAPID_OVR_LOW = 0x97;
+    static const uint8_t CMD_SPINDLE_OVR_RESET = 0x99;
+    static const uint8_t CMD_SPINDLE_OVR_COARSE_PLUS = 0x9A;
+    static const uint8_t CMD_SPINDLE_OVR_COARSE_MINUS = 0x9B;
+    static const uint8_t CMD_SPINDLE_OVR_FINE_PLUS = 0x9C;
+    static const uint8_t CMD_SPINDLE_OVR_FINE_MINUS = 0x9D;
+    static const uint8_t CMD_SPINDLE_OVR_STOP = 0x9E;
+    static const uint8_t CMD_COOLANT_FLOOD_OVR_TOGGLE = 0xA0;
+    static const uint8_t CMD_COOLANT_MIST_OVR_TOGGLE = 0xA1;
+    static const uint8_t CMD_PID_REPORT = 0xA2;
+    static const uint8_t CMD_TOOL_ACK = 0xA3;
+    static const uint8_t CMD_PROBE_CONNECTED_TOGGLE = 0xA4;
+
+    static const std::string CMD_STATUS_REPORT_LEGACY;
+    static const std::string CMD_CYCLE_START_LEGACY;
+    static const std::string CMD_FEED_HOLD_LEGACY;
+    static const std::string CMD_UNLOCK;
+    static const std::string CMD_HOMING;
+    static const std::string CMD_CHECK;
+    static const std::string CMD_GETSETTINGS;
+    static const std::string CMD_GETSETTINGS_ALL;
+    static const std::string CMD_GETPARSERSTATE;
+    static const std::string CMD_GETINFO;
+    static const std::string CMD_GETINFO_EXTENDED;
+    static const std::string CMD_GETNGCPARAMETERS;
+    static const std::string CMD_GETSTARTUPLINES;
+    static const std::string CMD_GETSETTINGSDETAILS;
+    static const std::string CMD_GETSETTINGSGROUPS;
+    static const std::string CMD_GETALARMCODES;
+    static const std::string CMD_GETERRORCODES;
+    static const std::string CMD_PROGRAM_DEMARCATION;
+    static const std::string CMD_SDCARD_MOUNT;
+    static const std::string CMD_SDCARD_DIR;
+    static const std::string CMD_SDCARD_DIR_ALL;
+    static const std::string CMD_SDCARD_REWIND;
+    static const std::string CMD_SDCARD_RUN;
+    static const std::string CMD_SDCARD_UNLINK;
+    static const std::string CMD_SDCARD_DUMP;
+    static const std::string FORMAT_METRIC;
+    static const std::string FORMAT_IMPERIAL;
+    static const std::string NO_TOOL;
+    static const std::string THCSIGNALS; // Keep in sync with THCSignals enum below!!
+
+    static const int X_AXIS = 0;
+    static const int Y_AXIS = 1;
+    static const int Z_AXIS = 2;
+    static const int A_AXIS = 3;
+    static const int B_AXIS = 4;
+    static const int C_AXIS = 5;
+};
+
+enum class CameraMoveMode {
+    XAxisFirst = 1,
+    YAxisFirst = 2,
+    BothAxes = 3
+};
+
+
+enum class GrblStates {
+    Unknown = 0,
+    Idle,
+    Run,
+    Tool,
+    Hold,
+    Home,
+    Check,
+    Jog,
+    Alarm,
+    Door,
+    Sleep
+};
+
+enum class GrblMode {
+    Normal = 0,
+    Laser,
+    Lathe
+};
+
+enum class GrblEncoderMode {
+    Unknown = 0,
+    FeedRate = 1,
+    RapidRate = 2,
+    SpindleRPM = 3
+};
+
+enum class GrblSetting {
+    PulseMicroseconds = 0,
+    StepperIdleLockTime = 1,
+    StepInvertMask = 2,
+    DirInvertMask = 3,
+    InvertStepperEnable = 4,
+    LimitPinsInvertMask = 5,
+    InvertProbePin = 6,
+    StatusReportMask = 10,
+    JunctionDeviation = 11,
+    ArcTolerance = 12,
+    ReportInches = 13,
+    SoftLimitsEnable = 20,
+    HardLimitsEnable = 21,
+    HomingEnable = 22,
+    HomingDirMask = 23,
+    HomingFeedRate = 24,
+    HomingSeekRate = 25,
+    HomingDebounceDelay = 26,
+    HomingPulloff = 27,
+    G73Retract = 28,
+    PulseDelayMicroseconds = 29,
+    RpmMax = 30,
+    RpmMin = 31,
+    Mode = 32, // enum GrblMode
+    PWMFreq = 33,
+    PWMOffValue = 34,
+    PWMMinValue = 35,
+    PWMMaxValue = 36,
+    TravelResolutionBase = 100,
+    MaxFeedRateBase = 110,
+    AccelerationBase = 120,
+    MaxTravelBase = 130,
+    MotorCurrentBase = 140,
+};
+
+enum class grblHALSetting {
+    PulseMicroseconds = 0,
+    StepperIdleLockTime = 1,
+    StepInvertMask = 2,
+    DirInvertMask = 3,
+    InvertStepperEnable = 4,
+    LimitPinsInvertMask = 5,
+    InvertProbePin = 6,
+    StatusReportMask = 10,
+    JunctionDeviation = 11,
+    ArcTolerance = 12,
+    ReportInches = 13,
+    ControlInvertMask = 14, // Note: Used for detecting GrblHAL firmware
+    CoolantInvertMask = 15,
+    SpindleInvertMask = 16,
+    ControlPullUpDisableMask = 17,
+    LimitPullUpDisableMask = 18,
+    ProbePullUpDisable = 19,
+    SoftLimitsEnable = 20,
+    HardLimitsEnable = 21,
+    HomingEnable = 22,
+    HomingDirMask = 23,
+    HomingFeedRate = 24,
+    HomingSeekRate = 25,
+    HomingDebounceDelay = 26,
+    HomingPulloff = 27,
+    G73Retract = 28,
+    PulseDelayMicroseconds = 29,
+    RpmMax = 30,
+    RpmMin = 31,
+    Mode = 32, // enum GrblMode
+    PWMFreq = 33,
+    PWMOffValue = 34,
+    PWMMinValue = 35,
+    PWMMaxValue = 36,
+    StepperDeenergizeMask = 37,
+    SpindlePPR = 38,
+    EnableLegacyRTCommands = 39,
+    SoftLimitJogging = 40,
+    HomingLocateCycles = 43,
+    HomingCycle_1 = 44,
+    HomingCycle_2 = 45,
+    HomingCycle_3 = 46,
+    HomingCycle_4 = 47,
+    HomingCycle_5 = 48,
+    HomingCycle_6 = 49,
+    JogStepSpeed = 50,
+    JogSlowSpeed = 51,
+    JogFastSpeed = 52,
+    JogStepDistance = 53,
+    JogSlowDistance = 54,
+    JogFastDistance = 55,
+    // Per axis settings
+    TravelResolutionBase = 100,
+    MaxFeedRateBase = 110,
+    AccelerationBase = 120,
+    MaxTravelBase = 130,
+    MotorCurrentBase = 140,
+    MicroStepsBase = 150,
+    StallGuardBase = 200,
+    // End per axis settings
+    ToolChangeMode = 341
+};
+
+enum class StreamingState {
+    NoFile = 0,
+    Idle,
+    Send,
+    SendMDI,
+    Home,
+    Halted,
+    FeedHold,
+    ToolChange,
+    Start,
+    Stop,
+    Paused,
+    JobFinished,
+    Reset,
+    AwaitResetAck,
+    Disabled,
+    Error
+};
+
+enum class HomedState {
+    Unknown = 0,
+    NotHomed,
+    Homed
+};
+
+// Keep in sync with GrblInfo.SignalLetters constant below
+enum class Signals : int {
+    Off = 0,
+    LimitX = 1 << 0,
+    LimitY = 1 << 1,
+    LimitZ = 1 << 2,
+    LimitA = 1 << 3,
+    LimitB = 1 << 4,
+    LimitC = 1 << 5,
+    LimitU = 1 << 6,
+    LimitV = 1 << 7,
+    LimitW = 1 << 8,
+    EStop = 1 << 9,
+    Probe = 1 << 10,
+    Reset = 1 << 11,
+    SafetyDoor = 1 << 12,
+    Hold = 1 << 13,
+    CycleStart = 1 << 14,
+    BlockDelete = 1 << 15,
+    OptionalStop = 1 << 16,
+    ProbeDisconnected = 1 << 17,
+    MotorWarning = 1 << 18,
+    MotorFault = 1 << 19
+};
+
+enum class THCSignals : int {
+    Off = 0,
+    ArcOk = 1 << 0,
+    THCEnabled = 1 << 1,
+    THCActive = 1 << 2,
+    TorchOn = 1 << 3,
+    OhmicProbe = 1 << 4,
+    VelocityLock = 1 << 5,
+    VoidLock = 1 << 6,
+    Down = 1 << 7,
+    Up = 1 << 8
+};
+
+struct GrblState {
+    GrblStates State;
+    int Substate;
+    int LastAlarm;
+    int Error;
+    Color color;
+    bool MPG;
+};
+
+
+class Grbl {
+public:
+    static void Reset();
+};
+
+
+}

terje/machine.cpp

@@ -0,0 +1 @@
+#include "machine.h"

terje/machine.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <vector>
+#include "gcode.h"
+
+namespace iosender {
+
+class CoordinateSystem {
+};
+
+class Tool {
+};
+
+class Machine {
+public:
+
+    void Reset() {
+//        // Sync with controller
+//        if (GrblInfo.IsGrblHAL) {
+//            GrblParserState.Get();
+//            GrblWorkParameters.Get();
+//        } else {
+//            GrblParserState.Get(true);
+//        }
+//        coordinateSystems.Clear();
+//        for (CoordinateSystem c : GrblWorkParameters.CoordinateSystems) {
+//            coordinateSystems.Add(c);
+//        }
+
+    }
+
+protected:
+    double rpm_ = 0;
+    bool isRelative = false;
+    int _tool = 0;
+    double offsets[9];
+    double origin[9];
+    double scaleFactors[9];
+    double toolOffsets[9];
+    std::vector<CoordinateSystem> coordinateSystems;
+    std::vector<Tool> toolTable;
+    Point6D machinePos;
+
+private:
+
+};
+
+}

terje/measure_view_model.cpp

@@ -0,0 +1 @@
+#include "measure_view_model.h"

terje/measure_view_model.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <string>
+#include "grbl.h"
+
+namespace iosender {
+
+class MeasureViewModel {
+public:
+
+    bool _isMetric = true;
+    const double MM_PER_INCH = 25.4;
+
+    bool IsMetric() const { return _isMetric; }
+
+    void IsMetric(bool value) {
+        if (value != _isMetric) {
+            _isMetric = value;
+            // trigger OnPropertyChanged()
+        }
+    }
+
+    std::string Unit() const { return _isMetric ? "mm" : "in"; }
+
+    std::string FeedrateUnit() const { return _isMetric ? "mm/min" : "in/min"; }
+
+    double UnitFactor() const { return _isMetric ? 1.0 : 25.4; }
+
+    std::string Format() const { return _isMetric ? GrblConstants::FORMAT_METRIC : GrblConstants::FORMAT_IMPERIAL; }
+
+    std::string FormatSigned() const { return "-" + Format(); }
+
+    int Precision() const { return _isMetric ? 3 : 4; }
+
+    double ConvertMM2Current(double value) const {
+        if (!_isMetric)
+            value /= 25.4;
+        return value;
+    }
+
+};
+
+}

terje/telnet.cpp

@@ -0,0 +1,199 @@
+#include "telnet.h"
+#include "../string_utils.h"
+#include "grbl.h"
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <iostream>
+#include <fcntl.h>
+#include <chrono>
+
+bool iosender::TelnetStream::IsOpen() {
+    return fd > 0 && isConnected;
+}
+
+int iosender::TelnetStream::OutCount() {
+    return 0;
+}
+
+std::string iosender::TelnetStream::Reply() {
+    return reply;
+}
+
+iosender::Comms::StreamType iosender::TelnetStream::StreamType() {
+    return Comms::StreamType::Telnet;
+}
+
+bool iosender::TelnetStream::EventMode() {
+    return eventMode;
+}
+
+void iosender::TelnetStream::EventMode(bool mode) {
+    eventMode = mode;
+}
+
+void iosender::TelnetStream::Close() {
+    shouldQuit = true;
+    workerThread.join();
+}
+
+int iosender::TelnetStream::ReadByte() {
+    return 0;
+}
+
+void iosender::TelnetStream::WriteByte(uint8_t data) {
+    auto result = write(fd, &data, 1);
+    if (result == -1) {
+        std::cerr << "Failed while writing." << std::endl;
+    }
+}
+
+void iosender::TelnetStream::WriteBytes(uint8_t *bytes, int len) {
+    ssize_t written_bytes = 0;
+    for (; written_bytes < len;) {
+        auto result = write(fd, &bytes, len);
+        if (result == -1) {
+            std::cerr << "Failed while writing." << std::endl;
+            return;
+        }
+        written_bytes += result;
+    }
+}
+
+void iosender::TelnetStream::WriteString(std::string data) {
+    WriteBytes(reinterpret_cast<uint8_t *>(data.data()), data.size());
+}
+
+void iosender::TelnetStream::WriteCommand(std::string command) {
+    CommandState = Comms::State::AwaitAck;
+
+    if (command.size() == 1 && command != GrblConstants::CMD_PROGRAM_DEMARCATION) {
+        WriteByte(*command.data());
+    } else {
+        command += "\r";
+        WriteString(command);
+    }
+}
+
+std::string iosender::TelnetStream::GetReply(std::string command) {
+    reply = "";
+    AwaitResponse(command);
+    return reply;
+}
+
+void iosender::TelnetStream::AwaitAck() {
+    while (Comms::GetCom()->CommandState == Comms::State::DataReceived ||
+           Comms::GetCom()->CommandState == Comms::State::AwaitAck) {
+        std::this_thread::sleep_for(std::chrono::microseconds(500));
+    }
+
+}
+
+void iosender::TelnetStream::AwaitAck(std::string command) {
+    WriteCommand(command);
+    AwaitAck();
+}
+
+void iosender::TelnetStream::AwaitResponse() {
+    while (Comms::GetCom()->CommandState == Comms::State::AwaitAck) {
+        std::this_thread::sleep_for(std::chrono::microseconds(500));
+    }
+}
+
+void iosender::TelnetStream::AwaitResponse(std::string command) {
+    WriteCommand(command);
+    AwaitResponse();
+}
+
+void iosender::TelnetStream::PurgeQueue() {
+}
+
+iosender::TelnetStream::TelnetStream(std::string host, iosender::DataReceivedHandler *dataHandler,
+                                     iosender::ByteReceivedHandler *byteHandler) {
+    Comms::SetCom(this);
+    reply = "";
+    dataReceivedHandler = dataHandler;
+    byteReceivedHandler = byteHandler;
+
+    if (!string_contains(host, ":")) {
+        host += ":23";
+    }
+
+    auto parameter = split_string(host, ":");
+    auto ip = parameter[0];
+    auto port = std::stoi(parameter[1]);
+
+    fd = socket(AF_INET, SOCK_STREAM, 0);
+    if (fd == -1) {
+        std::cerr << "Error creating socket" << std::endl;
+    }
+
+    struct sockaddr_in serv_addr{};
+    serv_addr.sin_family = AF_INET;
+    serv_addr.sin_port = htons(port);
+
+    if (inet_pton(AF_INET, ip.c_str(), &serv_addr.sin_addr) <= 0) {
+        std::cerr << "Invalid address/ Address not supported" << std::endl;
+        return;
+    }
+
+    auto status = connect(fd, (struct sockaddr *) &serv_addr, sizeof(serv_addr));
+    if (status < 0) {
+        std::cerr << "Connection failed";
+        return;
+    }
+
+    isConnected = true;
+
+    // set non-blocking please, after we connected
+    fcntl(fd, F_SETFL, O_NONBLOCK);
+
+    workerThread = std::thread(&iosender::TelnetStream::readWorker, this);
+}
+
+iosender::TelnetStream::~TelnetStream() {
+    Close();
+}
+
+void iosender::TelnetStream::readWorker() {
+    std::string received;
+    uint8_t buffer[200];
+
+    while (!shouldQuit) {
+        if (isConnected) {
+
+            // anything to read?
+            auto readBytes = read(fd, buffer, 200);
+            if (readBytes == -1 && (errno == EWOULDBLOCK || errno == EAGAIN)) {
+                // do nothing
+            } else if (readBytes == -1) {
+                std::cerr << "Failed while reading." << std::endl;
+                Close();
+            } else {
+                for (int i = 0; i < readBytes; i++) {
+                    auto isEol = buffer[i] == '\n';
+                    if (isEol) {
+                        reply = received;
+
+                        CommandState = reply == "ok" ?
+                                       Comms::State::ACK :
+                                       (starts_with(reply, "error") ?
+                                        Comms::State::NAK :
+                                        Comms::State::DataReceived);
+
+                        if (!reply.empty() && dataReceivedHandler != nullptr) {
+                            dataReceivedHandler->DataReceived(reply);
+                        }
+
+                        received.clear();
+                    } else {
+                        received.push_back(static_cast<char>(buffer[i]));
+                    }
+                }
+            }
+
+            // give some time to others
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+    }
+}

terje/telnet.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <thread>
+#include "comms.h"
+
+namespace iosender {
+
+class TelnetStream : public StreamComms {
+public:
+
+    TelnetStream(std::string host, DataReceivedHandler* dataHandler = nullptr, ByteReceivedHandler *byteHandler = nullptr);
+    ~TelnetStream() override;
+
+    bool IsOpen() override;
+    int OutCount() override;
+    std::string Reply() override;
+    Comms::StreamType StreamType() override;
+    bool EventMode() override;
+    void EventMode(bool mode) override;
+    void Close() override;
+    int ReadByte() override;
+    void WriteByte(uint8_t data) override;
+    void WriteBytes(uint8_t *bytes, int len) override;
+    void WriteString(std::string data) override;
+    void WriteCommand(std::string command) override;
+    std::string GetReply(std::string command) override;
+    void AwaitAck() override;
+    void AwaitAck(std::string command) override;
+    void AwaitResponse(std::string command) override;
+    void AwaitResponse() override;
+    void PurgeQueue() override;
+
+
+private:
+    volatile bool shouldQuit = false;
+    void readWorker();
+
+    std::string reply;
+    int fd;
+    bool isConnected;
+    bool eventMode = true;
+    std::thread workerThread;
+};
+
+
+}