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

@@ -25,6 +25,8 @@ 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 ${TESTS} grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp)
+add_executable(sender main.cpp ${TESTS} ${TERJE} ${SENDER_GRBL_SRC})
 target_link_libraries(sender nanogui GL gtest gtest_main)

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) {
-    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment << " }";
+std::ostream &operator<<(std::ostream &out, const grbl::instruction &i) {
+    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment
+        << " }";
     return out;
 }
 
-void grbl::program::dump(std::ostream& out) {
-    for (auto& i: instructions) {
+void grbl::program::dump(std::ostream &out) {
+    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;
@@ -42,7 +38,7 @@ 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;
 
     // pin values are always reset when a report arrives
@@ -75,7 +71,7 @@ grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::r
                 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]) {
+                for (auto &c: elements[1]) {
                     switch (c) {
                         case 'P':
                             result.signals.bit.probe = true;
@@ -115,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;
@@ -129,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);
 }
@@ -153,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);
@@ -163,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";
@@ -594,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;
 }
 
@@ -687,7 +685,7 @@ void grbl::machine::start_z_probe(float min_z, float feed_rate) {
     awaiting_responses++;
 }
 
-void grbl::machine::probe_heightmap(grbl::heightmap& grid) {
+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);
@@ -760,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, ":");
@@ -837,7 +836,8 @@ void grbl::machine_state_idle::on_line_received(std::string line) {
     } 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, ":");
@@ -952,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);
@@ -1018,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);
@@ -1120,21 +1122,23 @@ bool grbl::machine_state_heightmap_probing::continue_program() {
 void grbl::machine_state_heightmap_probing::move_to_next_stage() {
     switch (stage) {
         case heightmap_probing_stage::start:
-            cnc->pipe->send("G0X0Y0Z15");
+            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 F100");
+            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 Z1");
+            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-5 F5");
+            cnc->pipe->send("G38.2 Z-1 F5");
             stage = heightmap_probing_stage::initial_probe;
             break;
         case heightmap_probing_stage::initial_probe:
@@ -1151,7 +1155,6 @@ void grbl::machine_state_heightmap_probing::move_to_next_stage() {
                 grid->vertices[probed_locations].z = 0;
             }
 
-
             stage = heightmap_probing_stage::goto_next_location;
             break;
         case heightmap_probing_stage::goto_next_location: {
@@ -1176,7 +1179,7 @@ void grbl::machine_state_heightmap_probing::move_to_next_stage() {
                 cnc->pipe->send("G0Z15"); // safe height
                 stage = heightmap_probing_stage::done;
             } else {
-                cnc->pipe->send("G90 G0 Z1.5");
+                cnc->pipe->send("G90 G0 Z1");
                 stage = heightmap_probing_stage::goto_next_location_move;
             }
             break;
@@ -1186,13 +1189,13 @@ void grbl::machine_state_heightmap_probing::move_to_next_stage() {
             stage = heightmap_probing_stage::goto_start_probing_at;
             break;
         case heightmap_probing_stage::goto_start_probing_at:
-            cnc->pipe->send("G0 Z0.5"); // this appears to move Z upwards instead of downwards. why?
+            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-5 F5");
+            cnc->pipe->send("G38.2 Z-1 F5");
             stage = heightmap_probing_stage::goto_next_location;
             break;
         case heightmap_probing_stage::done:
@@ -1212,17 +1215,17 @@ 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)
+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)
+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)
+grbl::machine_event_message::machine_event_message(const std::string &m)
         : message{m} {
     machine_event::type = machine_event_type::message;
 }
@@ -1264,7 +1267,8 @@ grbl::machine_event_probe_result::machine_event_probe_result(bool touched, const
     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)
+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_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;
@@ -54,3 +56,45 @@ TEST(grbl_status_report, parse) {
     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

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

heightmap.h

@@ -7,17 +7,18 @@
 
 namespace grbl {
 
-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;
+    bool float_equal(float a, float b);
 
-    float from_x, from_y;
-    float to_x, to_y;
-    float resolution;
-    size_t x_segments, y_segments;
-    std::vector<glm::vec3> vertices;
-};
+    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

@@ -24,3 +24,26 @@ TEST(heightmap, get_z_at) {
     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));
+}

render.cpp

@@ -4,6 +4,8 @@
 #include "glm/vec3.hpp"
 #include "glm/vec4.hpp"
 #include "glm/gtc/type_ptr.hpp"
+#include "gcode_parser.h"
+#include <fstream>
 
 static const char *vs_code = R"(
     #version 330
@@ -101,7 +103,7 @@ static const char *ps_heightmap_code = R"(
     }
 )";
 
-static void add_vertex(std::vector<float>& buffer_data, glm::vec3 v, glm::vec3 n, glm::vec4 col) {
+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);
@@ -116,7 +118,7 @@ static void add_vertex(std::vector<float>& buffer_data, glm::vec3 v, glm::vec3 n
     buffer_data.push_back(col.a);
 }
 
-static void add_vertex(std::vector<float>& buffer_data, glm::vec3 v, glm::vec4 col) {
+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);
@@ -127,24 +129,27 @@ static void add_vertex(std::vector<float>& buffer_data, glm::vec3 v, glm::vec4 c
     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) {
+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) {
+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) {
+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) {
+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
@@ -191,7 +196,7 @@ void grbl::program_renderer::render(glm::mat4 model, glm::mat4 view, glm::mat4 p
     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);
@@ -338,80 +343,45 @@ void grbl::program_renderer::initialize_program_buffers() {
     glBindVertexArray(0);
 }
 
-GLsizei grbl::program_renderer::update_model_vbo(const grbl::program& pgm) {
+GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) {
     if (!pgm.is_loaded) return 0;
 
-    static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))");
-    bool is_tool_on = false;
-
-    glm::vec3 tool_pos;
-    min_pos = max_pos = tool_pos = glm::vec3(0);
+    min_pos = glm::vec3(std::numeric_limits<float>::max());
+    max_pos = glm::vec3(std::numeric_limits<float>::min());
 
     std::vector<float> buffer_data;
-    for (auto& i: pgm.instructions) {
-        if (i.type == grbl::instruction_type::gcode) {
 
-            std::vector<std::string> tokens;
-            std::smatch res;
-            std::string::const_iterator start(i.command.cbegin());
-            while (std::regex_search(start, i.command.cend(), res, movement_re)) {
-                auto str = res[0].str();
+    grbl::grbl_parser parser;
+    std::ifstream in_file{pgm.filename};
+    if (in_file) {
+        parser.parse(in_file);
 
-                // make upper case
-                std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+        auto rapid_color = glm::vec4(1.0f);
+        auto feed_color = glm::vec4(1.0f, 0, 0, 1);
 
-                // remove whitespace from things like "X 123.1234"
-                str.erase(remove_if(str.begin(), str.end(), isspace), str.end());
+        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) {
+                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);
 
-                tokens.push_back(str);
-                start = res.suffix().first;
-            }
-
-            if (!tokens.empty()) {
-                auto new_pos = tool_pos;
-
-                for (auto& t: tokens) {
-                    if (t[0] == 'X') {
-                        new_pos.x = std::stof(t.substr(1));
-                    } else if (t[0] == 'Y') {
-                        new_pos.y = std::stof(t.substr(1));
-                    } else if (t[0] == 'Z') {
-                        new_pos.z = std::stof(t.substr(1));
-                    }
+                    update_model_extents(line->start);
+                    update_model_extents(line->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);
+            } else if (arc != nullptr) {
+                auto pieces = arc->split(0.1);
+                for (auto &p: pieces) {
+                    // transform arc to line
+                    add_line(buffer_data, p->start, feed_color, p->end, feed_color);
 
-                auto from_color = glm::vec4(1.0f);
-                auto to_color = glm::vec4(1.0f);
-                if (is_tool_on) {
-                    from_color = glm::vec4(1, 0.6, 0.6, 1);
-                    to_color = glm::vec4(1, 0.6, 0.6, 1);
+                    update_model_extents(p->start);
+                    update_model_extents(p->end);
                 }
-
-                if (is_plunge) {
-                    to_color = glm::vec4(1, 0.6, 0.6, 1);
-                } else if (is_retract) {
-                    to_color = glm::vec4(0.6, 1, 0.6, 1);
-                }
-
-                add_line(buffer_data, tool_pos, from_color, new_pos, to_color);
-
-                // 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;
             }
         }
     }
@@ -426,7 +396,7 @@ GLsizei grbl::program_renderer::update_model_vbo(const grbl::program& pgm) {
 }
 
 
-void grbl::program_renderer::update_grid(const grbl::heightmap& grid) {
+void grbl::program_renderer::update_grid(const grbl::heightmap &grid, float exaggeration_factor) {
 
     glm::vec4 color = {0.5, 0.3, 0, 1};
 
@@ -447,7 +417,6 @@ void grbl::program_renderer::update_grid(const grbl::heightmap& grid) {
             auto p3 = grid.vertices[next];
 
             // exaggerate Z
-            auto exaggeration_factor = 100.0f;
             p1.z *= exaggeration_factor;
             p2.z *= exaggeration_factor;
             p3.z *= exaggeration_factor;
@@ -501,6 +470,17 @@ void grbl::program_renderer::initialize_heightmap_buffers() {
     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);
@@ -528,7 +508,8 @@ bool check_compile_error(GLuint shader_id) {
     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;
@@ -540,7 +521,8 @@ 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

@@ -19,9 +19,10 @@ public:
 
     glm::vec3 get_extents_max() const { return max_pos; };
 
-    void update_grid(const grbl::heightmap& grid);
+    void update_grid(const grbl::heightmap& grid, float exaggeration_factor);
 private:
     GLsizei update_model_vbo(const grbl::program& pgm);
+    void update_model_extents(glm::vec3 point);
 
     GLuint spindle_vbo_id;
     GLuint spindle_vao_id;

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;
+};
+
+
+}