#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"

//#define DISABLE_HOMING_STATE
#define ENABLE_MOVEMENT_TEST_STATE
#define ENABLE_INPUT_TEST_STATE

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

int preset_selection = 0, dispense_off = 0,  dispense_count = 0, dispense_index = 0;
float move_pos_x = 0.0, move_pos_y = 0.0, move_pos_z = 0.0, move_pos_e = 0.0;

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

StateMenu sm_auto = StateMenu(&sm_menu);
StateDynamicMenu sm_presets = StateDynamicMenu(&sm_auto);

StateValue<int> sm_disp_off = StateValue<int>(&sm_presets, dispense_off, 1, 1);
StateValue<int> sm_disp_cnt = StateValue<int>(&sm_disp_off, dispense_count, 1, 1);
State sm_disp_go = State(&sm_disp_cnt);
State sm_disp_run_z_travel = State(&sm_disp_go);
State sm_disp_run_xy_travel = State(&sm_disp_run_z_travel);
State sm_disp_run_z_disp = State(&sm_disp_run_xy_travel);
State sm_disp_run = State(&sm_disp_run_z_disp);
State sm_disp_done = State(&sm_disp_run);

State sm_new_preset = State(&sm_auto);
State sm_wiz_move = State(&sm_new_preset);
StateValues<uint8_t, 2> sm_wiz_count = StateValues<uint8_t, 2>(&sm_wiz_move);
StateValues<float, 2> sm_wiz_first_pos = StateValues<float, 2>(&sm_wiz_count);
StateValues<float, 2> sm_wiz_last_pos = StateValues<float, 2>(&sm_wiz_first_pos);
// TODO move z above container
// TODO move z inside bottom container
// TODO move z inside top container
// TODO move to extruder depth
State sm_new_preset_done = State(&sm_wiz_last_pos);

StateDynamicMenu sm_mod_preset = StateDynamicMenu(&sm_auto);
// TODO modify

StateDynamicMenu sm_del_preset = StateDynamicMenu(&sm_auto);
// TODO delete

StateMenu sm_move = StateMenu(&sm_menu);
StateValue<float> sm_move_x = StateValue<float>(&sm_move, move_pos_x, X_AXIS_MIN, X_AXIS_MAX);
StateValue<float> sm_move_y = StateValue<float>(&sm_move, move_pos_y, Y_AXIS_MIN, Y_AXIS_MAX);
StateValue<float> sm_move_z = StateValue<float>(&sm_move, move_pos_z, Z_AXIS_MIN, Z_AXIS_MAX);
StateValue<float> sm_move_e = StateValue<float>(&sm_move, move_pos_e, E_AXIS_MIN, E_AXIS_MAX);

State sm_disable_steppers = State(&sm_menu);

StateMenu sm_config = StateMenu(&sm_menu);
State sm_conf_load = State(&sm_config);
State sm_conf_save = State(&sm_config);
State sm_conf_clear = State(&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 sm_error = State(NULL);

#if defined(ENABLE_MOVEMENT_TEST_STATE) || defined(ENABLE_INPUT_TEST_STATE)
StateMenu sm_debug = StateMenu(&sm_menu);
#endif

#ifdef ENABLE_MOVEMENT_TEST_STATE
StateMenu sm_movement_test = StateMenu(&sm_debug);
State sm_move_xy_test = State(&sm_movement_test);
State sm_move_x_test = State(&sm_movement_test);
State sm_move_y_test = State(&sm_movement_test);
State sm_move_z_test = State(&sm_movement_test);
State sm_move_e_test = State(&sm_movement_test);
#endif // ENABLE_MOVEMENT_TEST_STATE

#ifdef ENABLE_INPUT_TEST_STATE
State sm_input_test = State(&sm_debug);
#endif // ENABLE_INPUT_TEST_STATE

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

State *current_state = NULL;
String strbuf;
struct data_config_preset wizard_preset = { .count_x = 1, .count_y = 1 };
float wizard_first_x = 0.0, wizard_first_y = 0.0;
float wizard_last_x = 0.0, wizard_last_y = 0.0;
unsigned long last_state_change = 0, last_input = 0;

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([]() {
#ifndef DISABLE_HOMING_STATE
        steppers_start_homing();
#endif // DISABLE_HOMING_STATE
    });

    sm_do_homing.whenIn([](StateMachineInput smi) {
#ifndef DISABLE_HOMING_STATE
        if (smi.all_motors_done) {
            if (steppers_homed()) {
#endif // DISABLE_HOMING_STATE

                async_beep(HOMING_BEEP_TIME, HOMING_BEEP_FREQ);
                states_go_to(&sm_menu);

#ifndef DISABLE_HOMING_STATE
            }
        }
#endif // DISABLE_HOMING_STATE

        static char last_axis = ' ';
        char axis = steppers_homing_axis();
        if ((axis != ' ') && (axis != last_axis)) {
            last_axis = axis;
            strbuf = String(F("Currently homing ")) + axis + String(F(" axis"));
            sm_do_homing.setText(strbuf.c_str());
            sm_do_homing.updateText();
        }
    });

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

    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_x.onEnter([]() {
        move_pos_x = steppers_pos_x();
    });
    sm_move_x.onLiveUpdate([](float v) {
        steppers_move_x(v);
    });

    sm_move_y.setTitle("Y Axis");
    sm_move_y.onEnter([]() {
        move_pos_y = steppers_pos_y();
    });
    sm_move_y.onLiveUpdate([](float v) {
        steppers_move_y(v);
    });

    sm_move_z.setTitle("Z Axis");
    sm_move_z.onEnter([]() {
        move_pos_z = steppers_pos_z();
    });
    sm_move_z.onLiveUpdate([](float v) {
        steppers_move_z(v);
    });

    sm_move_e.setTitle("E Axis");
    sm_move_e.onEnter([]() {
        move_pos_e = steppers_pos_e();
    });
    sm_move_e.onLiveUpdate([](float v) {
        steppers_move_e(v);
    });

    sm_disable_steppers.setTitle("Disable Steppers");
    sm_disable_steppers.onEnter([]() {
        steppers_kill();
        states_go_to(&sm_ask_homing);
    });

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

    sm_auto.setTitle("Filling Menu");

    auto preset_name_func = [](int i) {
        strbuf = String(i + 1);
        strbuf += String(F(" ("));
        strbuf += String(data_preset(i)->count_x);
        strbuf += String(F("*"));
        strbuf += String(data_preset(i)->count_y);
        strbuf += String(F(" / "));
        strbuf += String(data_preset(i)->distance_x);
        strbuf += String(F("*"));
        strbuf += String(data_preset(i)->distance_y);
        strbuf += String(F(")"));
        return strbuf.c_str();
    };

    sm_presets.setTitle("Use Preset");
    sm_presets.setPrefix("Preset ");
    sm_presets.dataCount([]() {
        return (int)data_preset_count();
    });
    sm_presets.dataGet(preset_name_func);
    sm_presets.dataCall([](int i) {
        preset_selection = i;
        states_go_to(&sm_disp_off);
    });

    sm_disp_off.setTitle("Offset");
    sm_disp_off.setText("Where to start dispensing?");
    sm_disp_off.onEnter([]() {
        dispense_off = 1;
        sm_disp_off.setMax(data_preset(preset_selection)->count_x * data_preset(preset_selection)->count_y);
    });

    sm_disp_cnt.setTitle("Count");
    sm_disp_cnt.setText("How many to fill?");
    sm_disp_cnt.onEnter([]() {
        dispense_count = data_preset(preset_selection)->count_x * data_preset(preset_selection)->count_y - (dispense_off - 1);
        sm_disp_cnt.setMax(dispense_count);
    });

    sm_disp_go.setTitle("Confirm");
    sm_disp_go.onEnter([]() {
        dispense_off -= 1;
        strbuf = String(F("Filling from ")) + String(dispense_off + 1);
        strbuf += String(F(" to ")) + String(dispense_off + dispense_count);
        strbuf += String(F("!\nIs this ok?"));
        sm_disp_go.setText(strbuf.c_str());

        dispense_index = dispense_off;

        // TODO allow yes/no choice?
    });

    sm_disp_run_z_travel.setTitle("Z Travel");
    sm_disp_run_z_travel.onEnter([]() {
        // TODO move z to travel height
    });
    sm_disp_run_z_travel.whenIn([](StateMachineInput smi) {
        // TODO update status screen

        if (smi.all_motors_done) {
            states_go_to(&sm_disp_run_xy_travel);
        }
    });

    sm_disp_run_xy_travel.setTitle("XY Movement");
    sm_disp_run_xy_travel.onEnter([]() {
        int y = dispense_index / data_preset(preset_selection)->count_x;
        int x = dispense_index % data_preset(preset_selection)->count_x;

        Serial.print(F("xy_travel: index="));
        Serial.print(dispense_index);
        Serial.print(F(" x="));
        Serial.print(x);
        Serial.print(F(" y="));
        Serial.println(y);

        float x_pos = data_preset(preset_selection)->offset_x + x * data_preset(preset_selection)->distance_x;
        float y_pos = data_preset(preset_selection)->offset_y + y * data_preset(preset_selection)->distance_y;

        steppers_move_x(x_pos);
        steppers_move_y(y_pos);
    });
    sm_disp_run_xy_travel.whenIn([](StateMachineInput smi) {
        // TODO update status screen

        if (smi.all_motors_done) {
            states_go_to(&sm_disp_run_z_disp);
        }
    });

    sm_disp_run_z_disp.setTitle("Z Dispense");
    sm_disp_run_z_disp.onEnter([]() {
        // TODO move z to dispension height
    });
    sm_disp_run_z_disp.whenIn([](StateMachineInput smi) {
        // TODO update status screen

        if (smi.all_motors_done) {
            states_go_to(&sm_disp_run);
        }
    });

    sm_disp_run.setTitle("Dispensing");
    sm_disp_run.onEnter([]() {
        // TODO start dispensing with e and moving z up
    });
    sm_disp_run.whenIn([](StateMachineInput smi) {
        // TODO update status screen

        if (smi.all_motors_done) {
            dispense_index++;
            if (dispense_index >= (dispense_count + dispense_off)) {
                states_go_to(&sm_disp_done);
            } else {
                states_go_to(&sm_disp_run_z_travel);
            }
        }
    });

    sm_disp_done.setTitle("Finished");
    sm_disp_done.onEnter([]() {
        // TODO show end screen, "stats"
    });
    sm_disp_done.whenIn([](StateMachineInput smi) {
        if (smi.click) {
            states_go_to(&sm_auto);
        }
    });

    sm_new_preset.setTitle("Add new Preset");
    sm_new_preset.setHeading("Preset Wizard");
    sm_new_preset.setText("Moving axes into initial positions. Click to continue.");

    sm_wiz_move.setTitle("Initial Movement");
    sm_wiz_move.onEnter([]() {
        steppers_start_homing();
    });
    sm_wiz_move.whenIn([](StateMachineInput smi) {
#ifndef DISABLE_HOMING_STATE
        if (smi.all_motors_done) {
            if (steppers_homed()) {
#endif // DISABLE_HOMING_STATE
                states_go_to(&sm_wiz_count);
#ifndef DISABLE_HOMING_STATE
            }
        }
#endif // DISABLE_HOMING_STATE

        static char last_axis = ' ';
        char axis = steppers_homing_axis();
        if ((axis != ' ') && (axis != last_axis)) {
            last_axis = axis;
            strbuf = String(F("Currently homing ")) + axis + String(F(" axis"));
            sm_wiz_move.setText(strbuf.c_str());
            sm_wiz_move.updateText();
        }
    });

    sm_wiz_count.setTitle("Container Count");
    sm_wiz_count.setData(0, "X: ", &wizard_preset.count_x, 1, 50);
    sm_wiz_count.setData(1, "Y: ", &wizard_preset.count_y, 1, 100);

    sm_wiz_first_pos.setTitle("First Container Position");
    sm_wiz_first_pos.setData(0, "X: ", &wizard_first_x, X_AXIS_MIN, X_AXIS_MAX);
    sm_wiz_first_pos.setData(1, "Y: ", &wizard_first_y, Y_AXIS_MIN, Y_AXIS_MAX);
    sm_wiz_first_pos.onEnter([]() {
        wizard_first_x = steppers_current_pos(0);
        wizard_first_y = steppers_current_pos(1);
    });
    sm_wiz_first_pos.onLiveUpdate([](size_t i, float v) {
        if (i == 0) {
            steppers_move_x(v);
        } else {
            steppers_move_y(v);
        }
    });
    sm_wiz_first_pos.onUpdate([](size_t i, float v) {
        if (i == 0) {
            wizard_preset.offset_x = v;
        } else {
            wizard_preset.offset_y = v;
        }
    });

    sm_wiz_last_pos.setTitle("Last Container Position");
    sm_wiz_last_pos.setData(0, "X: ", &wizard_last_x, X_AXIS_MIN, X_AXIS_MAX);
    sm_wiz_last_pos.setData(1, "Y: ", &wizard_last_y, Y_AXIS_MIN, Y_AXIS_MAX);
    sm_wiz_last_pos.onEnter([]() {
        wizard_last_x = steppers_current_pos(0);
        wizard_last_y = steppers_current_pos(1);
    });
    sm_wiz_last_pos.onLiveUpdate([](size_t i, float v) {
        if (i == 0) {
            steppers_move_x(v);
        } else {
            steppers_move_y(v);
        }
    });
    sm_wiz_last_pos.onUpdate([](size_t i, float v) {
        if (i == 0) {
            wizard_preset.distance_x = (v - wizard_preset.offset_x) / wizard_preset.count_x;
        } else {
            wizard_preset.distance_y = (v - wizard_preset.offset_y) / wizard_preset.count_y;
        }
    });

    sm_new_preset_done.setTitle("Preset Wizard Done");
    sm_new_preset_done.setHeading("Preset Wizard Done");
    sm_new_preset_done.onEnter([]() {
        if (!data_preset_add(wizard_preset)) {
            strbuf = String(data_eeprom_error()) + F(" Error adding preset!");
            sm_new_preset_done.setText(strbuf.c_str());
        } else {
            strbuf = F("Preset ");
            strbuf += String(data_preset_count());
            strbuf += F(" has been added! Don't forget to store config to EEPROM to keep it.");
            sm_new_preset_done.setText(strbuf.c_str());
        }
    });
    sm_new_preset_done.whenIn([](StateMachineInput smi) {
        if (smi.click) {
            states_go_to(&sm_auto);
        }
    });

    // TODO
    sm_mod_preset.setTitle("Modify Preset");
    sm_mod_preset.setPrefix("Preset ");
    sm_mod_preset.dataCount([]() {
        return (int)data_preset_count();
    });
    sm_mod_preset.dataGet(preset_name_func);

    sm_del_preset.setTitle("Delete Preset");
    sm_del_preset.setPrefix("Preset ");
    sm_del_preset.dataCount([]() {
        return (int)data_preset_count();
    });
    sm_del_preset.dataGet(preset_name_func);
    sm_del_preset.dataCall([](int i) {
        data_preset_remove(i);
        states_go_to(&sm_auto);
    });

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

    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_clear.setTitle("Clear EEPROM & RAM");
    sm_conf_clear.setHeading("Clear EEPROM & RAM");
    sm_conf_clear.setText("EEPROM and RAM Configuration cleared!");
    sm_conf_clear.onEnter([]() {
        data_clear();
        data_eeprom_write();

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

    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);
    });

    sm_error.setChild(&sm_ask_homing);
    sm_error.setTitle("Axis Error");
    sm_error.setHeading("Axis Error");
    sm_error.setText("An unknown error occured!");

#if defined(ENABLE_MOVEMENT_TEST_STATE) || defined(ENABLE_INPUT_TEST_STATE)
    sm_debug.setTitle("Debug Menu");
#endif

#ifdef ENABLE_MOVEMENT_TEST_STATE
    sm_movement_test.setTitle("Movement Test");

    sm_move_xy_test.setTitle("XY Axes");
    sm_move_xy_test.setHeading("XY Move Test");
    sm_move_xy_test.setText("Moving axis through full range with selected speed.");
    sm_move_xy_test.whenIn([](StateMachineInput smi) {
        static bool sx = false, sy = false;
        if (smi.motor_done[X_AXIS]) {
            sx = !sx;
            steppers_move_x(sx ? (X_AXIS_MAX - X_AXIS_EPSILON) : (X_AXIS_MIN + X_AXIS_EPSILON));
        }
        if (smi.motor_done[Y_AXIS]) {
            sy = !sy;
            steppers_move_y(sy ? (Y_AXIS_MAX - Y_AXIS_EPSILON) : (Y_AXIS_MIN + Y_AXIS_EPSILON));
        }
        if (smi.click) {
            states_go_to(&sm_movement_test);
        }
    });

    sm_move_x_test.setTitle("X Axis");
    sm_move_x_test.setHeading("X Move Test");
    sm_move_x_test.setText("Moving axis through full range with selected speed.");
    sm_move_x_test.whenIn([](StateMachineInput smi) {
        static bool s = false;
        if (smi.motor_done[X_AXIS]) {
            s = !s;
            steppers_move_x(s ? (X_AXIS_MAX - X_AXIS_EPSILON) : (X_AXIS_MIN + X_AXIS_EPSILON));
        }
        if (smi.click) {
            states_go_to(&sm_movement_test);
        }
    });

    sm_move_y_test.setTitle("Y Axis");
    sm_move_y_test.setHeading("Y Move Test");
    sm_move_y_test.setText("Moving axis through full range with selected speed.");
    sm_move_y_test.whenIn([](StateMachineInput smi) {
        static bool s = false;
        if (smi.motor_done[Y_AXIS]) {
            s = !s;
            steppers_move_y(s ? (Y_AXIS_MAX - Y_AXIS_EPSILON) : (Y_AXIS_MIN + Y_AXIS_EPSILON));
        }
        if (smi.click) {
            states_go_to(&sm_movement_test);
        }
    });

    sm_move_z_test.setTitle("Z Axis");
    sm_move_z_test.setHeading("Z Move Test");
    sm_move_z_test.setText("Moving axis through full range with selected speed.");
    sm_move_z_test.whenIn([](StateMachineInput smi) {
        static bool s = false;
        if (smi.motor_done[Z_AXIS]) {
            s = !s;
            steppers_move_z(s ? (Z_AXIS_MAX - Z_AXIS_EPSILON) : (Z_AXIS_MIN + Z_AXIS_EPSILON));
        }
        if (smi.click) {
            states_go_to(&sm_movement_test);
        }
    });

    sm_move_e_test.setTitle("E Axis");
    sm_move_e_test.setHeading("E Move Test");
    sm_move_e_test.setText("Moving axis through full range with selected speed.");
    sm_move_e_test.whenIn([](StateMachineInput smi) {
        static bool s = false;
        if (smi.motor_done[E_AXIS]) {
            s = !s;
            steppers_move_e(s ? (E_AXIS_MAX - E_AXIS_EPSILON) : (E_AXIS_MIN + E_AXIS_EPSILON));
        }
        if (smi.click) {
            states_go_to(&sm_movement_test);
        }
    });
#endif // ENABLE_MOVEMENT_TEST_STATE

#ifdef ENABLE_INPUT_TEST_STATE
    sm_input_test.setTitle("Input Test");
    sm_input_test.setHeading("Endstop Test");

    sm_input_test.whenIn([](StateMachineInput smi) {
        String s = "Endstops: ";
        for (int i = 0; i < AXIS_COUNT; i++) {
            if (steppers_home_switch(i)) {
                s += '1';
            } else {
                s += '0';
            }
            if (i < 3) {
                s += ' ';
            }
        }
        if (s != strbuf) {
            strbuf = s;
            sm_input_test.setText(strbuf.c_str());
            sm_input_test.updateText();
        }
    });
#endif // ENABLE_INPUT_TEST_STATE

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

    states_go_to(&sm_ask_homing);
}

void states_run(StateMachineInput smi) {
    if (smi.click || smi.kill || (smi.encoder != 0)) {
        last_input = millis();
    }

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

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

#if (IDLE_TIMEOUT > 0)
    unsigned long last_time = max(last_input, last_state_change);
    if ((millis() - last_time) > IDLE_TIMEOUT) {
        if (current_state != &sm_ask_homing) {
            Serial.println(F("Idle timeout, disabling motors"));
            steppers_kill();
            states_go_to(&sm_ask_homing);
            return;
        }
    }
#endif // (IDLE_TIMEOUT > 0)

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

State *states_get(void) {
    return current_state;
}

void states_go_to(State *state) {
    last_state_change = millis();

    current_state = state;

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