#include <nanogui/opengl.h>
#include <nanogui/screen.h>
#include <nanogui/window.h>
#include <nanogui/layout.h>
#include <nanogui/label.h>
#include <nanogui/button.h>
#include <nanogui/popupbutton.h>
#include <nanogui/progressbar.h>
#include <nanogui/messagedialog.h>
#include <nanogui/texture.h>
#include <nanogui/textarea.h>
#include <nanogui/textbox.h>
#include <nanogui/tabwidget.h>
#include <nanogui/shader.h>
#include <nanogui/renderpass.h>
#include <iostream>
#include <memory>

#define STB_IMAGE_STATIC
#define STB_IMAGE_IMPLEMENTATION
#if defined(_MSC_VER)
#  pragma warning (disable: 4505) // don't warn about dead code in stb_image.h
#elif defined(__GNUC__)
#   pragma GCC diagnostic ignored "-Wunused-function"
#endif

#include <stb_image.h>
#include "grbl.h"
#include <gtest/gtest.h>
#include <regex>
#include "grbl_machine.h"
#include "string_utils.h"
#include "render.h"
#include "glm/gtx/quaternion.hpp"
#include "nanogui/nanogui.h"
#include "glm/gtc/matrix_inverse.hpp"
#include "heightmap.h"
#include <glm/ext/quaternion_float.hpp>
#include <glm/ext/quaternion_trigonometric.hpp>
#include <glm/vec3.hpp> // glm::vec3
#include <glm/vec4.hpp> // glm::vec4
#include <glm/mat4x4.hpp> // glm::mat4
#include <glm/ext/matrix_transform.hpp> // glm::translate, glm::rotate, glm::scale
#include <glm/ext/matrix_clip_space.hpp> // glm::perspective
#include <glm/ext/scalar_constants.hpp> // glm::pi

using namespace nanogui;

grbl::machine cnc{};

class SenderApp : public Screen, public grbl::machine_listener {
public:

    Window *window;
    grbl::jog_state jog;
    TextBox *txt_status;
    TextBox *txtMessage;
    nanogui::Color color_red = nanogui::Color(255, 0, 0, 255);
    nanogui::Color colGreen = nanogui::Color(0, 255, 0, 255);
    int last_alarm = 0;
    grbl::program pgm;
    Button *btn_load_program, *btn_check_program, *btn_run_program;

    std::vector<std::string> jog_distances = {"0.01", "0.1", "1", "10"};
    std::vector<std::string> jog_feed_rates = {"5", "100", "500", "1000"};

    grbl::program_renderer renderer;

    glm::vec3 cam_target = glm::vec3(0);
    glm::vec3 cam_pan = glm::vec3(0);
    float cam_zoom = 0;
    glm::quat cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0); // identity quaternion

    Widget *info_layer, *heightmap_layer;
    TabWidget *tab_widget;
    VScrollPanel *settings_vscroll;
    Widget *settings_layer;

    VScrollPanel *parameters_vscroll;
    Widget *parameters_layer;
    TextBox *mpos_x_text, *mpos_y_text, *mpos_z_text;
    ComboBox *cbo_work_offset, *cbo_tool, *cbo_jog_feed_rates, *cbo_jog_distance;

    ToolButton *btn_pin_door, *btn_pin_hold, *btn_pin_reset, *btn_pin_cycle_start;
    ToolButton *btn_pin_limit_x, *btn_pin_limit_y, *btn_pin_limit_z, *btn_pin_probe;

    Button *btn_keyboard_jog;
    std::stringstream dro_ss;
    grbl::heightmap heightmap_grid;


    SenderApp() : Screen(Vector2i(1024, 768), "GRBL Sender") {
        inc_ref();

        window = new Window(this, "Machine status");
        window->set_position(Vector2i(0, 0));
        window->set_layout(new BoxLayout(nanogui::Orientation::Vertical));

        tab_widget = window->add<TabWidget>();
        tab_widget->set_tabs_draggable(true);
        tab_widget->set_callback([&](int index) {
            tab_widget->set_selected_index(index);
        });

        info_layer = tab_widget->add<Widget>();
        info_layer->set_layout(new GroupLayout(10, 20, 30, 0));
        info_layer->add<Label>("");

        tab_widget->append_tab("Info", info_layer);

        auto probe_layer = tab_widget->add<Widget>();
        probe_layer->set_layout(new GroupLayout(10, 20, 30, 0));
        probe_layer->add<Label>("");

        add_status_markup();
        add_dro_markup();
        add_program_extents();
        add_pins_markup();
        add_jogging_markup();
        add_work_parameters_markup();
        add_z_probe_markup();

        heightmap_layer = tab_widget->add<Widget>();
        heightmap_layer->set_layout(new GroupLayout(10, 20, 30, 0));
        heightmap_layer->add<Label>("");
        tab_widget->append_tab("Heightmap", heightmap_layer);
        add_heightmap_markup();


        perform_layout();

        m_render_pass = new RenderPass({this}, nullptr, nullptr, nullptr, true);
        m_render_pass->set_clear_color(0, Color(0.13f, 0.13f, 0.13f, 1.f));
        m_render_pass->set_depth_test(RenderPass::DepthTest::Greater, true);
        m_render_pass->set_cull_mode(RenderPass::CullMode::Disabled);
    }

    TextBox *txt_min_z, *txt_feed;

    void add_z_probe_markup() {
        info_layer->add<Label>("Z-Probe", "sans-bold", 20);
        auto z_probe_holder = info_layer->add<Widget>();
        z_probe_holder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 2, 2));

        auto another_holder = z_probe_holder->add<Widget>();
        another_holder->set_layout(new GridLayout(Orientation::Horizontal, 2, Alignment::Fill, 2, 2));

        another_holder->add<Label>("Feed", "sans-bold");
        txt_feed = another_holder->add<TextBox>("100");
        txt_feed->set_editable(true);
        txt_feed->set_fixed_width(100);

        another_holder->add<Label>("Min Z", "sans-bold");
        txt_min_z = another_holder->add<TextBox>("-65");
        txt_min_z->set_editable(true);
        txt_min_z->set_fixed_width(100);

        z_probe_holder->add<Label>("");
        auto btn_start_probing = z_probe_holder->add<Button>("Start probing");
        btn_start_probing->set_callback([&]() {
            cnc.start_z_probe(std::stof(txt_min_z->value()), std::stof(txt_feed->value()));
        });
    }

    void add_work_parameters_markup() {
        // work parameters
        info_layer->add<Label>("Work parameters", "sans-bold", 20);
        auto work_holder = info_layer->add<Widget>();
        work_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Fill, 6, 6));

        auto work_data_holder = work_holder->add<Widget>();
        work_data_holder->set_layout(new GridLayout(Orientation::Horizontal, 2, Alignment::Fill, 6, 6));

        work_data_holder->add<Label>("Offset", "sans-bold");
        std::vector<std::string> offset_items = {"G54", "G55", "G56", "G57", "G58", "G59"};
        cbo_work_offset = work_data_holder->add<ComboBox>(offset_items);
        cbo_work_offset->set_callback([&](int idx) {
            refresh_offset();
            update_dro();
        });

        work_data_holder->add<Label>("Tool", "sans-bold");
        std::vector<std::string> tool_items = {"None"};
        cbo_tool = work_data_holder->add<ComboBox>(tool_items);

        auto work_btn_holder = work_holder->add<Widget>();
        work_btn_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 6));


        btn_load_program = work_btn_holder->add<Button>("Load");
        btn_load_program->set_callback([&] {
            auto path = file_dialog(
                    {{"gcode", "G-Code files"},
                     {"nc",    "G-Code files"},
                     {"ngc",   "G-Code files"}}, true);

            if (pgm.load_from_file(path)) {
                init_program_geometry();
                set_program_extents();
                fill_heightmap_from_model();
                update_grid();
            } else {
            }
        });
        btn_load_program->set_tooltip("Load program");

        btn_check_program = work_btn_holder->add<Button>("Check");
//        btnCheckProgram->set_enabled(false);
        btn_check_program->set_callback([&] {
            cnc.check_program(pgm);
        });
        btn_check_program->set_tooltip("Check program");

        btn_run_program = work_btn_holder->add<Button>("Run");
//        btnRunProgram->set_enabled(false);
        btn_run_program->set_callback([&] {
            cnc.run_program(pgm, "G" + std::to_string(cbo_work_offset->selected_index() + 54));
        });
        btn_run_program->set_tooltip("Execute program");
    }

    void add_status_markup() {
        auto status_holder = info_layer->add<Widget>();
        status_holder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 0, 0));

        auto status_text_holder = status_holder->add<Widget>();
        status_text_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 0));

        status_text_holder->add<Label>("Status", "sans-bold", 20);

        txt_status = status_text_holder->add<TextBox>();
        txt_status->set_editable(false);
        txt_status->set_fixed_width(300);

        auto *btn_reset = status_text_holder->add<Button>("Reset");
        btn_reset->set_background_color(color_red);
        btn_reset->set_callback([&] {
            cnc.request_reset();
        });

        //
        status_text_holder->add<Label>("");

        txtMessage = status_text_holder->add<TextBox>("");
        txtMessage->set_fixed_width(300);

        status_text_holder->add<Label>("");

        // buttons
        auto status_btn_holder = status_holder->add<Widget>();
        status_btn_holder->set_layout(new GridLayout(Orientation::Horizontal, 2, Alignment::Fill, 6, 6));

        auto *btn_homing = new Button(status_btn_holder, "Homing", FA_HOME);
        btn_homing->set_callback([&] {
            cnc.request_home();
        });

        auto *btn_unlock = new Button(status_btn_holder, "Unlock", FA_UNLOCK);
        btn_unlock->set_callback([&] {
            cnc.request_unlock();
        });

        // buttons to change state
        auto *btn_cycle_start = status_btn_holder->add<Button>("Cycle Start", FA_PLAY);
        btn_cycle_start->set_callback([&] {
            cnc.request_cycle_start();
        });
        auto *btn_feed_hold = status_btn_holder->add<Button>("Feed Hold", FA_PAUSE);
        btn_feed_hold->set_callback([&] {
            cnc.request_feed_hold();
        });
    }

    void add_jogging_markup() {
        // jogging
        info_layer->add<Label>("Jogging", "sans-bold", 20);
        auto holder = new Widget(info_layer);

        holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 0));

        // feed and distance
        auto jog_params_holder = holder->add<Widget>();
        jog_params_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 4));

        jog_params_holder->add<Label>("Feed", "sans-bold");
        cbo_jog_feed_rates = jog_params_holder->add<ComboBox>(jog_feed_rates);
        cbo_jog_feed_rates->set_selected_index(2);
        jog_params_holder->add<Label>("mm/min");

        jog_params_holder->add<Label>("Distance", "sans-bold");
        cbo_jog_distance = jog_params_holder->add<ComboBox>(jog_distances);
        cbo_jog_distance->set_selected_index(2);
        jog_params_holder->add<Label>("mm");

        jog_params_holder->add<Label>("Keyboard Jog", "sans-bold");
        btn_keyboard_jog = jog_params_holder->add<Button>("", FA_KEYBOARD);
        btn_keyboard_jog->set_flags(Button::ToggleButton);
        jog_params_holder->add<Label>("");

        // X and Y
        auto jog_btn_holder = holder->add<Widget>();
        jog_btn_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 0, 6));

        jog_btn_holder->add<Label>("");
        auto jog_btn = jog_btn_holder->add<Button>("", FA_ARROW_ALT_CIRCLE_UP);
        jog_btn->set_callback([&]() {
            // FIXME: get rid of std::stof
            cnc.request_jog_fixed(grbl::jog_direction::y_up,
                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
        });
        jog_btn_holder->add<Label>("");

        jog_btn = jog_btn_holder->add<Button>("", FA_ARROW_ALT_CIRCLE_LEFT);
        jog_btn->set_callback([&]() {
            // FIXME: get rid of std::stof
            cnc.request_jog_fixed(grbl::jog_direction::x_down,
                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
        });

        jog_btn = jog_btn_holder->add<Button>("", FA_HOME);
        jog_btn->set_callback([&]() {
            cnc.go_to_zero(true, true, false);
        });


        jog_btn = jog_btn_holder->add<Button>("", FA_ARROW_ALT_CIRCLE_RIGHT);
        jog_btn->set_callback([&]() {
            // FIXME: get rid of std::stof
            cnc.request_jog_fixed(grbl::jog_direction::x_up,
                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
        });

        jog_btn_holder->add<Label>("");
        jog_btn = jog_btn_holder->add<Button>("", FA_ARROW_ALT_CIRCLE_DOWN);
        jog_btn->set_callback([&]() {
            // FIXME: get rid of std::stof
            cnc.request_jog_fixed(grbl::jog_direction::y_down,
                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
        });
        jog_btn_holder->add<Label>("");


        // z up/down
        auto jog_z_btn_holder = holder->add<Widget>();
        jog_z_btn_holder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 0, 6));

        jog_btn = jog_z_btn_holder->add<Button>("", FA_CHEVRON_UP);
        jog_btn->set_callback([&]() {
            // FIXME: get rid of std::stof
            cnc.request_jog_fixed(grbl::jog_direction::z_up,
                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
        });

        jog_btn = jog_z_btn_holder->add<Button>("", FA_HOME);
        jog_btn->set_callback([&]() {
            cnc.go_to_zero(false, false, true);
        });

        jog_btn = jog_z_btn_holder->add<Button>("", FA_CHEVRON_DOWN);
        jog_btn->set_callback([&]() {
            // FIXME: get rid of std::stof
            cnc.request_jog_fixed(grbl::jog_direction::z_down,
                                  std::stof(jog_distances[cbo_jog_distance->selected_index()]),
                                  std::stof(jog_feed_rates[cbo_jog_feed_rates->selected_index()]));
        });
    }

    void add_dro_markup() {
        dro_ss << std::setprecision(3) << std::fixed;

        // DRO
        info_layer->add<Label>("DRO", "sans-bold", 20);
        auto dro_holder = info_layer->add<Widget>();
        dro_holder->set_layout(new GridLayout(Orientation::Horizontal, 1, Alignment::Middle, 0, 6));

        auto x_holder = dro_holder->add<Widget>();
        x_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 6));
        auto lbl = x_holder->add<Label>("X", "sans-bold", 20);
        lbl->set_fixed_width(30);
        mpos_x_text = x_holder->add<TextBox>(std::to_string(cnc.get_status().machine_pos[0]));
        mpos_x_text->set_fixed_width(200);
        auto zero_btn = x_holder->add<ToolButton>(FA_BAN);
        zero_btn->set_flags(Button::NormalButton);
        zero_btn->set_tooltip("Zero axis");
        zero_btn->set_callback([&]() {
            cnc.zero_offset_axis(cbo_work_offset->selected_index(), 0);
        });

        auto y_holder = dro_holder->add<Widget>();
        y_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 6));
        lbl = y_holder->add<Label>("Y", "sans-bold", 20);
        lbl->set_fixed_width(30);
        mpos_y_text = y_holder->add<TextBox>(std::to_string(cnc.get_status().machine_pos[1]));
        mpos_y_text->set_fixed_width(200);
        zero_btn = y_holder->add<ToolButton>(FA_BAN);
        zero_btn->set_flags(Button::NormalButton);
        zero_btn->set_tooltip("Zero axis");
        zero_btn->set_callback([&]() {
            cnc.zero_offset_axis(cbo_work_offset->selected_index(), 1);
        });

        auto z_holder = dro_holder->add<Widget>();
        z_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 6));
        lbl = z_holder->add<Label>("Z", "sans-bold", 20);
        lbl->set_fixed_width(30);
        mpos_z_text = z_holder->add<TextBox>(std::to_string(cnc.get_status().machine_pos[0]));
        mpos_z_text->set_fixed_width(200);
        zero_btn = z_holder->add<ToolButton>(FA_BAN);
        zero_btn->set_flags(Button::NormalButton);
        zero_btn->set_tooltip("Zero axis");
        zero_btn->set_callback([&]() {
            cnc.zero_offset_axis(cbo_work_offset->selected_index(), 2);
        });

        auto axis_btn_holder = dro_holder->add<Widget>();
        axis_btn_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 0, 6));

        auto zero_all_btn = axis_btn_holder->add<Button>("Zero all");
        zero_all_btn->set_callback([&]() {
            cnc.zero_offset(cbo_work_offset->selected_index());
        });

        auto goto_zero_btn = axis_btn_holder->add<Button>("Goto 0");
        goto_zero_btn->set_callback([&]() {
            cnc.go_to_zero(true, true, true);
        });
    }

    TextBox *extents_min_x, *extents_max_x;
    TextBox *extents_min_y, *extents_max_y;
    TextBox *extents_min_z, *extents_max_z;

    void add_program_extents() {
        dro_ss << std::setprecision(3) << std::fixed;

        // Program extents
        info_layer->add<Label>("Program extents", "sans-bold", 20);
        auto extents_holder = info_layer->add<Widget>();
        extents_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 4, 4));

        extents_holder->add<Label>("X", "sans-bold", 20);
        extents_min_x = extents_holder->add<TextBox>("");
        extents_max_x = extents_holder->add<TextBox>("");

        extents_holder->add<Label>("Y", "sans-bold", 20);
        extents_min_y = extents_holder->add<TextBox>("");
        extents_max_y = extents_holder->add<TextBox>("");

        extents_holder->add<Label>("Z", "sans-bold", 20);
        extents_min_z = extents_holder->add<TextBox>("");
        extents_max_z = extents_holder->add<TextBox>("");
    }

    TextBox *txt_heightmap_from_x, *txt_heightmap_from_y;
    TextBox *txt_heightmap_to_x, *txt_heightmap_to_y;
    TextBox *txt_grid_res;
    TextBox *txt_clearance_height, *txt_start_probing_at, *txt_max_negative_z, *txt_final_z_height;

    void add_heightmap_markup() {
        heightmap_layer->add<Label>("Grid definition", "sans-bold", 20);
        auto heightmap_params_holder = heightmap_layer->add<Widget>();
        heightmap_params_holder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 4, 4));

        // grid size
        auto grid_size_holder = heightmap_params_holder->add<Widget>();
        grid_size_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Middle, 4, 4));

        grid_size_holder->add<Label>("", "sans-bold", 20);
        grid_size_holder->add<Label>("X", "sans-bold", 20);
        grid_size_holder->add<Label>("Y", "sans-bold", 20);

        grid_size_holder->add<Label>("From", "sans-bold", 20);
        txt_heightmap_from_x = grid_size_holder->add<TextBox>("");
        txt_heightmap_from_x->set_editable(true);
        txt_heightmap_from_x->set_fixed_width(150);
        txt_heightmap_from_y = grid_size_holder->add<TextBox>("");
        txt_heightmap_from_y->set_editable(true);
        txt_heightmap_from_y->set_fixed_width(150);

        grid_size_holder->add<Label>("To", "sans-bold", 20);
        txt_heightmap_to_x = grid_size_holder->add<TextBox>("");
        txt_heightmap_to_x->set_editable(true);
        txt_heightmap_to_x->set_fixed_width(150);
        txt_heightmap_to_y = grid_size_holder->add<TextBox>("");
        txt_heightmap_to_y->set_editable(true);
        txt_heightmap_to_y->set_fixed_width(150);

        grid_size_holder->add<Label>("");
        auto btn = grid_size_holder->add<Button>("Fetch from model");
        btn->set_callback([&]() {
            fill_heightmap_from_model();
        });
        grid_size_holder->add<Label>("");

        txt_heightmap_from_x->set_callback([&](const std::string& new_value) {
            update_grid();
            return true;
        });
        txt_heightmap_from_y->set_callback([&](const std::string& new_value) {
            update_grid();
            return true;
        });

        txt_heightmap_to_x->set_callback([&](const std::string& new_value) {
            update_grid();
            return true;
        });
        txt_heightmap_to_y->set_callback([&](const std::string& new_value) {
            update_grid();
            return true;
        });


        // grid resolution
        auto grid_res_holder = heightmap_params_holder->add<Widget>();
        grid_res_holder->set_layout(new BoxLayout(nanogui::Orientation::Horizontal, Alignment::Middle, 4, 4));
        grid_res_holder->add<Label>("Grid resolution", "sans-bold", 20);
        txt_grid_res = grid_res_holder->add<TextBox>("5");
        txt_grid_res->set_editable(true);
        txt_grid_res->set_fixed_width(150);
        txt_grid_res->set_callback([&](const std::string& new_value) {
            update_grid();
            return true;
        });
        grid_res_holder->add<Label>("mm", "sans-bold", 20);

        // business params
        auto heightmap_business_holder = heightmap_params_holder->add<Widget>();
        heightmap_business_holder->set_layout(new GridLayout(nanogui::Orientation::Horizontal, 3, Alignment::Fill, 4, 4));

        heightmap_business_holder->add<Label>("Clearance height Z", "sans-bold", 20);
        txt_clearance_height = heightmap_business_holder->add<TextBox>("1.5");
        txt_clearance_height->set_editable(true);
        heightmap_business_holder->add<Label>("", "sans-bold");

        heightmap_business_holder->add<Label>("Start probing at Z", "sans-bold", 20);
        txt_start_probing_at = heightmap_business_holder->add<TextBox>("0.5");
        txt_start_probing_at->set_editable(true);
        heightmap_business_holder->add<Label>("", "sans-bold");

        heightmap_business_holder->add<Label>("Max negative Z", "sans-bold", 20);
        txt_max_negative_z = heightmap_business_holder->add<TextBox>("-1.0");
        txt_max_negative_z->set_editable(true);
        heightmap_business_holder->add<Label>("(when to fail probing)", "sans-bold");

        heightmap_business_holder->add<Label>("Final Z safety height", "sans-bold", 20);
        txt_final_z_height = heightmap_business_holder->add<TextBox>("15");
        txt_final_z_height->set_editable(true);
        heightmap_business_holder->add<Label>("", "sans-bold");

        heightmap_business_holder->add<Label>("", "sans-bold");
        auto btn_start_probing = heightmap_business_holder->add<Button>("Start Probing");
        btn_start_probing->set_callback([&]() {
            cnc.probe_heightmap(heightmap_grid);
        });
        heightmap_business_holder->add<Label>("", "sans-bold");
    }

    void fill_heightmap_from_model() {
        auto min = renderer.get_extents_min();
        auto max = renderer.get_extents_max();
        txt_heightmap_from_x->set_value(std::to_string(min.x));
        txt_heightmap_from_y->set_value(std::to_string(min.y));
        txt_heightmap_to_x->set_value(std::to_string(max.x));
        txt_heightmap_to_y->set_value(std::to_string(max.y));
    }

    void update_grid() {
        auto from_x = std::stof(txt_heightmap_from_x->value());
        auto from_y = std::stof(txt_heightmap_from_y->value());
        auto to_x = std::stof(txt_heightmap_to_x->value());
        auto to_y = std::stof(txt_heightmap_to_y->value());
        auto res = std::stof(txt_grid_res->value());

        if (from_x != heightmap_grid.from_x ||
            from_y != heightmap_grid.from_y ||
            to_x != heightmap_grid.to_x ||
            to_y != heightmap_grid.to_y ||
            res != heightmap_grid.resolution) {
            heightmap_grid = std::move(grbl::heightmap::from_params(from_x, from_y, to_x, to_y, res));
            renderer.update_grid(heightmap_grid);
        }

    }

    void add_pins_markup() {
        // Pins
        info_layer->add<Label>("Pins", "sans-bold", 20);
        auto pins_holder = info_layer->add<Widget>();
        pins_holder->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Fill, 4, 4));

        btn_pin_door = pins_holder->add<ToolButton>(0, "D");
//        btn_pin_door->set_enabled(false);
        btn_pin_door->set_flags(Button::Flags::NormalButton);

        btn_pin_hold = pins_holder->add<ToolButton>(0, "H");
//        btn_pin_hold->set_enabled(false);
        btn_pin_hold->set_flags(Button::Flags::NormalButton);

        btn_pin_reset = pins_holder->add<ToolButton>(0, "R");
//        btn_pin_reset->set_enabled(false);
        btn_pin_reset->set_flags(Button::Flags::NormalButton);

        btn_pin_cycle_start = pins_holder->add<ToolButton>(0, "S");
//        btn_pin_cycle_start->set_enabled(false);
        btn_pin_cycle_start->set_flags(Button::Flags::NormalButton);

        auto a = pins_holder->add<Label>("");
        a->set_fixed_width(30);

        btn_pin_limit_x = pins_holder->add<ToolButton>(0, "X");
//        btn_pin_limit_x->set_enabled(false);
        btn_pin_limit_x->set_flags(Button::Flags::NormalButton);

        btn_pin_limit_y = pins_holder->add<ToolButton>(0, "Y");
//        btn_pin_limit_y->set_enabled(false);
        btn_pin_limit_y->set_flags(Button::Flags::NormalButton);

        btn_pin_limit_z = pins_holder->add<ToolButton>(0, "Z");
//        btn_pin_limit_z->set_enabled(false);
        btn_pin_limit_z->set_flags(Button::Flags::NormalButton);

        btn_pin_probe = pins_holder->add<ToolButton>(0, "P");
//        btn_pin_probe->set_enabled(false);
        btn_pin_probe->set_flags(Button::Flags::NormalButton);
    }

    void refresh_offset() const {
        auto offset_name = "G" + std::to_string(cbo_work_offset->selected_index() + 54);
        cnc.set_work_offset(offset_name);
    }

    void fill_in_parameters() {
        parameters_vscroll = new VScrollPanel(tab_widget);
        tab_widget->append_tab("Parameters", parameters_vscroll);

        parameters_layer = new Widget(parameters_vscroll);
        parameters_layer->set_layout(new GridLayout(Orientation::Horizontal, 1, Alignment::Middle));

        auto& parameters = cnc.get_parameters();

        for (auto& entry: parameters) {
            auto w = parameters_layer->add<Widget>();
            w->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 2, 2));
            auto x = w->add<Label>(entry.first, "sans-bold", 20);
            x->set_fixed_width(50);

            auto y = w->add<TextBox>(entry.second);
            y->set_editable(true);
            y->set_fixed_width(200);

            auto z = w->add<Button>("", FA_SAVE);
//            z->set_flags(Button::Flags::NormalButton); // no toggle, please
            z->set_tooltip("save");
        }
    }


    void fill_in_settings() {
        settings_vscroll = new VScrollPanel(tab_widget);
        tab_widget->append_tab("Settings", settings_vscroll);

        settings_layer = new Widget(settings_vscroll);
        settings_layer->set_layout(new GridLayout(Orientation::Horizontal, 1, Alignment::Middle));

        auto& settings = cnc.get_settings();

        for (auto& s: settings) {
            auto w = settings_layer->add<Widget>();
            w->set_layout(new BoxLayout(Orientation::Horizontal, Alignment::Middle, 2, 2));
            auto x = w->add<Label>(s.first, "sans-bold", 20);
            x->set_fixed_width(40);

            auto desc = grbl::setting_description(std::stoi(s.first.substr(1)));

            auto y = w->add<TextBox>(s.second);
            y->set_editable(true);
            y->set_tooltip(desc);
            y->set_fixed_width(150);

            auto z = w->add<ToolButton>(FA_SAVE);
            z->set_flags(Button::Flags::NormalButton); // no toggle, please
            z->set_tooltip("save");

            auto t = w->add<TextArea>();
            t->set_fixed_width(200);
            t->set_fixed_height(20);
            t->clear();
            t->append(desc.substr(0, desc.find_first_of('\n')));
        }
    }

    void init_program_geometry() {
        renderer.update(pgm, cnc);

        auto max_pos = renderer.get_extents_max();
        auto min_pos = renderer.get_extents_min();

        cam_target = (max_pos - min_pos) / 2.0f;
        cam_zoom = (max_pos.x - min_pos.x);
        cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0);
    }

    void set_program_extents() {
        auto min = renderer.get_extents_min();
        auto max = renderer.get_extents_max();

        extents_min_x->set_value(std::to_string(min.x));
        extents_max_x->set_value(std::to_string(max.x));

        extents_min_y->set_value(std::to_string(min.y));
        extents_max_y->set_value(std::to_string(max.y));

        extents_min_z->set_value(std::to_string(min.z));
        extents_max_z->set_value(std::to_string(max.z));
    }

    bool resize_event(const Vector2i& size) override {
//        window->set_fixed_height(this->height() / 2);
        tab_widget->set_fixed_height((this->height() - 30));
        perform_layout();

        return Screen::resize_event(size);
    }

    void on_connected() override {
    }

    void on_disconnected() override {
    }

    void on_settings_reloaded() override {
    }

    void on_parameters_reloaded() override {
        refresh_offset();
        update_dro();
    }


    grbl::realtime_status_report last_report;

    volatile bool machine_initialized = false;

    void on_init_completed() override {
        machine_initialized = true;
        // need to do UI related things in UI thread
    }

    void on_realtime_status_report(grbl::realtime_status_report report) override {
//        if (report == last_report) return;

//        if (last_report.status != report.status) {
        txt_status->set_value(grbl::status_to_string(cnc.get_status().status));

        if (cnc.get_status().status == grbl::machine_status::alarm) {
            txt_status->set_tooltip(grbl::alarm_to_string(last_alarm));
        } else {
            txt_status->set_tooltip(cnc.get_status().sub_status);
        }
//        }


        // FIXME: ugly way of retrieving the bg color
        btn_pin_door->set_background_color(report.signals.bit.door ? color_red : btn_load_program->background_color());
        btn_pin_hold->set_background_color(report.signals.bit.hold ? color_red : btn_load_program->background_color());
        btn_pin_reset->set_background_color(report.signals.bit.soft_reset ? color_red : btn_load_program->background_color());
        btn_pin_cycle_start->set_background_color(report.signals.bit.cycle_start ? color_red : btn_load_program->background_color());
        btn_pin_limit_x->set_background_color(report.signals.bit.x_limit ? color_red : btn_load_program->background_color());
        btn_pin_limit_y->set_background_color(report.signals.bit.y_limit ? color_red : btn_load_program->background_color());
        btn_pin_limit_z->set_background_color(report.signals.bit.z_limit ? color_red : btn_load_program->background_color());
        btn_pin_probe->set_background_color(report.signals.bit.probe ? color_red : btn_load_program->background_color());

        update_dro();
        last_report = report;
    }

    void update_dro() {
        auto work_pos = cnc.get_work_pos();

        dro_ss.str("");
        dro_ss << work_pos[0];
        mpos_x_text->set_value(dro_ss.str());

        dro_ss.str("");
        dro_ss << work_pos[1];
        mpos_y_text->set_value(dro_ss.str());

        dro_ss.str("");
        dro_ss << work_pos[2];
        mpos_z_text->set_value(dro_ss.str());
    }

    void on_banner(std::string line) override {
    }

    void on_message(std::string message) override {
        txtMessage->set_value(message);
        set_caption(message);
    }

    void on_alarm(int alarm) override {
        last_alarm = alarm;
    }

    void on_probe_result(bool probe_touched, float *probe_coords) override {
        std::stringstream ss;
        ss << "Probing ended. Result: " << std::boolalpha << probe_touched << " at coords: ";
        for (auto i = 0; i < 3; i++) {
            ss << probe_coords[i] << ", ";
        }
        new MessageDialog(this, MessageDialog::Type::Warning, "Probe result", ss.str());
    }

    void on_heightmap_probe_acquired(grbl::heightmap *grid) override {
        std::cout << "Updating grid" << std::endl;
        renderer.update_grid(*grid);
    }

    void on_check_completed(bool success, size_t failed_index, size_t error) override {
        btn_check_program->set_background_color(success ? colGreen : color_red);
        if (success) {
            btn_run_program->set_enabled(true);
            new MessageDialog(this, MessageDialog::Type::Information, "Check result", "Program checked successfully");
        } else {
            btn_run_program->set_enabled(false);
            std::stringstream ss;
            auto i = pgm.instruction_at(failed_index);
            ss << "Program check failed at line " << i.line << " (" << i.command << "). Error: " << error << ", "
               << grbl::error_to_string(error);
            new MessageDialog(this, MessageDialog::Type::Warning, "Check result", ss.str());
        }
    }

    void on_run_completed(bool success, size_t failed_index, size_t error) override {
        btn_run_program->set_background_color(success ? colGreen : color_red);
        if (success) {
            new MessageDialog(this, MessageDialog::Type::Information, "Run result", "Program executed successfully");
        } else {
            std::stringstream ss;
            auto i = pgm.instruction_at(failed_index);
            ss << "Program execution failed at line " << i.line << " (" << i.command << "). Error: " << error << ", "
               << grbl::error_to_string(error);
            new MessageDialog(this, MessageDialog::Type::Warning, "Run result", ss.str());
        }
    }


    bool keyboard_event(int key, int scancode, int action, int modifiers) override {
        if (Screen::keyboard_event(key, scancode, action, modifiers))
            return true;

//        if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) {
//            set_visible(false);
//            return true;
//        }

        if (key == GLFW_KEY_SPACE && action == GLFW_PRESS && !window->mouse_focused()) {
            // reset trackball rotation
            cam_src_rotation = glm::quat(1, 0, 0, 0);
        }

        auto new_jog = jog;

        if (key == GLFW_KEY_LEFT_SHIFT) {
            if (action == GLFW_PRESS) {
                new_jog.speed_fast_pressed = true;
            } else if (action == GLFW_RELEASE) {
                new_jog.speed_fast_pressed = false;
            }
        }

        if (key == GLFW_KEY_LEFT_CONTROL) {
            if (action == GLFW_PRESS) {
                new_jog.speed_slow_pressed = true;
            } else if (action == GLFW_RELEASE) {
                new_jog.speed_slow_pressed = false;
            }
        }

        if (key == GLFW_KEY_UP) {
            if (action == GLFW_PRESS) {
                new_jog.up_pressed = true;
            } else if (action == GLFW_RELEASE) {
                new_jog.up_pressed = false;
            }
        }

        if (key == GLFW_KEY_DOWN) {
            if (action == GLFW_PRESS) {
                new_jog.down_pressed = true;
            } else if (action == GLFW_RELEASE) {
                new_jog.down_pressed = false;
            }
        }

        if (key == GLFW_KEY_LEFT) {
            if (action == GLFW_PRESS) {
                new_jog.left_pressed = true;
            } else if (action == GLFW_RELEASE) {
                new_jog.left_pressed = false;
            }
        }

        if (key == GLFW_KEY_RIGHT) {
            if (action == GLFW_PRESS) {
                new_jog.right_pressed = true;
            } else if (action == GLFW_RELEASE) {
                new_jog.right_pressed = false;
            }
        }

        if (key == GLFW_KEY_PAGE_UP) {
            if (action == GLFW_PRESS) {
                new_jog.z_up_pressed = true;
            } else if (action == GLFW_RELEASE) {
                new_jog.z_up_pressed = false;
            }
        }

        if (key == GLFW_KEY_PAGE_DOWN) {
            if (action == GLFW_PRESS) {
                new_jog.z_down_pressed = true;
            } else if (action == GLFW_RELEASE) {
                new_jog.z_down_pressed = false;
            }
        }

        if (btn_keyboard_jog->pushed() && jog != new_jog) {
            cnc.request_jog(new_jog);
            jog = new_jog;
        }

        return false;
    }

    bool mouse_motion_event(const Vector2i& p, const Vector2i& rel, int button, int modifiers) override {
        auto result = Widget::mouse_motion_event(p, rel, button, modifiers);
        if (window->mouse_focused()) {
            return result;
        } else {
//            std::cout << "Mouse motion p:" << p << ", rel:" << rel << ", button:" << button << ", .modifiers:" << modifiers << std::endl;
            bool is_rotating = button == 1;
            bool is_panning = button == 2;
            if (is_rotating) {
                if (abs(rel.x()) > abs(rel.y())) {
                    cam_src_rotation *= glm::angleAxis(rel.x() / 100.0f, glm::vec3(0, 1, 0));
                } else {
                    cam_src_rotation *= glm::angleAxis(rel.y() / 100.0f, glm::vec3(1, 0, 0));
                }

            } else if (is_panning) {
                cam_pan.x += (float) rel.x() / 10.0f;
                cam_pan.y -= (float) rel.y() / 10.0f;
            }
            return true;
        }
    }

    bool scroll_event(const Vector2i& p, const Vector2f& rel) override {
        if (window->mouse_focused()) {
            return Widget::scroll_event(p, rel);
        } else {
//            std::cout << "Scroll event: p:" << p << ", rel:" << rel << std::endl;
            cam_zoom -= rel.y() * cam_zoom / 10.0f;
            cam_zoom = std::max(cam_zoom, 0.1f);

            return true;
        }
    }

    void draw(NVGcontext *ctx) override {
        // not pretty but need to do this in UI thread
        if (machine_initialized) {
            fill_in_settings();
            fill_in_parameters();

            perform_layout();

            refresh_offset();
            update_dro();
            machine_initialized = false;
        }

        Widget::draw(ctx);
    }

    void draw_contents() override {
        auto fb_size = framebuffer_size();

        // start rendering
        m_render_pass->resize(framebuffer_size());
        m_render_pass->begin();

        if (pgm.is_loaded) {
            renderer.update(pgm, cnc);
            // compute mvp
            glm::mat4 projection = glm::perspective(45.0f * glm::pi<float>() / 180.0f,
                                                    (float) fb_size.x() / (float) fb_size.y(),
                                                    0.1f,
                                                    10000.f);

            glm::mat4 view = glm::translate(glm::mat4(1.0f), glm::vec3(0, 0, -cam_zoom)) *
                             glm::toMat4(cam_src_rotation) *
                             glm::translate(glm::mat4(1.0f), cam_pan - cam_target);

            glm::mat4 model = glm::mat4(1.0f);
            glm::mat3 normal_mat = glm::inverseTranspose(glm::mat3(view * model));

            renderer.render(model, view, projection, normal_mat, glm::vec2(fb_size.x(), fb_size.y()));
        }

        m_render_pass->end();
    }

private:
    ProgressBar *m_progress;
    ref<RenderPass> m_render_pass;

    using ImageHolder = std::unique_ptr<uint8_t[], void (*)(void *)>;
    std::vector<std::pair<ref<Texture>, ImageHolder>> m_images;
    int m_current_image;
};


int main(int argc, char **argv) {

    testing::InitGoogleTest(&argc, argv);
    auto result = RUN_ALL_TESTS();
    if (result) {
        exit(result);
    }

    try {
        nanogui::init();

        // scoped variables
        {
            ref<SenderApp> app = new SenderApp();
            app->dec_ref();
            app->draw_all();
            app->set_visible(true);

            cnc.set_listener(app);
            cnc.connect();

            nanogui::mainloop(1 / 60.f * 1000);
        }

        nanogui::shutdown();
    } catch (const std::exception& e) {
        std::string error_msg = std::string("Caught a fatal error: ") + std::string(e.what());
        std::cerr << error_msg << std::endl;
        return -1;
    } catch (...) {
        std::cerr << "Caught an unknown error!" << std::endl;
        return -2;
    }

    return 0;
}