diff --git a/grbl_machine.cpp b/grbl_machine.cpp
index 977da0b..8167d02 100644
--- a/grbl_machine.cpp
+++ b/grbl_machine.cpp
@@ -89,15 +89,6 @@ grbl::machine_status grbl::status_from_string(const std::string& status) {
     return machine_status::unknown;
 }
 
-void grbl::machine::run_program(const grbl::program& pgm) {
-    std::cout << "running program (" << pgm.filename << ") with " << pgm.number_of_instructions() << " instructions" << std::endl;
-    running_program = pgm;
-    state = grbl_machine_state::run_program;
-
-    executed_instructions = 0;
-    continue_program();
-}
-
 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()
@@ -105,6 +96,13 @@ void grbl::machine::check_program(const grbl::program& pgm) {
     switch_to_state(grbl_machine_state::check_program);
 }
 
+void grbl::machine::run_program(const grbl::program& pgm) {
+    running_program = pgm;
+    std::cout << "running program (" << running_program.filename << ") with " << running_program.number_of_instructions() << " instructions"
+              << std::endl;
+    switch_to_state(grbl_machine_state::run_program);
+}
+
 std::string grbl::status_to_string(const grbl::machine_status& status) {
     switch (status) {
         case machine_status::idle:
@@ -460,76 +458,6 @@ void grbl::machine::on_line_received(std::string line, grbl::transport *transpor
         std::cout << ">> " << line << std::endl;
 
     states[state]->on_line_received(line);
-
-//    if (starts_with(line, "ok")) {
-//        if (state == grbl_machine_state::run_program || state == grbl_machine_state::check_program) {
-//            if (!entered_check_mode) {
-//                entered_check_mode = true;
-//            }
-//            continue_program();
-//        } else if (state == grbl_machine_state::idle && entered_check_mode) {
-//            entered_check_mode = false;
-//        }
-//
-//    } else if (starts_with(line, "error")) {
-//        size_t error = std::stoi(line.substr(6));
-//        if (state == grbl_machine_state::run_program) {
-//            listener->on_run_completed(false, executed_instructions - 1, error);
-//            state = grbl_machine_state::idle;
-//        } else if (state == grbl_machine_state::check_program) {
-//            listener->on_check_completed(false, executed_instructions - 1, error);
-//            state = grbl_machine_state::idle;
-//            pipe->send("$C"); // exit check mode
-//        }
-////        on_error(error);
-//    } else {
-//        // we have a push message
-//        if (starts_with(line, "Grbl")) {
-//            listener->on_banner(line);
-//            pipe->send("$10=0"); // display position in work pos, please
-//            pipe->send("$$"); // get all settings
-//            pipe->send("$#"); // get all offsets
-//            reset_machine_state();
-//        } else if (starts_with(line, "<")) {
-//            last_report = parse_status_report(line, last_report);
-//            listener->on_realtime_status_report(last_report);
-//        } else if (starts_with(line, "[MSG:")) {
-//            listener->on_message(line.substr(5, line.size() - 6));
-//        } else if (starts_with(line, "ALARM:")) {
-//            listener->on_alarm(std::stoi(line.substr(6)));
-//        } else {
-//            std::cout << "received >> " << line << std::endl;
-//        }
-//    }
-
-}
-
-void grbl::machine::continue_program() {
-    bool program_ended = false;
-    if (executed_instructions < running_program.number_of_instructions()) {
-        instruction to_send;
-        do {
-            to_send = running_program.instruction_at(executed_instructions++);
-        } while (to_send.type != instruction_type::gcode && executed_instructions < running_program.number_of_instructions());
-
-        if (to_send.type == instruction_type::gcode) {
-            pipe->send(to_send.command);
-        } else {
-            program_ended = true;
-        }
-    } else {
-        program_ended = true;
-    }
-
-    if (program_ended) {
-        if (state == grbl_machine_state::check_program) {
-            pipe->send("$C");
-            listener->on_check_completed(true, 0, 0);
-        } else if (state == grbl_machine_state::run_program) {
-            listener->on_run_completed(true, 0, 0);
-        }
-        state = grbl_machine_state::idle;
-    }
 }
 
 void grbl::machine::request_jog(jog_state jog) const {
@@ -597,8 +525,8 @@ void grbl::machine::request_feed_hold() {
 }
 
 void grbl::machine::reset_machine_state() {
-    state = grbl_machine_state::idle;
-    executed_instructions = false;
+    switch_to_state(grbl_machine_state::idle);
+    executed_instructions = 0;
 }
 
 void grbl::machine::switch_to_state(grbl::grbl_machine_state new_state) {
@@ -884,13 +812,79 @@ void grbl::machine_state_run_program::on_disconnected(machine *m) {
 }
 
 void grbl::machine_state_run_program::on_enter(grbl::machine *m) {
+    cnc = m;
 
+    stage = run_stage::start;
+    run_failed = false;
+    run_error = 0;
+    move_to_next_run_stage();
 }
 
 void grbl::machine_state_run_program::on_exit(grbl::machine *m) {
 
 }
 
-void grbl::machine_state_run_program::on_line_received(std::string line) {
-
+void grbl::machine_state_run_program::move_to_next_run_stage() {
+    switch (stage) {
+        case run_stage::start:
+            cnc->executed_instructions = 0;
+            continue_program();
+            stage = run_stage::run_program;
+            break;
+        case run_stage::run_program:
+            if (run_failed || !continue_program()) {
+                if (run_failed) {
+                    cnc->listener->on_run_completed(false, cnc->executed_instructions - 1, run_error);
+                } else {
+                    cnc->listener->on_run_completed(true, 0, 0);
+                }
+                cnc->switch_to_state(grbl_machine_state::idle);
+            }
+            break;
+    }
+}
+
+bool grbl::machine_state_run_program::continue_program() {
+    if (cnc->executed_instructions >= cnc->running_program.number_of_instructions())
+        return false;
+
+    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());
+
+    if (to_send.type == instruction_type::gcode) {
+        cnc->pipe->send(to_send.command);
+        cnc->awaiting_responses++;
+        return true;
+    }
+
+    return false;
+}
+
+void grbl::machine_state_run_program::on_line_received(std::string line) {
+    if (starts_with(line, "ok")) {
+        if (cnc->awaiting_responses > 0) {
+            cnc->awaiting_responses--;
+        }
+        move_to_next_run_stage();
+    } else if (starts_with(line, "error")) {
+        if (cnc->awaiting_responses > 0) {
+            cnc->awaiting_responses--;
+        }
+        run_failed = true;
+        run_error = std::stoi(line.substr(6));
+        move_to_next_run_stage();
+
+    } else if (starts_with(line, "Grbl")) {
+        cnc->listener->on_banner(line);
+        cnc->reset_machine_state();
+    } else if (starts_with(line, "<")) {
+        cnc->last_report = parse_status_report(line, cnc->last_report);
+        cnc->listener->on_realtime_status_report(cnc->last_report);
+    } else if (starts_with(line, "[MSG:")) {
+        cnc->listener->on_message(line.substr(5, line.size() - 6));
+    } else if (starts_with(line, "ALARM:")) {
+        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+    }
 }
diff --git a/grbl_machine.h b/grbl_machine.h
index b2ec59b..356622c 100644
--- a/grbl_machine.h
+++ b/grbl_machine.h
@@ -131,6 +131,11 @@ enum class check_stage {
     disable_check_mode
 };
 
+enum class run_stage {
+    start,
+    run_program,
+};
+
 struct machine;
 
 struct machine_state {
@@ -179,10 +184,11 @@ struct machine_state_check_program : public machine_state {
     void on_exit(machine *m) override;
     void on_line_received(std::string line) override;
 
-    check_stage stage = check_stage::start;
-    machine *cnc;
     bool continue_program();
     void move_to_next_check_stage();
+
+    check_stage stage = check_stage::start;
+    machine *cnc;
     bool check_failed;
     size_t check_error;
 };
@@ -193,6 +199,14 @@ struct machine_state_run_program : public machine_state {
     void on_enter(machine *m) override;
     void on_exit(machine *m) override;
     void on_line_received(std::string line) override;
+
+    bool continue_program();
+    void move_to_next_run_stage();
+
+    machine* cnc;
+    run_stage stage = run_stage::start;
+    bool run_failed;
+    size_t run_error;
 };
 
 
@@ -259,7 +273,6 @@ protected:
     grbl_machine_state state = grbl_machine_state::disconnected;
     program running_program;
     size_t executed_instructions = 0;
-    void continue_program();
     void reset_machine_state();
 };