#include <iostream>
#include <sstream>
#include <utility>
#include <array>
#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;
    states[grbl_machine_state::heightmap_probing] = new machine_state_heightmap_probing;

    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;

    // pin values are always reset when a report arrives
    // if there is no value in the Pn: field then it means
    // no pin is active
    result.signals.value = 0;

    auto l = line.substr(1, line.size() - 2);
    auto pieces = split_string(l, "|");
    for (auto i = 0; i < pieces.size(); i++) {
        if (i == 0) {
            // 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 if (elements[0] == "Pn") {
                for (auto& c: elements[1]) {
                    switch (c) {
                        case 'P':
                            result.signals.bit.probe = true;
                            break;
                        case 'X':
                            result.signals.bit.x_limit = true;
                            break;
                        case 'Y':
                            result.signals.bit.y_limit = true;
                            break;
                        case 'Z':
                            result.signals.bit.z_limit = true;
                            break;
                        case 'D':
                            result.signals.bit.door = true;
                            break;
                        case 'H':
                            result.signals.bit.hold = true;
                            break;
                        case 'R':
                            result.signals.bit.soft_reset = true;
                            break;
                        case 'S':
                            result.signals.bit.cycle_start = true;
                            break;
                        default:
                            std::cerr << "Unknown pin value [" << c << "] when parsing status report" << std::endl;
                            break;
                    }
                }
            } else {
                // not implemented
            }
        }
    }
    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::set_work_offset(std::string work_offset) {
    std::cout << "Setting work offsset " << work_offset << std::endl;
//    if (state != grbl_machine_state::disconnected) {
    pipe->send(work_offset);
//    awaiting_responses++;
//    while (awaiting_responses > 0);

    current_work_offset = work_offset;

    auto pieces = split_string(parameters[work_offset], ",");
    current_work_offset_values[0] = std::stof(pieces[0]);
    current_work_offset_values[1] = std::stof(pieces[1]);
    current_work_offset_values[2] = std::stof(pieces[2]);
//    }

}

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

    set_work_offset(work_offset);

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

    push_event(std::make_shared<machine_event_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);

    push_event(std::make_shared<machine_event_parameters_reloaded>());
}

void grbl::machine::go_to_zero(bool x, bool y, bool z) {
    std::string command = "G0";
    if (x) command += "X0";
    if (y) command += "Y0";
    if (z) command += "Z0";

    while (awaiting_responses > 0);
    pipe->send(command);
    awaiting_responses++;
    while (awaiting_responses > 0);
}

const float *grbl::machine::get_current_work_offset_values() const {
    return current_work_offset_values;
}

std::array<float, 3> grbl::machine::get_work_pos() const {
    std::array<float, 3> result = {
            last_report.machine_pos[0] - current_work_offset_values[0],
            last_report.machine_pos[1] - current_work_offset_values[1],
            last_report.machine_pos[2] - current_work_offset_values[2],
    };
    return result;
}

void grbl::machine::request_jog_fixed(grbl::jog_direction dir, float distance, float feed) {
    std::string command = "$J=G21 G91";
    switch (dir) {
        case jog_direction::x_up:
            command += "X";
            break;
        case jog_direction::x_down:
            command += "X-";
            break;
        case jog_direction::y_up:
            command += "Y";
            break;
        case jog_direction::y_down:
            command += "Y-";
            break;
        case jog_direction::z_up:
            command += "Z";
            break;
        case jog_direction::z_down:
            command += "Z-";
            break;
    }
    command += std::to_string(distance) + " F" + std::to_string(feed);

    pipe->send(command);
    awaiting_responses++;
}

void grbl::machine::start_z_probe(float min_z, float feed_rate) {
    std::string command = "G38.2 Z" + std::to_string(min_z) + "F" + std::to_string(feed_rate);
    pipe->send(command);
    awaiting_responses++;
}

void grbl::machine::probe_heightmap(grbl::heightmap& grid) {
    std::cout << "probing heightmap" << std::endl;
    dynamic_cast<machine_state_heightmap_probing *>(states[grbl_machine_state::heightmap_probing])->grid = &grid;
    switch_to_state(grbl_machine_state::heightmap_probing);
}

void grbl::machine::push_event(std::shared_ptr<machine_event> event) {
    std::scoped_lock<std::mutex> lock(event_mutex);
    events.push_back(event);
}

std::shared_ptr<grbl::machine_event> grbl::machine::pop_event() {
    // TODO: make this more efficient
    std::scoped_lock<std::mutex> lock(event_mutex);
    if (events.empty())
        return nullptr;
    else {
        auto result = events.front();
        events.pop_front();
        return result;
    }
}

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

// 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->push_event(std::make_shared<machine_event_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->push_event(std::make_shared<machine_event_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=511"); // machine pos in report, please. also sensors and buffers info
            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->push_event(std::make_shared<machine_event_banner>(line));
        cnc->reset_machine_state();
    } else if (starts_with(line, "<")) {
        cnc->last_report = parse_status_report(line, cnc->last_report);
        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
    } else if (starts_with(line, "[MSG:")) {
        auto message = line.substr(5, line.size() - 6);
        cnc->push_event(std::make_shared<machine_event_message>(message));
    } else if (starts_with(line, "ALARM:")) {
        auto alarm = std::stoi(line.substr(6));
        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
    } else if (starts_with(line, "$")) {
        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];

        if (starts_with(line, "PRB")) {
            auto pieces = split_string(line, ":");
            auto coords_as_string = split_string(pieces[1], ",");

            float probe_coords[3];
            for (auto i = 0; i < 3; i++) {
                probe_coords[i] = std::stof(coords_as_string[i]);
            }
            bool probe_touched = pieces[2] == "1";
            cnc->push_event(std::make_shared<machine_event_probe_result>(probe_touched, probe_coords));
        }
    }
}

// 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) {
                bool success = false;
                size_t failed_idx = cnc->executed_instructions - 1;
                size_t error = check_error;
                cnc->push_event(std::make_shared<machine_event_check_completed>(success, failed_idx, error));
            } else {
                bool success = true;
                size_t failed_idx = 0;
                size_t error = 0;
                cnc->push_event(std::make_shared<machine_event_check_completed>(success, failed_idx, error));
            }
            cnc->switch_to_state(grbl_machine_state::idle);
            break;
    }
}

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->push_event(std::make_shared<machine_event_banner>(line));
        cnc->reset_machine_state();
    } else if (starts_with(line, "<")) {
        cnc->last_report = parse_status_report(line, cnc->last_report);
        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
    } else if (starts_with(line, "[MSG:")) {
        auto message = line.substr(5, line.size() - 6);
        cnc->push_event(std::make_shared<machine_event_message>(message));
    } else if (starts_with(line, "ALARM:")) {
        auto alarm = std::stoi(line.substr(6));
        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
    }
}

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) {
                    bool success = false;
                    size_t failed_index = cnc->executed_instructions - 1;
                    size_t error = run_error;
                    cnc->push_event(std::make_shared<machine_event_run_completed>(success, failed_index, error));
                } else {
                    bool success = true;
                    size_t failed_index = 0;
                    size_t error = 0;
                    cnc->push_event(std::make_shared<machine_event_run_completed>(success, failed_index, error));
                }
                cnc->switch_to_state(grbl_machine_state::idle);
            }
            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->push_event(std::make_shared<machine_event_banner>(line));
        cnc->reset_machine_state();
    } else if (starts_with(line, "<")) {
        cnc->last_report = parse_status_report(line, cnc->last_report);
        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
    } else if (starts_with(line, "[MSG:")) {
        auto message = line.substr(5, line.size() - 6);
        cnc->push_event(std::make_shared<machine_event_message>(message));
    } else if (starts_with(line, "ALARM:")) {
        auto alarm = std::stoi(line.substr(6));
        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
    }
}

// heightmap probing
void grbl::machine_state_heightmap_probing::on_connected(grbl::machine *m) {

}

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

}

void grbl::machine_state_heightmap_probing::on_enter(grbl::machine *m) {
    cnc = m;
    stage = heightmap_probing_stage::start;
    failed = false;
    error = 0;
    probed_locations = 0;
    move_to_next_stage();
}

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

}

void grbl::machine_state_heightmap_probing::on_line_received(std::string line) {
    if (starts_with(line, "ok")) {
        if (cnc->awaiting_responses > 0) {
            cnc->awaiting_responses--;
        }
        move_to_next_stage();
    } else if (starts_with(line, "error")) {
        if (cnc->awaiting_responses > 0) {
            cnc->awaiting_responses--;
        }
        failed = true;
        error = std::stoi(line.substr(6));
        move_to_next_stage();
    } else if (starts_with(line, "Grbl")) {
        cnc->push_event(std::make_shared<machine_event_banner>(line));
        cnc->reset_machine_state();
    } else if (starts_with(line, "<")) {
        cnc->last_report = parse_status_report(line, cnc->last_report);
        cnc->push_event(std::make_shared<machine_event_report_received>(cnc->last_report));
    } else if (starts_with(line, "[MSG:")) {
        auto message = line.substr(5, line.size() - 6);
        cnc->push_event(std::make_shared<machine_event_message>(message));
    } else if (starts_with(line, "ALARM:")) {
        auto alarm = std::stoi(line.substr(6));
        cnc->push_event(std::make_shared<machine_event_alarm>(alarm));
    } else if (starts_with(line, "[PRB")) {
        std::cout << "received PRB" << std::endl;
        line = line.substr(1, line.size() - 2);
        auto pieces = split_string(line, ":");
        cnc->parameters[pieces[0]] = pieces[1] + ":" + pieces[2];
    }
}

bool grbl::machine_state_heightmap_probing::continue_program() {
    return false;
}

void grbl::machine_state_heightmap_probing::move_to_next_stage() {
    switch (stage) {
        case heightmap_probing_stage::start:
            cnc->pipe->send("G0X0Y0Z15");
            stage = heightmap_probing_stage::goto_home;
            break;
        case heightmap_probing_stage::goto_home:
            cnc->pipe->send("G38.2 Z-65 F100");
            stage = heightmap_probing_stage::initial_probe_step_back;
            break;
        case heightmap_probing_stage::initial_probe_step_back:
            // step back a bit (1mm, relative)
            cnc->pipe->send("G91 G0 Z1");
            stage = heightmap_probing_stage::initial_probe_fine_seek;
            break;
        case heightmap_probing_stage::initial_probe_fine_seek:
            // probe again but finer
            cnc->pipe->send(" G38.2 Z-5 F5");
            stage = heightmap_probing_stage::initial_probe;
            break;
        case heightmap_probing_stage::initial_probe:
            std::cout << "Initial probe: " << cnc->parameters["PRB"];

            std::cout << ". Setting this as new zero" << std::endl;
            cnc->pipe->send("G10 L20 P1 Z0");

            {
                auto pieces = split_string(cnc->parameters["PRB"], ":");
                auto axes = split_string(pieces[0], ",");
                z_zero_in_mpos = std::stof(axes[2]);
                std::cout << "Z zero in mpos: " << z_zero_in_mpos << std::endl;
                grid->vertices[probed_locations].z = 0;
            }


            stage = heightmap_probing_stage::goto_next_location;
            break;
        case heightmap_probing_stage::goto_next_location: {
            auto pieces = split_string(cnc->parameters["PRB"], ":");
            auto axes = split_string(pieces[0], ",");
            auto current_z = std::stof(axes[2]);
            if (probed_locations == 0) {
                z_zero_in_mpos = current_z;
                std::cout << "Z zero in mpos: " << z_zero_in_mpos << std::endl;
                grid->vertices[probed_locations].z = 0;
            } else {
                auto delta_z = current_z - z_zero_in_mpos;
                std::cout << "Z[" << probed_locations << "]  = " << delta_z << std::endl;
                grid->vertices[probed_locations].z = delta_z;

                cnc->push_event(std::make_shared<machine_event_heightmap_probe_acquired>(grid, probed_locations));
            }
        }

            probed_locations++;
            if (probed_locations == grid->vertices.size()) {
                cnc->pipe->send("G0Z15"); // safe height
                stage = heightmap_probing_stage::done;
            } else {
                cnc->pipe->send("G90 G0 Z1.5");
                stage = heightmap_probing_stage::goto_next_location_move;
            }
            break;
        case heightmap_probing_stage::goto_next_location_move:
            cnc->pipe->send("G0 X" + std::to_string(grid->vertices[probed_locations].x) + " Y" +
                            std::to_string(grid->vertices[probed_locations].y));
            stage = heightmap_probing_stage::goto_start_probing_at;
            break;
        case heightmap_probing_stage::goto_start_probing_at:
            cnc->pipe->send("G0 Z0.5"); // this appears to move Z upwards instead of downwards. why?
            // faking the G0 Z0.5
//            cnc->pipe->send("G91 G0 Z-1"); // 1.5 - 1 = 0.5.//
            stage = heightmap_probing_stage::probing;
            break;
        case heightmap_probing_stage::probing:
            cnc->pipe->send("G38.2 Z-5 F5");
            stage = heightmap_probing_stage::goto_next_location;
            break;
        case heightmap_probing_stage::done:
            cnc->switch_to_state(grbl_machine_state::idle);
            break;
        default:
            break;
    }
}


grbl::machine_event_connect::machine_event_connect() {
    machine_event::type = machine_event_type::connected;
}

grbl::machine_event_disconnect::machine_event_disconnect() {
    machine_event::type = machine_event_type::disconnected;
}

grbl::machine_event_report_received::machine_event_report_received(const grbl::realtime_status_report& r)
        : report(r) {
    machine_event::type = machine_event_type::report_received;
}

grbl::machine_event_banner::machine_event_banner(const std::string& b)
        : banner{b} {
    machine_event::type = machine_event_type::banner;
}

grbl::machine_event_message::machine_event_message(const std::string& m)
        : message{m} {
    machine_event::type = machine_event_type::message;
}

grbl::machine_event_alarm::machine_event_alarm(int code)
        : alarm(code) {
    machine_event::type = machine_event_type::alarm;
}

grbl::machine_event_init_completed::machine_event_init_completed() {
    machine_event::type = machine_event_type::init_completed;
}

grbl::machine_event_run_completed::machine_event_run_completed(bool success, size_t failed_index, size_t error)
        : success(success),
          failed_index(failed_index),
          error(error) {
    machine_event::type = machine_event_type::run_completed;
}

grbl::machine_event_check_completed::machine_event_check_completed(bool success, size_t failed_idx, size_t error)
        : success(success),
          failed_index(failed_idx),
          error(error) {
    machine_event::type = machine_event_type::check_completed;
}

grbl::machine_event_settings_reloaded::machine_event_settings_reloaded() {
    machine_event::type = machine_event_type::settings_reloaded;
}

grbl::machine_event_parameters_reloaded::machine_event_parameters_reloaded() {
    machine_event::type = machine_event_type::parameters_reloaded;
}

grbl::machine_event_probe_result::machine_event_probe_result(bool touched, const float *coords)
        : probe_touched(touched),
          probe_coords{coords[0], coords[1], coords[2]} {
    machine_event::type = machine_event_type::probe_result;
}

grbl::machine_event_heightmap_probe_acquired::machine_event_heightmap_probe_acquired(grbl::heightmap *g, size_t location)
        : grid(g),
          probed_location(location) {
    machine_event::type = machine_event_type::heightmap_probe_acquired;
}