#include "gcode_parser.h"

grbl::parse_exception::parse_exception(const std::string &reason, size_t lineNumber)
        : reason(reason), line_number(lineNumber) {}

const char *grbl::parse_exception::what() const noexcept {
    return reason.c_str();
}

struct word {
    char command;
    double parameter;

    std::string to_string() const {
        std::string result;
        result += command;
        result += std::to_string(parameter);
        return result;
    }
};

static std::array<double, 4> motion_commands = {0, 1, 2, 3};
static std::string valid_words = "GMXYZIJKFRSP";
static std::string ignore_axes = "ABC";

std::string grbl::grbl_parser::cleanup_line(std::string line, int line_number) {
    auto comment_index = line.rfind(';');
    if (comment_index != std::string::npos) {
        line = line.substr(0, comment_index);
    }

    size_t start;
    while ((start = line.find('(')) != std::string::npos) {
        auto end = line.find(')');
        if (end == std::string::npos || end < start)
            throw parse_exception("mismatched parentheses", line_number);

        line.erase(start, end - start + 1);
    }

    return line;
}

void grbl::grbl_parser::parse(std::istream &in) {
    int i = 0;
    for (std::string line; std::getline(in, line);) {
        i++;
        line = trim(cleanup_line(line, i));
        if (line.empty()) continue;

        parse(to_upper(line), i);
    }
}

bool grbl::grbl_parser::is_motion_command(double param) {
    for (auto &p: motion_commands) {
        if (p == param)
            return true;
    }
    return false;
}

void grbl::grbl_parser::parse(std::string line, int line_number) {
    static auto gcode_re = std::regex(R"([a-zA-Z]\s*\-?\d*\.?\d*)");

    std::vector<word> words;

    std::smatch matches;
    std::string::const_iterator start(line.cbegin());
    while (regex_search(start, line.cend(), matches, gcode_re)) {
        auto token = remove_spaces(matches[0]);

        words.push_back(
                word{
                        .command= token[0],
                        .parameter = std::stod(token.substr(1))
                }
        );

        start = matches.suffix().first;
    }

    auto it = words.begin();
    while (it != words.end()) {
        if ((*it).command == 'N') {
            it = words.erase(it);
            continue;
        }

        bool ignore_additional_axes = true;
        if (ignore_axes.find((*it).command) != std::string::npos && ignore_additional_axes) {
            it = words.erase(it);
            continue;
        }

        if (valid_words.find((*it).command) == std::string::npos) {
            warnings.push_back("ignoring unknown word (letter): \"" + (*it).to_string() + "\". (line " +
                               std::to_string(line_number) + ")");
            it = words.erase(it);
            continue;
        }

        if ((*it).command != 'F') {
            it++;
            continue;
        }

        state.feed = (*it).parameter;
        if (state.unit == parse_unit::imperial)
            state.feed *= 25.4;

        it = words.erase(it);
    }


    it = words.begin();
    while (it != words.end()) {
        if ((*it).command == 'M') {
            int param = (int) (*it).parameter;
            if (param != (*it).parameter || param < 0) {
                throw parse_exception("M code can only have positive integer parameters", line_number);
            }

            commands.push_back(std::make_shared<mcode_cmd>(param, line_number));
            it = words.erase(it);
            continue;
        }

        if ((*it).command == 'S') {
            double param = (*it).parameter;
            if (param < 0) {
                warnings.push_back("spindle speed must be positive. (line " + std::to_string(line_number) + ")");
            }

            commands.push_back(std::make_shared<spindle_cmd>(std::abs(param), line_number));
            it = words.erase(it);
            continue;
        }


        if ((*it).command == 'G' && !is_motion_command((*it).parameter)) {
            auto param = (*it).parameter;

            if (param == 90) {
                state.distance_mode = parse_distance_mode::absolute;
                it = words.erase(it);
                continue;
            }

            if (param == 91) {
                state.distance_mode = parse_distance_mode::incremental;
                it = words.erase(it);
                continue;
            }

            if (param == 90.1) {
                state.arc_distance_mode = parse_distance_mode::absolute;
                it = words.erase(it);
                continue;
            }

            if (param == 91.1) {
                state.arc_distance_mode = parse_distance_mode::incremental;
                it = words.erase(it);
                continue;
            }

            if (param == 21) {
                state.unit = parse_unit::metric;
                it = words.erase(it);
                continue;
            }

            if (param == 20) {
                state.unit = parse_unit::imperial;
                it = words.erase(it);
                continue;
            }

            if (param == 17) {
                state.plane = arc_plane::xy;
                it = words.erase(it);
                continue;
            }

            if (param == 18) {
                state.plane = arc_plane::zx;
                it = words.erase(it);
                continue;
            }

            if (param == 19) {
                state.plane = arc_plane::yz;
                it = words.erase(it);
                continue;
            }

            if (param == 4) {
                auto next = it;
                next++;
                if (next != words.end() && (*next).command == 'P') {
                    if ((*next).parameter < 0) {
                        warnings.push_back("dwell time must be positive. (line " + std::to_string(line_number) + ")");
                    }

                    commands.push_back(std::make_shared<dwell_cmd>(std::abs((*next).parameter), line_number));
                    it = words.erase(it);
                    it = words.erase(it);
                    continue;
                }
            }

            // we do not support G64, only G61
            if (param == 64) {
                warnings.push_back("G64 not supported. Ignoring. (line " + std::to_string(line_number) + ")");
                auto next = it;
                next++;
                if (next != words.end() && (*next).command == 'P') {
                    it = words.erase(it);
                }
                it = words.erase(it);
                continue;
            }

            if (param == 94 || param == 93 || param == 95) {
                // G93 sets feed rate mode to inverse time
                // G94 sets feed rate mode to units per minute (mm/min or inch/min)
                // G95 sets feed rate mode to units per revolution
                it = words.erase(it);
                continue;
            }

            // coordinate system offset
            if (param == 54 || param == 55 || param == 56 || param == 57) {
                it = words.erase(it);
                continue;
            }


            warnings.push_back(
                    "ignoring unknown command " + (*it).to_string() + ". (line " + std::to_string(line_number) + ")");
            it = words.erase(it);
        }

        if (words.empty())
            return;

        int MotionMode = state.last_motion_mode;
        if (words.begin()->command == 'G') {
            MotionMode = (int) words.begin()->parameter;
            state.last_motion_mode = MotionMode;
            words.erase(words.begin());
        }

        if (MotionMode < 0) {
            throw parse_exception("no motion mode active", line_number);
        }

        double unit_multiplier = (state.unit == parse_unit::metric) ? 1 : 25.4;
        glm::vec<3, double> end_pos = state.position;
        auto StartValid = state.position_valid[0] && state.position_valid[1] && state.position_valid[2];
        if (state.distance_mode == parse_distance_mode::incremental && !StartValid) {
            throw parse_exception(
                    "incremental motion is only allowed after an absolute position has been established (eg. with \"G90 G0 X0 Y0 Z5\")",
                    line_number);
        }

        if ((MotionMode == 2 || MotionMode == 3) && !StartValid) {
            throw parse_exception(
                    "arcs (G2/G3) are only allowed after an absolute position has been established (eg. with \"G90 G0 X0 Y0 Z5\")",
                    line_number);
        }

        {
            float incremental = (state.distance_mode == parse_distance_mode::incremental) ? 1 : 0;

            it = words.begin();
            while (it != words.end()) {
                if ((*it).command != 'X') {
                    it++;
                    continue;
                }

                end_pos.x = (*it).parameter * unit_multiplier + incremental * end_pos.x;
                it = words.erase(it);
                state.position_valid[0] = true;
                break;
            }

            it = words.begin();
            while (it != words.end()) {
                if ((*it).command != 'Y') {
                    it++;
                    continue;
                }

                end_pos.y = (*it).parameter * unit_multiplier + incremental * end_pos.y;
                it = words.erase(it);
                state.position_valid[1] = true;
                break;
            }

            it = words.begin();
            while (it != words.end()) {
                if ((*it).command != 'Z') {
                    it++;
                    continue;
                }

                end_pos.z = (*it).parameter * unit_multiplier + incremental * end_pos.z;
                it = words.erase(it);
                state.position_valid[2] = true;
                break;
            }
        }

        if (MotionMode != 0 && state.feed <= 0) {
            throw parse_exception("feed rate undefined", line_number);
        }

        if (MotionMode == 1 && !StartValid) {
            warnings.push_back(
                    "a feed move is used before an absolute position is established, height maps will not be applied to this motion. (line " +
                    std::to_string(line_number) + ")");
        }

        if (MotionMode <= 1) {
            if (!words.empty()) {
                warnings.push_back(
                        "motion command must be last in line (ignoring unused words (TOOD: show serialized words)in block). (line " +
                        std::to_string(line_number) + ")");
            }

            auto l = new line_motion_cmd;
            l->start = state.position;
            l->end = end_pos;
            l->feed = state.feed;
            l->rapid = MotionMode == 0;
            l->line_number = line_number;
            l->start_valid = StartValid;

            std::memcpy(l->position_valid, state.position_valid, sizeof(bool) * 3);

            commands.push_back(std::shared_ptr<line_motion_cmd>(l));
            state.position = end_pos;
            return;
        }

        double U, V;
        bool ijk_used = false;

        switch (state.plane) {
            case arc_plane::yz:
                U = state.position.y;
                V = state.position.z;
                break;
            case arc_plane::zx:
                U = state.position.z;
                V = state.position.x;
                break;
            default:
                U = state.position.x;
                V = state.position.y;
                break;
        }

        // find IJK
        {
            float arc_incremental = (state.arc_distance_mode == parse_distance_mode::incremental) ? 1 : 0;

            it = words.begin();
            while (it != words.end()) {
                if ((*it).command != 'I') {
                    it++;
                    continue;
                }

                switch (state.plane) {
                    case arc_plane::xy:
                        U = (*it).parameter * unit_multiplier + arc_incremental * state.position.x;
                        break;
                    case arc_plane::yz:
                        throw parse_exception("current plane is YZ, I word is invalid", line_number);
                    case arc_plane::zx:
                        V = (*it).parameter * unit_multiplier + arc_incremental * state.position.x;
                        break;
                }

                ijk_used = true;
                it = words.erase(it);
                break;
            }

            it = words.begin();
            while (it != words.end()) {
                if ((*it).command != 'J') {
                    it++;
                    continue;
                }

                switch (state.plane) {
                    case arc_plane::xy:
                        V = (*it).parameter * unit_multiplier + arc_incremental * state.position.y;
                        break;
                    case arc_plane::yz:
                        U = (*it).parameter * unit_multiplier + arc_incremental * state.position.y;
                        break;
                    case arc_plane::zx:
                        throw parse_exception("current plane is ZX, J word is invalid", line_number);
                }

                ijk_used = true;
                it = words.erase(it);
                break;
            }

            it = words.begin();
            while (it != words.end()) {
                if ((*it).command != 'K') {
                    it++;
                    continue;
                }

                switch (state.plane) {
                    case arc_plane::xy:
                        throw parse_exception("current plane is XY, K word is invalid", line_number);
                        break;
                    case arc_plane::yz:
                        V = (*it).parameter * unit_multiplier + arc_incremental * state.position.z;
                        break;
                    case arc_plane::zx:
                        U = (*it).parameter * unit_multiplier + arc_incremental * state.position.z;
                        break;
                }

                ijk_used = true;
                it = words.erase(it);
                break;
            }

            // resolve radius
            it = words.begin();
            while (it != words.end()) {
                if ((*it).command != 'R') {
                    it++;
                    continue;
                }

                if (ijk_used) {
                    throw parse_exception("both IJK and R notation used", line_number);
                }

                if (state.position == end_pos) {
                    throw parse_exception("arcs in R-notation must have non-coincident start and end points",
                                          line_number);
                }

                double radius = (*it).parameter * unit_multiplier;
                if (radius == 0)
                    throw parse_exception("radius can't be zero", line_number);

                double A, B;
                switch (state.plane) {
                    case arc_plane::yz:
                        A = end_pos.y;
                        B = end_pos.z;
                        break;
                    case arc_plane::zx:
                        A = end_pos.z;
                        B = end_pos.x;
                        break;
                    default:
                        A = end_pos.x;
                        B = end_pos.y;
                        break;
                }

                A -= U;     //(AB) = vector from start to end of arc along the axes of the current plane
                B -= V;

                // see grbl/gcode.c
                double h_x2_div_d = 4.0 * (radius * radius) - (A * A + B * B);
                if (h_x2_div_d < 0) {
                    throw parse_exception("arc radius too small to reach both ends", line_number);
                }

                h_x2_div_d = -sqrt(h_x2_div_d) / sqrt(A * A + B * B);
                if (MotionMode == 3 ^ radius < 0) {
                    h_x2_div_d = -h_x2_div_d;
                }

                U += 0.5 * (A - (B * h_x2_div_d));
                V += 0.5 * (B + (A * h_x2_div_d));

                it = words.erase(it);
                break;
            }

            if (!words.empty()) {
                warnings.push_back(
                        "motion command must be last in line (ignoring unused words (add serialized words here) in block). (line " +
                        std::to_string(line_number) + ")");
            }

            auto a = new arc_motion_cmd;
            a->start = state.position;
            a->end = end_pos;
            a->feed = state.feed;
            a->direction = (MotionMode == 2) ? arc_direction::cw : arc_direction::ccw;
            a->u = U;
            a->v = V;
            a->line_number = line_number;
            a->plane = state.plane;

            commands.push_back(std::shared_ptr<arc_motion_cmd>(a));
            state.position = end_pos;
            return;
        }

    }
}

grbl::grbl_parser::grbl_parser() {
    reset();
}

void grbl::grbl_parser::reset() {
    state = parser_state{};
}