#include <Arduino.h>

#include "config.h"
#include "config_pins.h"
#include "data.h"
#include "common.h"
#include "lcd.h"
#include "steppers.h"
#include "statemachine.h"
#include "states.h"

// --------------------------------------

StateText sm_ask_homing = StateText();
StateText sm_do_homing = StateText(&sm_ask_homing);
StateMenu sm_menu = StateMenu(&sm_do_homing, false);

StateMenu sm_auto = StateMenu(&sm_menu);
StateDynamicMenu sm_presets = StateDynamicMenu(&sm_auto);
StateText sm_new_preset = StateText(&sm_auto);
StateDynamicMenu sm_mod_preset = StateDynamicMenu(&sm_auto);
StateDynamicMenu sm_del_preset = StateDynamicMenu(&sm_auto);

StateMenu sm_move = StateMenu(&sm_menu);
StateText sm_move_x = StateText(&sm_move);
StateText sm_move_y = StateText(&sm_move);
StateText sm_move_z = StateText(&sm_move);
StateText sm_move_e = StateText(&sm_move);

StateMenu sm_config = StateMenu(&sm_menu);
StateText sm_conf_load = StateText(&sm_config);
StateText sm_conf_save = StateText(&sm_config);
StateValue<float> sm_conf_speed_xy = StateValue<float>(&sm_config, data_options()->speed_x, 1.0, XY_MAX_SPEED);
StateValue<float> sm_conf_speed_z = StateValue<float>(&sm_config, data_options()->speed_z, 1.0, Z_MAX_SPEED);
StateValue<float> sm_conf_speed_e = StateValue<float>(&sm_config, data_options()->speed_e, 1.0, E_MAX_SPEED);
StateValue<float> sm_conf_accel_xy = StateValue<float>(&sm_config, data_options()->accel_x, 1.0, XY_MAX_ACCEL);
StateValue<float> sm_conf_accel_z = StateValue<float>(&sm_config, data_options()->accel_z, 1.0, Z_MAX_ACCEL);
StateValue<float> sm_conf_accel_e = StateValue<float>(&sm_config, data_options()->accel_e, 1.0, E_MAX_ACCEL);

// --------------------------------------

State *current_state = NULL;
String strbuf;

void states_init(void) {
    // ----------------------------------

    sm_ask_homing.setTitle("Ask for homing");
    sm_ask_homing.setHeading("Homing Required!");
    sm_ask_homing.setText("Click to home all four axes.");

    // ----------------------------------

    sm_do_homing.setTitle("Home all axes");
    sm_do_homing.setHeading("Homing...");

    sm_do_homing.onEnter([]() {
        //steppers_start_homing();
    });

    sm_do_homing.whenIn([](StateMachineInput smi) {
        //if (smi.motors_done) {
        //    if (steppers_homed()) {
                async_beep(HOMING_BEEP_TIME, HOMING_BEEP_FREQ);
                states_go_to(&sm_menu);
        //    }
        //}

        // TODO update text with current axis
    });

    // ----------------------------------

    sm_menu.setTitle("Main Menu");
    sm_menu.addChild(&sm_do_homing, 1);

    // ----------------------------------

    sm_move.setTitle("Move Axis");

    sm_move_x.setTitle("X Axis");
    sm_move_y.setTitle("Y Axis");
    sm_move_z.setTitle("Z Axis");
    sm_move_e.setTitle("E Axis");

    // ----------------------------------

    sm_auto.setTitle("Filling Menu");

    sm_presets.setTitle("Use Preset");
    sm_presets.dataCount([]() {
        return (int)data_preset_count();
    });
    sm_presets.dataGet([](int i) {
        // TODO can not build a name string here
        // dynamically. need to have a name stored
        // somewhere, in data/eeprom, for each preset
        // that we can pass here
        return "foo";
    });

    sm_new_preset.setTitle("Add new Preset");
    sm_new_preset.setHeading("Add new Preset");
    sm_new_preset.onEnter([]() {
        struct data_config_preset preset;
        if (!data_preset_add(preset)) {
            strbuf = String(data_eeprom_error()) + " Error adding preset!";
            sm_new_preset.setText(strbuf.c_str());
        } else {
            strbuf = "Preset " + String(data_preset_count()) + " has been added!";
            sm_new_preset.setText(strbuf.c_str());
        }
    });

    sm_mod_preset.setTitle("Modify Preset");
    sm_mod_preset.dataCount([]() {
        return (int)data_preset_count();
    });
    sm_mod_preset.dataGet([](int i) {
        // TODO can not build a name string here
        // dynamically. need to have a name stored
        // somewhere, in data/eeprom, for each preset
        // that we can pass here
        return "foo";
    });

    sm_del_preset.setTitle("Delete Preset");
    sm_del_preset.dataCount([]() {
        return (int)data_preset_count();
    });
    sm_del_preset.dataGet([](int i) {
        // TODO can not build a name string here
        // dynamically. need to have a name stored
        // somewhere, in data/eeprom, for each preset
        // that we can pass here
        return "foo";
    });

    // ----------------------------------

    sm_config.setTitle("Configuration");

    sm_conf_load.setTitle("Load from EEPROM");
    sm_conf_load.setHeading("Load from EEPROM");
    sm_conf_load.onEnter([]() {
        if (!data_eeprom_read()) {
            strbuf = String(data_eeprom_error()) + " Error reading configuration!";
            sm_conf_load.setText(strbuf.c_str());
        } else {
            sm_conf_load.setText("Configuration read successful!");

            steppers_set_speed_x(data_options()->speed_x);
            steppers_set_accel_x(data_options()->accel_x);
            steppers_set_speed_y(data_options()->speed_y);
            steppers_set_accel_y(data_options()->accel_y);
            steppers_set_speed_z(data_options()->speed_z);
            steppers_set_accel_z(data_options()->accel_z);
            steppers_set_speed_e(data_options()->speed_e);
            steppers_set_accel_e(data_options()->accel_e);
        }
    });

    sm_conf_save.setTitle("Store to EEPROM");
    sm_conf_save.setHeading("Store to EEPROM");
    sm_conf_save.setText("Configuration written to EEPROM!");
    sm_conf_save.onEnter([]() {
        data_eeprom_write();
    });

    sm_conf_speed_xy.setTitle("XY Speed");
    sm_conf_speed_xy.onUpdate([](float v) {
        steppers_set_speed_x(v);
        steppers_set_speed_y(v);
    });

    sm_conf_speed_z.setTitle("Z Speed");
    sm_conf_speed_z.onUpdate([](float v) {
        steppers_set_speed_z(v);
    });

    sm_conf_speed_e.setTitle("E Speed");
    sm_conf_speed_e.onUpdate([](float v) {
        steppers_set_speed_e(v);
    });

    sm_conf_accel_xy.setTitle("XY Acceleration");
    sm_conf_accel_xy.onUpdate([](float v) {
        steppers_set_accel_x(v);
        steppers_set_accel_y(v);
    });

    sm_conf_accel_z.setTitle("Z Acceleration");
    sm_conf_accel_z.onUpdate([](float v) {
        steppers_set_accel_z(v);
    });

    sm_conf_accel_e.setTitle("E Acceleration");
    sm_conf_accel_e.onUpdate([](float v) {
        steppers_set_accel_e(v);
    });

    // ----------------------------------

    states_go_to(&sm_ask_homing);
}

void states_run(StateMachineInput smi) {
    if (smi.click) {
        async_beep(ENCODER_CLICK_BEEP_TIME, ENCODER_CLICK_BEEP_FREQ);
    }

    if (smi.kill) {
        steppers_kill();
        Serial.println("Kill button pressed!");
        blocking_beep(KILL_BEEP_TIME, KILL_BEEP_FREQ, KILL_BEEP_REPEAT - 1);
        states_go_to(&sm_ask_homing);
        return;
    }

    if (current_state != NULL) {
        current_state->inState(smi);
    }
}

State *states_get(void) {
    return current_state;
}

void states_go_to(State *state) {
    current_state = state;

    if (current_state != NULL) {
        current_state->enterState();
    }
}