#include <iostream>
#include <sstream>
#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;
    states[grbl_machine_state::init] = new machine_state_init;
    states[grbl_machine_state::idle] = new machine_state_idle;
    states[grbl_machine_state::check_program] = new machine_state_check_program;
    states[grbl_machine_state::run_program] = new machine_state_run_program;

    switch_to_state(grbl_machine_state::disconnected);
}

grbl::machine::~machine() {
    delete pipe;
}

void grbl::machine::connect() {
    pipe->open(*this);
}

void grbl::machine::on_connected(grbl::transport *transport) {
    std::cout << "grbl machine connected" << std::endl;
    states[state]->on_connected(this);
    // telnet handshake so that we get the banner. banner won't be coming otherwise
//        t->send("\xff\xfd\x18\xff\xfd\x20\xff\xfd\x23\xff\xfd\x27");
//    pipe->send("\xff\xfd\x18");
}

void grbl::machine::on_disconnected(grbl::transport *transport) {
    std::cout << "grbl machine disconnected" << std::endl;
    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 result;

    auto l = line.substr(1, -1);
    auto pieces = split_string(l, "|");
    for (auto i = 0; i < pieces.size(); i++) {
        if (i == 0) {
            // status
            auto elements = split_string(pieces[i], ":");
            result.status = status_from_string(elements[0]);
            result.sub_status = elements.size() > 1 ? elements[1] : "";
        } else {
            auto elements = split_string(pieces[i], ":");
            if (elements[0] == "WPos") {
                auto axis = split_string(elements[1], ",");
                result.work_pos[0] = std::stof(axis[0]);
                result.work_pos[1] = std::stof(axis[1]);
                result.work_pos[2] = std::stof(axis[2]);
            } else if (elements[0] == "MPos") {
                auto axis = split_string(elements[1], ",");
                result.machine_pos[0] = std::stof(axis[0]);
                result.machine_pos[1] = std::stof(axis[1]);
                result.machine_pos[2] = std::stof(axis[2]);
            } else if (elements[0] == "Bf") {
                auto p = split_string(elements[1], ",");
                result.buffers_free = std::stoi(p[0]);
                result.rx_chars_free = std::stoi(p[1]);
            } else {
                // not implemented
            }
        }
    }
    return result;
}


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;
    if (status == "Jog") return machine_status::jog;
    if (status == "Alarm") return machine_status::alarm;
    if (status == "Door") return machine_status::door;
    if (status == "Check") return machine_status::check;
    if (status == "Home") return machine_status::home;
    if (status == "Sleep") return machine_status::sleep;
    if (status == "Tool") return machine_status::tool;
    return machine_status::unknown;
}

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()
              << " instructions" << std::endl;
    switch_to_state(grbl_machine_state::check_program);
}

void grbl::machine::run_program(const grbl::program& pgm, std::string work_offset) {
    running_program = pgm;
    std::cout << "running program (" << running_program.filename << ") with " << running_program.number_of_instructions() << " instructions"
              << " on work offset " << work_offset << std::endl;

    // set desired offset
    while (awaiting_responses > 0);
    pipe->send(work_offset);
    while (awaiting_responses > 0);

    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:
            return "Idle";
        case machine_status::run:
            return "Run";
        case machine_status::hold:
            return "Hold";
        case machine_status::jog:
            return "Jog";
        case machine_status::alarm:
            return "Alarm";
        case machine_status::door:
            return "Door";
        case machine_status::check:
            return "Check";
        case machine_status::home:
            return "Home";
        case machine_status::sleep:
            return "Sleep";
        case machine_status::tool:
            return "Tool";
        case machine_status::unknown:
        default:
            return "Unknown";
    }
}

std::string grbl::alarm_to_string(int alarm) {
    switch (alarm) {
        case 1:
            return "Hard limit has been triggered.\nMachine position is likely lost due to sudden halt.\nRe-homing is highly recommended.";
        case 2:
            return "Soft limit alarm. G-code motion target exceeds\nmachine travel. Machine position retained.\nAlarm may be safely unlocked.";
        case 3:
            return "Reset while in motion. Machine position is likely lost due to sudden halt. Re-homing is highly recommended.";
        case 4:
            return "Probe fail. Probe is not in the expected initial state before starting probe cycle when G38.2 and G38.3 is not triggered and G38.4 and G38.5 is triggered.";
        case 5:
            return "Probe fail. Probe did not contact the workpiece within the programmed travel for G38.2 and G38.4.";
        case 6:
            return "Homing fail. The active homing cycle was reset.";
        case 7:
            return "Homing fail. Safety door was opened during homing cycle.";
        case 8:
            return "Homing fail. Pull off travel failed to clear limit switch. Try increasing pull-off setting or check wiring.";
        case 9:
            return "Homing fail. Could not find limit switch within search distances. Try increasing max travel, decreasing pull-off distance, or check wiring.";
        case 10:
            return "Homing fail. Second dual axis limit switch failed to trigger within configured search distance after first. Try increasing trigger fail distance or check wiring.";
        default:
            return "unknown alarm code";
    }
}

std::string grbl::error_to_string(size_t error) {
    switch (error) {
        case 1:
            return "G-code words consist of a letter and a value. Letter was not found.";
        case 2:
            return "Missing the expected G-code word value or numeric value format is not valid.";
        case 3:
            return "Grbl '$' system command was not recognized or supported.";
        case 4:
            return "Negative value received for an expected positive value.";
        case 5:
            return "Homing cycle failure. Homing is not enabled via settings.";
        case 6:
            return "Minimum step pulse time must be greater than 3usec.";
        case 7:
            return "An EEPROM read failed. Auto-restoring affected EEPROM to default values.";
        case 8:
            return "Grbl '$' command cannot be used unless Grbl is IDLE. Ensures smooth operation during a job.";
        case 9:
            return "G-code commands are locked out during alarm or jog state.";
        case 10:
            return "Soft limits cannot be enabled without homing also enabled.";
        case 11:
            return "Max characters per line exceeded. Received command line was not executed.";
        case 12:
            return "Grbl '$' setting value cause the step rate to exceed the maximum supported.";
        case 13:
            return "Safety door detected as opened and door state initiated.";
        case 14:
            return "Build info or startup line exceeded EEPROM line length limit. Line not stored.";
        case 15:
            return "Jog target exceeds machine travel. Jog command has been ignored.";
        case 16:
            return "Jog command has no '=' or contains prohibited g-code.";
        case 17:
            return "Laser mode requires PWM output.";
        case 20:
            return "Unsupported or invalid g-code command found in block.";
        case 21:
            return "More than one g-code command from same modal group found in block.";
        case 22:
            return "Feed rate has not yet been set or is undefined.";
        case 23:
            return "G-code command in block requires an integer value.";
        case 24:
            return "More than one g-code command that requires axis words found in block.";
        case 25:
            return "Repeated g-code word found in block.";
        case 26:
            return "No axis words found in block for g-code command or current modal state which requires them.";
        case 27:
            return "Line number value is invalid.";
        case 28:
            return "G-code command is missing a required value word.";
        case 29:
            return "G59.x work coordinate systems are not supported.";
        case 30:
            return "G53 only allowed with G0 and G1 motion modes.";
        case 31:
            return "Axis words found in block when no command or current modal state uses them.";
        case 32:
            return "G2 and G3 arcs require at least one in-plane axis word.";
        case 33:
            return "Motion command target is invalid.";
        case 34:
            return "Arc radius value is invalid.";
        case 35:
            return "G2 and G3 arcs require at least one in-plane offset word.";
        case 36:
            return "Unused value words found in block.";
        case 37:
            return "G43.1 dynamic tool length offset is not assigned to configured tool length axis.";
        case 38:
            return "Tool number greater than max supported value.";
        default:
            return "unknown error";
    }
}

std::string grbl::setting_description(int number) {
    switch (number) {
        case 0:
            return "Step pulse time (microseconds)\nSets time length per step. Minimum 3 microseconds.";
        case 1:
            return "Step idle delay (milliseconds)\nSets a short hold delay when stopping to let dynamics settle before disabling steppers. Value 255 keeps motors enabled.";
        case 2:
            return "Step pulse invert (mask)\nInverts the step signals (active low).";
        case 3:
            return "Step direction invert (mask)\nInverts the direction signals (active low).";
        case 4:
            return "Invert step enable pin (boolean)\nInverts the stepper driver enable signals  (active low). If the stepper drivers shares the same enable signal only X is used.";
        case 5:
            return "Invert limit pins (mask)\nInverts the axis limit input signals. ";
        case 6:
            return "Invert probe pin (boolean)\nInverts the probe input pin signal.";
        case 10:
            return "Status report options (mask)\nSpecifies optional data included in status reports.";
        case 11:
            return "Junction deviation (mm)\nSets how fast Grbl travels through consecutive motions. Lower value slows it down.";
        case 12:
            return "Arc tolerance (mm)\nSets the G2 and G3 arc tracing accuracy based on radial error. Beware: A very small value may effect performance.";
        case 13:
            return "Report in inches (boolean)\nEnables inch units when returning any position and rate value that is not a settings value.";
        case 14:
            return "Invert control pins (mask)\nInverts the control signals  (active low).";
        case 15:
            return "Invert coolant pins (mask)\nInverts the coolant and mist signals  (active low).";
        case 16:
            return "Invert spindle signals (mask)\nInverts the spindle on counterclockwise and PWM signals (active low).";
        case 17:
            return "Pullup disable control pins (mask)\nDisable the control signals pullup resistors. Potentially enables pulldown resistor if available.";
        case 18:
            return "Pullup disable limit pins (mask)\nDisable the limit signals pullup resistors. Potentially enables pulldown resistor if available.";
        case 19:
            return "Pullup disable probe pin (boolean)\nDisable the probe signal pullup resistor. Potentially enables pulldown resistor if available.";
        case 20:
            return "Soft limits enable (boolean)\nEnables soft limits checks within machine travel and sets alarm when exceeded. Requires homing.";
        case 21:
            return "Hard limits enable (mask)\nWhen enabled immediately halts motion and throws an alarm when switch is triggered. In strict mode only homing is possible after switch is triggered. ";
        case 22:
            return "Homing cycle enable (boolean)\nEnables homing cycle. Requires limit switches on all axes.";
        case 23:
            return "Homing direction invert (mask)\nHoming searches for a switch in the positive direction. Set axis bit to search in negative direction.";
        case 24:
            return "Homing locate feed rate (mm/min)\nFeed rate to slowly engage limit switch to determine its location accurately.";
        case 25:
            return "Homing search seek rate (mm/min)\nSeek rate to quickly find the limit switch before the slower locating phase.";
        case 26:
            return "Homing switch debounce delay (milliseconds)\nSets a short delay between phases of homing cycle to let a switch debounce.";
        case 27:
            return "Homing switch pull-off distance (mm)\nRetract distance after triggering switch to disengage it. Homing will fail if switch isn't cleared.";
        case 28:
            return "G73 Retract distance (mm)\nG73 retract distance (for chip breaking drilling).";
        case 29:
            return "Pulse delay (microseconds)\nStep pulse delay.";
        case 30:
            return "Maximum spindle speed (RPM)\nMaximum spindle speed. Sets PWM to maximum duty cycle.";
        case 31:
            return "Minimum spindle speed (RPM)\nMinimum spindle speed. Sets PWM to minimum duty cycle.";
        case 32:
            return "Mode of operation (integer)\nLaser mode: consecutive G1/2/3 commands will not halt when spindle speed is changed. Lathe mode: allows use of G7, G8, G96 and G97.";
        case 33:
            return "Spindle PWM frequency (Hz)\nSpindle PWM frequency.";
        case 34:
            return "Spindle PWM off value (percent)\nSpindle PWM off value in percent (duty cycle).";
        case 35:
            return "Spindle PWM min value (percent)\nSpindle PWM min value in percent (duty cycle).";
        case 36:
            return "Spindle PWM max value (percent)\nSpindle PWM max value in percent (duty cycle).";
        case 37:
            return "Steppers deenergize  (mask)\nSpecifies which steppers not to disable when stopped.";
        case 38:
            return "Spindle PPR (pulses)\nSpindle encoder pulses per revolution.";
        case 39:
            return "Enable legacy RT commands (boolean)\nEnables \"normal\" processing of ?, ! and ~ characters when part of $-setting or comment. If disabled then they are added to the input string instead.";
        case 40:
            return "Limit jog commands (boolean)\nLimit jog commands to machine limits for homed axes.";
        case 43:
            return "Homing passes, (Number of homing passes. Minimum 1)\n maximum 128.";
        case 44:
            return "Axes homing, first pass (mask)\nAxes to home in first pass.";
        case 45:
            return "Axes homing, second pass (mask)\nAxes to home in second pass.";
        case 46:
            return "Axes homing,third pass (mask)\nAxes to home in third pass.";
        case 47:
            return "Axes homing, fourth pass (mask)\nAxes to home in fourth pass.";
        case 48:
            return "Axes homing, fifthpass (mask)\nAxes to home in fifth pass.";
        case 49:
            return "Axes homing, sixth pass (mask)\nAxes to home in sixth pass.";
        case 50:
            return "Step jog speed  (mm/min)\nStep jogging speed in millimeters per minute.";
        case 51:
            return "Slow jog speed (mm/min)\nSlow jogging speed in millimeters per minute.";
        case 52:
            return "Fast jog speed  (mm/min)\nFast jogging speed in millimeters per minut.";
        case 53:
            return "Step jog distance (mm)\nJog distance for single step jogging.";
        case 54:
            return "Slow jog distance (mm)\nJog distance before automatic stop.";
        case 55:
            return "Fast jog distance (mm)\nJog distance before automatic stop.";
        case 60:
            return "Restore overrides ()\nRestore overrides to default values at program end.";
        case 61:
            return "Ignore door when idle ()\nEnable this if it is desirable to open the safety door when in IDLE mode (eg. for jogging).";
        case 62:
            return "Sleep enable ()\nEnable sleep mode. ";
        case 63:
            return "Disable laser ()\nDisable laser during hold. ";
        case 64:
            return "Force init alarm ()\nStarts Grbl in alarm mode after a cold reset.";
        case 65:
            return "Check limits at init ()\nIf limit switches are engaged after reset this forces Grbl to start in alarm mode.";
        case 66:
            return "Homing init lock, (If homing is enabled)\n homing init lock sets Grbl into an alarm state upon power up. Clear by performing a homing cycle.";
        case 70:
            return "Stream,,Input stream source: 0 - serial, 1 - bluetooth ( 2 - ethernet)\n 3 - WiFi";
        case 71:
            return "WiFi SSID ()\nWiFi SSID.";
        case 72:
            return "WiFi Password ()\nWiFi Password.";
        case 73:
            return "WiFi Port ()\nWiFi Port Number listening for incoming connections.";
        case 74:
            return "Bluetooth device ()\nBluetooth device name.";
        case 75:
            return "Bluetooth service ()\nBluetooth service name.";

        case 80:
            return "Spindle P-gain";
        case 81:
            return "Spindle I-gain";
        case 82:
            return "Spindle D-gain";
        case 84:
            return "Spindle PID max error";
        case 85:
            return "Spindle PID max I error\nSpindle PID max integrator error";

        case 90:
            return "Spindle sync P-gain";
        case 91:
            return "Spindle sync I-gain";
        case 92:
            return "Spindle sync D-gain";
        case 95:
            return "Spindle sync PID max I error\nSpindle sync PID max integrator error";

        case 100:
            return "X-axis travel resolution (step/mm)\nX-axis travel resolution in steps per millimeter.";
        case 101:
            return "Y-axis travel resolution (step/mm)\nY-axis travel resolution in steps per millimeter.";
        case 102:
            return "Z-axis travel resolution (step/mm)\nZ-axis travel resolution in steps per millimeter.";
        case 103:
            return "A-axis travel resolution (step/mm)\nA-axis travel resolution in steps per millimeter.";
        case 104:
            return "B-axis travel resolution (step/mm)\nB-axis travel resolution in steps per millimeter.";
        case 105:
            return "C-axis travel resolution (step/mm)\nC-axis travel resolution in steps per millimeter.";
        case 110:
            return "X-axis maximum rate (mm/min)\nX-axis maximum rate. Used as G0 rapid rate.";
        case 111:
            return "Y-axis maximum rate (mm/min)\nY-axis maximum rate. Used as G0 rapid rate.";
        case 112:
            return "Z-axis maximum rate (mm/min)\nZ-axis maximum rate. Used as G0 rapid rate.";
        case 113:
            return "A-axis maximum rate (mm/min)\nA-axis maximum rate. Used as G0 rapid rate.";
        case 114:
            return "B-axis maximum rate (mm/min)\nB-axis maximum rate. Used as G0 rapid rate.";
        case 115:
            return "C-axis maximum rate (mm/min)\nC-axis maximum rate. Used as G0 rapid rate.";
        case 120:
            return "X-axis acceleration (mm/sec^2)\nX-axis acceleration. Used for motion planning to not exceed motor torque and lose steps.";
        case 121:
            return "Y-axis acceleration (mm/sec^2)\nY-axis acceleration. Used for motion planning to not exceed motor torque and lose steps.";
        case 122:
            return "Z-axis acceleration (mm/sec^2)\nZ-axis acceleration. Used for motion planning to not exceed motor torque and lose steps.";
        case 123:
            return "A-axis acceleration (mm/sec^2)\nA-axis acceleration. Used for motion planning to not exceed motor torque and lose steps.";
        case 124:
            return "B-axis acceleration (mm/sec^2)\nB-axis acceleration. Used for motion planning to not exceed motor torque and lose steps.";
        case 125:
            return "C-axis acceleration (mm/sec^2)\nC-axis acceleration. Used for motion planning to not exceed motor torque and lose steps.";
        case 130:
            return "X-axis maximum travel (mm)\nMaximum X-axis travel distance from homing switch. Determines valid machine space for soft-limits and homing search distances.";
        case 131:
            return "Y-axis maximum travel (mm)\nMaximum Y-axis travel distance from homing switch. Determines valid machine space for soft-limits and homing search distances.";
        case 132:
            return "Z-axis maximum travel (mm)\nMaximum Z-axis travel distance from homing switch. Determines valid machine space for soft-limits and homing search distances.";
        case 133:
            return "A-axis maximum travel (mm)\nMaximum A-axis travel distance from homing switch. Determines valid machine space for soft-limits and homing search distances.";
        case 134:
            return "B-axis maximum travel (mm)\nMaximum B-axis travel distance from homing switch. Determines valid machine space for soft-limits and homing search distances.";
        case 135:
            return "C-axis maximum travel (mm)\nMaximum B-axis travel distance from homing switch. Determines valid machine space for soft-limits and homing search distances.";
        case 160:
            return "X-axis backlash compensation (mm)\nX-axis backlash distance to compensate for.";
        case 161:
            return "Y-axis backlash compensation (mm)\nY-axis backlash distance to compensate for.";
        case 162:
            return "Z-axis backlash compensation (mm)\nZ-axis backlash distance to compensate for.";
        case 163:
            return "A-axis backlash compensation (mm)\nA-axis backlash distance to compensate for.";
        case 164:
            return "B-axis backlash compensation (mm)\nB-axis backlash distance to compensate for.";
        case 165:
            return "C-axis backlash compensation (mm)\nB-axis backlash distance to compensate for.";
        default:
            return "unknown setting";
    }
}

void grbl::machine::on_line_received(std::string line, grbl::transport *transport) {
    if (line.at(0) != '<')
        std::cout << ">> " << line << std::endl;

    states[state]->on_line_received(line);
}

void grbl::machine::request_jog(jog_state jog) const {

    cancel_jog();
    if (jog.no_jogging()) {
        return;
    }

    std::stringstream ss;
    ss << "$J=G91 G21 ";

    if (jog.left_pressed)
        ss << " X-1000";
    else if (jog.right_pressed)
        ss << " X1000";

    if (jog.up_pressed)
        ss << " Y1000";
    else if (jog.down_pressed)
        ss << " Y-1000";

    if (jog.z_up_pressed)
        ss << " Z1000";
    else if (jog.z_down_pressed)
        ss << " Z-1000";

    if (jog.speed_fast_pressed) {
        ss << " F10000";
    } else if (jog.speed_slow_pressed) {
        ss << " F100";
    } else {
        ss << " F1000";
    }

    pipe->send(ss.str());
}

void grbl::machine::cancel_jog() const {
    pipe->send_single_char_command(0x85);
}

void grbl::machine::set_listener(grbl::machine_listener *l) {
    listener = l;
}

void grbl::machine::request_unlock() {
    pipe->send("$X");
}

void grbl::machine::request_home() {
    pipe->send("$H");
}

void grbl::machine::request_reset() {
    pipe->send_single_char_command(0x18);
}

void grbl::machine::request_cycle_start() {
    pipe->send_single_char_command(0x81);
}

void grbl::machine::request_feed_hold() {
    pipe->send_single_char_command(0x82);
}

void grbl::machine::reset_machine_state() {
    switch_to_state(grbl_machine_state::idle);
    executed_instructions = 0;
}

void grbl::machine::switch_to_state(grbl::grbl_machine_state new_state) {
    std::cout << "Switching from state " << (int) state << " to " << (int) new_state << std::endl;
    std::cout << "Exiting from state" << std::endl;
    states[state]->on_exit(this);
    state = new_state;
    std::cout << "Entering to state" << std::endl;
    states[state]->on_enter(this);
}

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 {
    return parameters;
}

void grbl::machine::zero_offset(int which) {
    while (awaiting_responses > 0);
    pipe->send("G10 L20 P" + std::to_string(1 + which) + " X0 Y0 Z0"); // P1 => G54
    awaiting_responses++;
    while (awaiting_responses > 0);

    pipe->send("$#");
    awaiting_responses++;
    while (awaiting_responses > 0);
    listener->on_parameters_reloaded();
}

void grbl::machine::zero_offset_axis(int offset_index, int axis) {
    while (awaiting_responses > 0);
    pipe->send("G10 L20 P" + std::to_string(1 + offset_index) + " " + char('X' + axis) + "0"); // P1 => G54
    awaiting_responses++;
    while (awaiting_responses > 0);

    pipe->send("$#");
    awaiting_responses++;
    while (awaiting_responses > 0);
    listener->on_parameters_reloaded();
}

bool grbl::jog_state::no_jogging() const {
    return !(up_pressed || down_pressed || left_pressed || right_pressed || z_up_pressed || z_down_pressed);
}

// connect state
void grbl::machine_state_connect::on_connected(machine *m) {
    // trigger a soft reset
//    m->pipe->send("\xff\xfd\x18");
    m->request_reset();
}

void grbl::machine_state_connect::on_disconnected(machine *m) {
}

void grbl::machine_state_connect::on_enter(grbl::machine *m) {
    cnc = m;
}

void grbl::machine_state_connect::on_exit(grbl::machine *m) {
}

void grbl::machine_state_connect::on_line_received(std::string line) {
//    std::cerr << "Should not get content while connecting!" << std::endl;
    if (starts_with(line, "Grbl")) {
        cnc->listener->on_banner(line);
        cnc->switch_to_state(grbl_machine_state::init);
    }
}

// init state
void grbl::machine_state_init::on_enter(grbl::machine *m) {
    cnc = m;
    init_state = init_stage::start;
    move_to_next_init_stage();
}

void grbl::machine_state_init::on_exit(grbl::machine *m) {
    cnc->listener->on_init_completed();
}

void grbl::machine_state_init::on_line_received(std::string line) {
    if (starts_with(line, "ok")) {
        move_to_next_init_stage();
    } else if (starts_with(line, "error")) {
        // TODO: how should we act?
        std::cerr << "Init failed!!!!!!!!!!!!!!!!!! FIXME!" << std::endl;
    } 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")) {
            line = line.substr(1, line.size() - 2);
            // TODO: some parameters have more than two :
            auto pieces = split_string(line, ":");
            cnc->parameters[pieces[0]] = pieces[1];
        }
    }
}

void grbl::machine_state_init::on_connected(machine *m) {
}

void grbl::machine_state_init::on_disconnected(machine *m) {
}

void grbl::machine_state_init::move_to_next_init_stage() {
    switch (init_state) {
        case init_stage::start:
            init_state = init_stage::set_work_pos;
            cnc->pipe->send("$10=1"); // machine pos in report, please
            break;
        case init_stage::set_work_pos:
            init_state = init_stage::fetch_settings;
            cnc->pipe->send("$$");
            break;
        case init_stage::fetch_settings:
            init_state = init_stage::fetch_parameters;
            cnc->pipe->send("$#");
            break;
        case init_stage::fetch_parameters:
            std::cout << "CNC initialization done" << std::endl;
            cnc->switch_to_state(grbl_machine_state::idle);
            break;
    }
}


// idle state
void grbl::machine_state_idle::on_connected(machine *m) {
}

void grbl::machine_state_idle::on_disconnected(machine *m) {

}

void grbl::machine_state_idle::on_enter(grbl::machine *m) {
    cnc = m;
}

void grbl::machine_state_idle::on_exit(grbl::machine *m) {

}

void grbl::machine_state_idle::on_line_received(std::string line) {
    if (starts_with(line, "ok")) {
        if (cnc->awaiting_responses > 0) {
            cnc->awaiting_responses--;
        }
    } else if (starts_with(line, "error")) {
        if (cnc->awaiting_responses > 0) {
            cnc->awaiting_responses--;
        }
    } else if (starts_with(line, "Grbl")) {
        cnc->listener->on_banner(line);
        cnc->reset_machine_state();
    } else if (starts_with(line, "<")) {
        cnc->last_report = parse_status_report(line, cnc->last_report);
        cnc->listener->on_realtime_status_report(cnc->last_report);
    } else if (starts_with(line, "[MSG:")) {
        cnc->listener->on_message(line.substr(5, line.size() - 6));
    } else if (starts_with(line, "ALARM:")) {
        cnc->listener->on_alarm(std::stoi(line.substr(6)));
    } else if (starts_with(line, "$")) {
        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")) {
        line = line.substr(1, line.size() - 2);
        // TODO: some parameters have more than two :
        auto pieces = split_string(line, ":");
        cnc->parameters[pieces[0]] = pieces[1];
    }
}

// check program
void grbl::machine_state_check_program::on_connected(machine *m) {
}

void grbl::machine_state_check_program::on_disconnected(machine *m) {
}

void grbl::machine_state_check_program::on_enter(grbl::machine *m) {
    cnc = m;

    stage = check_stage::start;
    check_failed = false;
    check_error = 0;
    move_to_next_check_stage();
}

void grbl::machine_state_check_program::on_exit(grbl::machine *m) {
}

void grbl::machine_state_check_program::move_to_next_check_stage() {
    switch (stage) {
        case check_stage::start:
            while (cnc->awaiting_responses > 0);
            cnc->pipe->send("$C");
            cnc->awaiting_responses++;

            cnc->executed_instructions = 0;
            stage = check_stage::enable_check_mode;
            break;
        case check_stage::enable_check_mode:
            continue_program();
            stage = check_stage::run_program;
            break;
        case check_stage::run_program:
            if (check_failed || !continue_program()) {
                stage = check_stage::disable_check_mode;

                while (cnc->awaiting_responses > 0);
                std::cout << "disabling $C mode" << std::endl;
                cnc->pipe->send("$C");
            }
            break;
        case check_stage::disable_check_mode:
            if (check_failed) {
                cnc->listener->on_check_completed(false, cnc->executed_instructions - 1, check_error);
            } else {
                cnc->listener->on_check_completed(true, 0, 0);
            }
            cnc->switch_to_state(grbl_machine_state::idle);
            break;
    }
}

void grbl::machine_state_check_program::on_line_received(std::string line) {
    if (starts_with(line, "ok")) {
        if (cnc->awaiting_responses > 0) {
            cnc->awaiting_responses--;
        }
        move_to_next_check_stage();
    } else if (starts_with(line, "error")) {
        if (cnc->awaiting_responses > 0) {
            cnc->awaiting_responses--;
        }
        check_failed = true;
        check_error = std::stoi(line.substr(6));
        move_to_next_check_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)));
    }
}

bool grbl::machine_state_check_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;
}


// run program
void grbl::machine_state_run_program::on_connected(machine *m) {

}

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::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)));
    }
}