#include "gcode_file.h"
#include "gcode_parser.h"
#include <fstream>
#include <iomanip>
#include <utility>

grbl::grbl_file grbl::grbl_file::load(std::string path) {
    grbl::grbl_parser parser;

    grbl::grbl_file result{};

    std::ifstream in_file{path};
    if (in_file) {
        parser.parse(in_file);
        result = grbl_file(parser.commands);
    }

    return result;
}

grbl::grbl_file::grbl_file(std::vector<grbl::command*> cmd)
        : commands(cmd) {

}

std::vector<std::string> grbl::grbl_file::get_gcode() {
    std::vector<std::string> result;

    result.emplace_back("G90 G91.1 G21 G17");

    grbl::parser_state state;
    auto xyz = "XYZ";

    std::stringstream ss;
    ss << std::fixed << std::setprecision(3);

    for (auto &c: commands) {
        auto *motion = dynamic_cast<motion_cmd *>(c);
        bool is_motion = motion != nullptr;

        auto *line = dynamic_cast<line_motion_cmd *>(c);
        auto *arc = dynamic_cast<arc_motion_cmd *>(c);
        auto *mcode = dynamic_cast<mcode_cmd *>(c);
        auto *spindle = dynamic_cast<spindle_cmd *>(c);
        auto *dwell = dynamic_cast<dwell_cmd *>(c);

        if (is_motion) {
            if (motion->feed != state.feed) {
                ss.str("");
                ss << "F" << motion->feed;
                result.push_back(ss.str());
                state.feed = motion->feed;
            }
        }

        if (line != nullptr) {
            std::string code = line->rapid ? "G0" : "G1";

            for (int i = 0; i < 3; i++) {
                if (!line->position_valid[i])
                    continue;
                if (!line->start_valid || state.position[i] != line->end[i]) {
                    ss.str("");
                    ss << xyz[i] << line->end[i];
                    code += ss.str();
                }
            }

            result.push_back(code);

            state.position = line->end;
            continue;
        }

        if (arc != nullptr) {
            if (state.plane != arc->plane) {
                switch (arc->plane) {
                    case arc_plane::xy:
                        result.emplace_back("G17");
                        break;
                    case arc_plane::yz:
                        result.emplace_back("G19");
                        break;
                    case arc_plane::zx:
                        result.emplace_back("G18");
                        break;
                }
                state.plane = arc->plane;
            }

            std::string code = arc->direction == arc_direction::cw ? "G2" : "G3";
            if (state.position.x != arc->end.x) {
                ss.str("");
                ss << "X" << arc->end.x;
                code += ss.str();
            }
            if (state.position.y != arc->end.y) {
                ss.str("");
                ss << "Y" << arc->end.y;
                code += ss.str();
            }
            if (state.position.z != arc->end.z) {
                ss.str("");
                ss << "Z" << arc->end.z;
                code += ss.str();
            }

            glm::vec3 center = roll_components({arc->u, arc->v, 0}, (int) arc->plane) - state.position;

            if (center.x != 0 && arc->plane != arc_plane::yz) {
                ss.str("");
                ss << " I" << center.x;
                code += ss.str();
            }
            if (center.y != 0 && arc->plane != arc_plane::zx) {
                ss.str("");
                ss << " J" << center.y;
                code += ss.str();
            }
            if (center.z != 0 && arc->plane != arc_plane::xy) {
                ss.str("");
                ss << " K" << center.z;
                code += ss.str();
            }

            result.push_back(code);
            state.position = arc->end;
            continue;
        }

        if (mcode != nullptr) {
            int code = mcode->code;
            if (code == 2 || code == 30)
                continue;

            result.push_back("M" + std::to_string(code));
            continue;
        }

        if (spindle != nullptr) {
            ss.str("");
            ss << "S" << spindle->speed;
            result.push_back(ss.str());
            continue;
        }

        if (dwell != nullptr) {
            ss.str("");
            ss << "G4 P" << dwell->seconds;
            result.push_back(ss.str());
            continue;
        }
    }

    return result;
}