#pragma once

#include <map>
#include <unordered_map>
#include <memory>
#include <mutex>
#include "grbl_communication.h"
#include "grbl.h"
#include "heightmap.h"

namespace grbl {

enum class machine_status {
    idle,
    run,
    hold,
    jog,
    alarm,
    door,
    check,
    home,
    sleep,
    tool,
    unknown
};

machine_status status_from_string(const std::string& status);
std::string status_to_string(const machine_status& status);
std::string alarm_to_string(int alarm);
std::string error_to_string(size_t error);
std::string setting_description(int number);

struct realtime_status_report {
    machine_status status;
    std::string sub_status;
    float work_pos[3] = {0};
    float machine_pos[3] = {0};
    size_t buffers_free = 0;
    size_t rx_chars_free = 0;
    size_t line_number = 0;
    float feed_rate = 0;
    float programmed_rpm = 0;
    float actual_rpm = 0;
    union {
        struct {
            bool x_limit: 1;
            bool y_limit: 1;
            bool z_limit: 1;
            bool probe: 1;
            bool door: 1;
            bool hold: 1;
            bool soft_reset: 1;
            bool cycle_start: 1;
        } bit;
        uint8_t value = 0;
    } signals;
    float axis_offsets[3] = {0};
    std::string coordinate_system;
    std::string overrides;
    std::string accessory_status;
    bool mpg = false;
    bool homing_complete = false;
    std::string scaled_axis;
    bool tool_length_reference_offset_set = false;
    std::string firmware;
};

inline bool operator==(const realtime_status_report& a, const realtime_status_report& b) {
    return a.status == b.status &&
           a.sub_status == b.sub_status &&
           a.machine_pos[0] == b.machine_pos[0] &&
           a.machine_pos[1] == b.machine_pos[1] &&
           a.machine_pos[2] == b.machine_pos[2] &&
           a.work_pos[0] == b.work_pos[0] &&
           a.work_pos[1] == b.work_pos[1] &&
           a.work_pos[2] == b.work_pos[2];
}

realtime_status_report parse_status_report(std::string line, grbl::realtime_status_report& result);

enum class grbl_machine_state {
    disconnected,
    init,
    run_program,
    check_program,
    idle,
    heightmap_probing,
};

struct jog_state {
    bool left_pressed = false;
    bool right_pressed = false;
    bool up_pressed = false;
    bool down_pressed = false;
    bool z_up_pressed = false;
    bool z_down_pressed = false;
    bool speed_fast_pressed = false;
    bool speed_slow_pressed = false;

    [[nodiscard]] bool no_jogging() const;
};

static bool operator==(const jog_state& a, const jog_state& b) {
    if (a.left_pressed != b.left_pressed ||
        a.right_pressed != b.right_pressed ||
        a.up_pressed != b.up_pressed ||
        a.down_pressed != b.down_pressed ||
        a.z_up_pressed != b.z_up_pressed ||
        a.z_down_pressed != b.z_down_pressed ||
        a.speed_fast_pressed != b.speed_fast_pressed ||
        a.speed_slow_pressed != b.speed_slow_pressed) {
        return false;
    }
    return true;
}

static bool operator!=(const jog_state& a, const jog_state& b) {
    return !(a == b);
}

enum class machine_event_type {
    connected,
    disconnected,
    report_received,
    banner,
    message,
    alarm,
    init_completed,
    run_completed,
    check_completed,
    settings_reloaded,
    parameters_reloaded,
    probe_result,
    heightmap_probe_acquired,
};

struct machine_event {
    machine_event_type type;
    // TODO: ewwww
    virtual void omg() {}
};

struct machine_event_connect : public machine_event {
    machine_event_connect();
};

struct machine_event_disconnect : public machine_event {
    machine_event_disconnect();
};

struct machine_event_report_received : public machine_event {
    explicit machine_event_report_received(const realtime_status_report& report);
    realtime_status_report report;
};

struct machine_event_banner : public machine_event {
    explicit machine_event_banner(const std::string& banner);
    std::string banner;
};

struct machine_event_message : public machine_event {
    explicit machine_event_message(const std::string& message);
    std::string message;
};

struct machine_event_alarm : public machine_event {
    explicit machine_event_alarm(int code);
    int alarm;
};

struct machine_event_init_completed : public machine_event {
    machine_event_init_completed();
};

struct machine_event_run_completed : public machine_event {
    machine_event_run_completed(bool success, size_t failed_index, size_t error);
    bool success;
    size_t failed_index;
    size_t error;
};

struct machine_event_check_completed : public machine_event {
    machine_event_check_completed(bool success, size_t failed_idx, size_t error);
    bool success;
    size_t failed_index;
    size_t error;
};

struct machine_event_settings_reloaded : public machine_event {
    machine_event_settings_reloaded();
};

struct machine_event_parameters_reloaded : public machine_event {
    machine_event_parameters_reloaded();
};

struct machine_event_probe_result : public machine_event {
    machine_event_probe_result(bool touched, const float *coords);
    bool probe_touched;
    float probe_coords[3];
};

struct machine_event_heightmap_probe_acquired : public machine_event {
    machine_event_heightmap_probe_acquired(heightmap *g, size_t location);
    grbl::heightmap *grid;
    size_t probed_location;
};


enum class init_stage {
    start,
    set_work_pos,
    fetch_settings,
    fetch_parameters
};

enum class check_stage {
    start,
    enable_check_mode,
    run_program,
    disable_check_mode
};

enum class run_stage {
    start,
    run_program,
};

enum class heightmap_probing_stage {
    start,
    goto_home,
    initial_probe,
    goto_next_location,
    probing, initial_probe_step_back, initial_probe_fine_seek, goto_next_location_move, goto_start_probing_at, done
};

struct machine;

struct machine_state {
    virtual void on_connected(machine *m) = 0;
    virtual void on_disconnected(machine *m) = 0;
    virtual void on_enter(machine *m) = 0;
    virtual void on_exit(machine *m) = 0;
    virtual void on_line_received(std::string line) = 0;
};

struct machine_state_connect : public machine_state {
    void on_connected(machine *m) override;
    void on_disconnected(machine *m) override;
    void on_enter(machine *m) override;
    void on_exit(machine *m) override;
    void on_line_received(std::string line) override;
    machine *cnc;
};

struct machine_state_init : public machine_state {
    void on_connected(machine *m) override;
    void on_disconnected(machine *m) override;
    void on_enter(machine *m) override;
    void on_exit(machine *m) override;
    void on_line_received(std::string line) override;

    init_stage init_state = init_stage::start;
    void move_to_next_init_stage();

    machine *cnc;
};

struct machine_state_idle : public machine_state {
    void on_connected(machine *m) override;
    void on_disconnected(machine *m) override;
    void on_enter(machine *m) override;
    void on_exit(machine *m) override;
    void on_line_received(std::string line) override;
    machine *cnc;
};

struct machine_state_check_program : public machine_state {
    void on_connected(machine *m) override;
    void on_disconnected(machine *m) override;
    void on_enter(machine *m) override;
    void on_exit(machine *m) override;
    void on_line_received(std::string line) override;

    bool continue_program();
    void move_to_next_check_stage();

    check_stage stage = check_stage::start;
    machine *cnc;
    bool check_failed;
    size_t check_error;
};

struct machine_state_run_program : public machine_state {
    void on_connected(machine *m) override;
    void on_disconnected(machine *m) override;
    void on_enter(machine *m) override;
    void on_exit(machine *m) override;
    void on_line_received(std::string line) override;

    bool continue_program();
    void move_to_next_run_stage();

    machine *cnc;
    run_stage stage = run_stage::start;
    bool run_failed;
    size_t run_error;
};

struct machine_state_heightmap_probing : public machine_state {
    void on_connected(machine *m) override;
    void on_disconnected(machine *m) override;
    void on_enter(machine *m) override;
    void on_exit(machine *m) override;
    void on_line_received(std::string line) override;

    bool continue_program();
    void move_to_next_stage();

    machine *cnc;
    heightmap_probing_stage stage = heightmap_probing_stage::start;
    bool failed;
    size_t error;
    size_t probed_locations;
    heightmap *grid;
    float z_zero_in_mpos = 0;
};


struct settings_cmp {
    bool operator()(const std::string& a, const std::string& b) const {
        return std::stoi(a.substr(1)) < std::stoi(b.substr(1));
    }
};

struct parameters_cmp {
    bool operator()(const std::string& a, const std::string& b) const {
        return a < b;
    }
};

enum class jog_direction {
    x_up,
    x_down,
    y_up,
    y_down,
    z_up,
    z_down,
};

struct machine : public transport_callbacks {
    machine();
    ~machine();

    void connect();

    void set_work_offset(std::string work_offset);
    std::array<float, 3> get_work_pos() const;

    void run_program(const grbl::program& pgm, const std::string& work_offset);
    void check_program(const grbl::program& pgm);
    void request_jog(jog_state jog) const;
    void request_jog_fixed(jog_direction dir, float distance, float feed);
    void cancel_jog() const;

    [[nodiscard]] realtime_status_report get_status() const { return last_report; };
    const std::map<std::string, std::string, settings_cmp>& get_settings() const;
    const std::map<std::string, std::string, parameters_cmp>& get_parameters() const;

    void request_unlock();
    void request_home();
    void request_reset();
    void request_cycle_start();
    void request_feed_hold();

    // 0 = G54, 1 = G55, etc
    void zero_offset(int which);

    void go_to_zero(bool x, bool y, bool z);

    // 0 = G54, 1 = G55, etc axis 0=X, 1=Y, 2=Z
    void zero_offset_axis(int offset_index, int axis);

    void start_z_probe(float min_z, float feed_rate);

    void probe_heightmap(grbl::heightmap& grid);

    void push_event(std::shared_ptr<machine_event> event);

    // pops nullptr if queue is empty
    std::shared_ptr<machine_event> pop_event();


protected:

    std::mutex event_mutex;
    std::deque<std::shared_ptr<machine_event>> events;

    void on_connected(transport *transport) override;
    void on_disconnected(transport *transport) override;
    void on_line_received(std::string line, transport *transport) override;

    void switch_to_state(grbl_machine_state new_state);

    friend class machine_state_connect;
    friend class machine_state_init;
    friend class machine_state_idle;
    friend class machine_state_check_program;
    friend class machine_state_run_program;
    friend class machine_state_heightmap_probing;

    std::map<grbl_machine_state, machine_state *> states;
    std::map<std::string, std::string, settings_cmp> settings;
    std::map<std::string, std::string, parameters_cmp> parameters;

    std::string current_work_offset;
    float current_work_offset_values[3] = {0};
public:
    const float *get_current_work_offset_values() const;
protected:

    volatile size_t awaiting_responses = 0;
    realtime_status_report last_report{};
    transport *pipe = nullptr;
    grbl_machine_state state = grbl_machine_state::disconnected;
    program running_program;
    size_t executed_instructions = 0;
    void reset_machine_state();
};


}

