#include <gtest/gtest.h>

#include "grbl.h"
#include "grbl_machine.h"
#include "gcode_parser.h"
#include "glm/gtx/string_cast.hpp"

TEST(grbl_program, default_state) {
    grbl::program pgm;

    EXPECT_EQ("", pgm.filename);
    EXPECT_EQ(0, pgm.number_of_instructions());
    EXPECT_EQ(false, pgm.is_loaded);
}

TEST(grbl_program, load_from_string) {
    grbl::program pgm;

    std::string content = R"(( pcb2gcode 1.2.2 )
( Software-independent Gcode )

G94 ( Millimeters per minute feed rate. )
G21 ( Units == Millimeters. )
F600.00000 ; another type of comment
X3.87739 Y78.52820
)";
    pgm.load_from_string(content);

    EXPECT_EQ("", pgm.filename);
    EXPECT_EQ(6, pgm.number_of_instructions());
    EXPECT_EQ(true, pgm.is_loaded);
}

TEST(grbl_status_report, parse) {
    auto content = "<Idle|MPos:0.000,1.000,2.000|Bf:35,1022|FS:0,0|Pn:Z|Ov:100,100,100>";
    grbl::realtime_status_report r;
    grbl::parse_status_report(content, r);

    EXPECT_EQ(grbl::machine_status::idle, r.status);
    EXPECT_EQ("", r.sub_status);

    EXPECT_EQ(0, r.machine_pos[0]);
    EXPECT_EQ(1, r.machine_pos[1]);
    EXPECT_EQ(2, r.machine_pos[2]);

    EXPECT_EQ(35, r.buffers_free);

    //
    grbl::parse_status_report("<Home|MPos:0.000,0.000,0.000|Bf:35,1022|FS:0,0|Pn:PZ|Ov:100,100,100>", r);
    EXPECT_EQ(grbl::machine_status::home, r.status);
    EXPECT_EQ(1022, r.rx_chars_free);

    EXPECT_EQ(true, r.signals.bit.probe);

    EXPECT_EQ(false, r.signals.bit.x_limit);
    EXPECT_EQ(false, r.signals.bit.y_limit);
    EXPECT_EQ(true, r.signals.bit.z_limit);
}

TEST(grbl_parser, parse) {
    std::string content = R"(G17 G21 G90 G94 G54
G0 Z0.25
X-0.5 Y0.
Z0.1
G01 Z0. F5.
G02 X0. Y0.5 I0.5 J0. F2.5
X0.5 Y0. I0. J-0.5
X0. Y-0.5 I-0.5 J0.
X-0.5 Y0. I0. J0.5
G01 Z0.1 F5.
G00 X0. Y0. Z0.25
)";
    std::stringstream ss;
    ss << content;

    grbl::grbl_parser parser;
    try {
        parser.parse(ss);
    } catch (std::exception &e) {
        std::cerr << e.what();
    }

    for (auto &c: parser.commands) {
        auto line = dynamic_cast<grbl::line_motion_cmd *>(c.get());
        if (line != nullptr) {
            std::cout << "Line from " << glm::to_string(line->start) << " to " << glm::to_string(line->end)
                      << std::endl;
            continue;
        }

        auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c.get());
        if (arc != nullptr) {
            std::cout << "Arc from " << glm::to_string(arc->start) << " to " << glm::to_string(arc->end) << std::endl;
            continue;
        }

    }

//    EXPECT_EQ(true, false);
}