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

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

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::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) {
    std::cout << "checking program (" << pgm.filename << ") with " << pgm.number_of_instructions() << " instructions" << std::endl;

    running_program = pgm;
    entered_check_mode = false;
    executed_instructions = 0;
    state = grbl_machine_state::check_program;
    pipe->send("$C");
}

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

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

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

//    if (state == grbl_machine_state::run_program) {
//        if (line.rfind("ok", 0) == 0) {
//            continue_program();
//        } else if (line.rfind("error", 0) == 0) {
//            std::cerr << "Received error" << std::endl;
//        } else {
//        }
//    } else {
//        // evaluate responses when not running a program
//    }
}

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 {

    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() {
    state = grbl_machine_state::idle;
    executed_instructions = false;
}

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