Enabled arcs render after adapting OpenCNCPilot code.

Started porting iosender to C++

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)
 
-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<std::shared_ptr<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 {std::shared_ptr<grbl::motion_cmd>(this)};
+    }
+
+    std::vector<std::shared_ptr<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(std::shared_ptr<line_motion_cmd>(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<std::shared_ptr<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<std::shared_ptr<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(std::shared_ptr<arc_motion_cmd>(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<std::shared_ptr<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<std::shared_ptr<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<std::shared_ptr<motion_cmd>> split(double length) override;
+
+        double length() const override;
+
+    };
+
+
+}
+

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(std::make_shared<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(std::make_shared<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(std::make_shared<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(std::shared_ptr<line_motion_cmd>(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(std::shared_ptr<arc_motion_cmd>(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<std::shared_ptr<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_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;

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.get());
+        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.get());
+        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);
+}

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
@@ -127,7 +129,8 @@ 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);
 }
@@ -137,14 +140,16 @@ static void add_line(std::vector<float>& buffer_data, glm::vec3 from, glm::vec3
     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
@@ -341,6 +346,7 @@ void grbl::program_renderer::initialize_program_buffers() {
 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;
 
@@ -415,6 +421,36 @@ GLsizei grbl::program_renderer::update_model_vbo(const grbl::program& pgm) {
             }
         }
     }
+     */
+
+
+    std::vector<float> buffer_data;
+
+    grbl::grbl_parser parser;
+    std::ifstream in_file{pgm.filename};
+    if (in_file) {
+        parser.parse(in_file);
+
+        auto rapid_color = glm::vec4(1.0f);
+        auto feed_color = glm::vec4(1.0f, 0, 0, 1);
+
+        for (auto &c: parser.commands) {
+            auto line = dynamic_cast<grbl::line_motion_cmd *>(c.get());
+            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c.get());
+            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);
+            } 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);
+                }
+            }
+        }
+    }
 
     const GLsizei size_of_vertex_in_bytes = 28;
     auto number_of_vertices = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
@@ -528,7 +564,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 +577,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;

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