#include "grbl.h"
#include "string_utils.h"
#include "gcode_parser.h"
#include "gcode_file.h"

#include <utility>
#include <fstream>
#include <regex>
#include <iostream>

grbl::instruction grbl::instruction::new_gcode(size_t line, std::string cmd, std::string comment) {
    return instruction{
            .line = line,
            .type = instruction_type::gcode,
            .command = std::move(cmd),
            .comment = std::move(comment),
    };
}

grbl::instruction grbl::instruction::new_user_message(size_t line, std::string comment) {
    return instruction{
            .line = line,
            .type = instruction_type::user_message,
            .comment = std::move(comment),
    };
}

grbl::instruction grbl::instruction::new_comment(size_t line, std::string comment) {
    return grbl::instruction{
            .line = line,
            .type = instruction_type::comment,
            .comment = std::move(comment),
    };
}

grbl::program::program(std::string filename) {
    load_from_file(std::move(filename));
}

static auto comment_re = std::regex(R"(^\s*\(([^)]*)\)\s*$)");
static auto gcode_re = std::regex(R"(([a-zA-Z0-9\s.\-]+)\s*(;.*|\(([^)]*)\))?)");

bool grbl::program::load_from_stream(std::istream &in) {
    instructions.clear();

    is_loaded = true;
    size_t line_number = 0;
    for (std::string line; std::getline(in, line);) {
        line_number++;
        if (!line.empty()) {

            std::smatch sm{};
            if (std::regex_match(line, sm, comment_re)) {
                auto comment = sm.str(1);
                instructions.emplace_back(instruction::new_comment(line_number, comment));
            } else if (std::regex_match(line, sm, gcode_re)) {
                auto command = trim(sm.str(1));
                auto comment = trim(sm.str(3));

//                // grblHAL does not support feed rates with decimal points,
//                // so we remove them if possible (only one feed command in this line)
//                static auto decimal_feed_re = std::regex(R"(^(F\d+)\.\d+$)");
//                std::smatch feed_matches{};
//                if (std::regex_match(command, feed_matches, decimal_feed_re)) {
//                    command = feed_matches.str(1);
//                }

                instructions.emplace_back(instruction::new_gcode(line_number, command, comment));
            } else {
                std::cerr << "Failed to parse line " << line << std::endl;
                is_loaded = false;
            }
        }
    }

    return is_loaded;
}

bool grbl::program::load_from_string(const std::string &content) {
    std::stringstream in_stream;
    in_stream << content;

    filename = "";
    is_loaded = load_from_stream(in_stream);
    return is_loaded;
}

bool grbl::program::load_from_file(std::string path) {
    filename = std::move(path);
    is_loaded = false;

    std::ifstream in_file(filename);
    if (!in_file) {
        return false;
    }
    is_loaded = load_from_stream(in_file);
    return is_loaded;
}

std::ostream &operator<<(std::ostream &out, const grbl::instruction_type &t) {
    switch (t) {
        case grbl::instruction_type::gcode:
            out << "gcode";
            break;
        case grbl::instruction_type::comment:
            out << "comment";
            break;
        case grbl::instruction_type::user_message:
            out << "user_message";
            break;
    }
    return out;
}

std::ostream &operator<<(std::ostream &out, const grbl::instruction &i) {
    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment
        << " }";
    return out;
}

void grbl::program::dump(std::ostream &out) {
    for (auto &i: instructions) {
        out << i << std::endl;
    }
}

grbl::program grbl::program::apply_heightmap(grbl::heightmap& grid) {

    double segmentLength = grid.resolution;

    grbl::program result;

    std::vector<command*> new_commands;

    grbl::grbl_parser parser;
    std::ifstream in_file{filename};
    if (in_file) {
        parser.parse(in_file);

        for (auto &c: parser.commands) {
            auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
            if (line != nullptr) {

                // do not split up or modify any lines that are rapid or not fully defined
                if (!line->start_valid ||
                    (!line->position_valid[0] || !line->position_valid[1] || !line->position_valid[2]) || line->rapid) {
                    new_commands.push_back(line);
                    continue;
                }

                for (auto &m: line->split(segmentLength)) {
                    m->start.z += grid.get_z_at(m->start.x, m->start.y);
                    m->end.z += grid.get_z_at(m->end.x, m->end.y);

                    new_commands.push_back(m);
                }

            } else if (arc != nullptr) {
                if (arc->plane != arc_plane::xy) {
                    throw std::runtime_error(
                            "GCode contains arcs in YZ or XZ plane (G18/19), can't apply height map. Use 'Arcs to Lines' if you really need this.");
                }

                for (auto &m: arc->split(segmentLength)) {
                    m->start.z += grid.get_z_at(m->start.x, m->start.y);
                    m->end.z += grid.get_z_at(m->end.x, m->end.y);

                    new_commands.push_back(m);
                }

            } else {
                new_commands.push_back(c);
            }
        }

        grbl::grbl_file file(new_commands);
        auto new_lines = file.get_gcode();
        result.load_from_lines(new_lines);
    }

    return result;

}

bool grbl::program::load_from_lines(std::vector<std::string> lines) {
    is_loaded = true;
    size_t line_number = 0;
    for (auto &line: lines) {
        line_number++;
        if (!line.empty()) {

            std::smatch sm{};
            if (std::regex_match(line, sm, comment_re)) {
                auto comment = sm.str(1);
                instructions.emplace_back(instruction::new_comment(line_number, comment));
            } else if (std::regex_match(line, sm, gcode_re)) {
                auto command = trim(sm.str(1));
                auto comment = trim(sm.str(3));
                instructions.emplace_back(instruction::new_gcode(line_number, command, comment));
            } else {
                std::cerr << "Failed to parse line " << line << std::endl;
                is_loaded = false;
            }
        }
    }

    return is_loaded;
}

void grbl::program::save(std::string path) {
    std::ofstream out_file{path};
    if (out_file) {
        for (auto &i: instructions) {
            switch (i.type) {
                case instruction_type::gcode:
                    out_file << i.command;
                    if (!i.comment.empty())
                        out_file << " (" << i.comment << ")";
                    out_file << std::endl;
                    break;
                default:
                    break;
            }

        }
    }

}