#include <Arduino.h>

#include "config.h"
#include "config_pins.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);

StateMenu sm_auto = StateMenu(&sm_menu);

StateMenu sm_config = StateMenu(&sm_menu);

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

State *current_state = NULL;

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

    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(100, 2000);
                states_go_to(&sm_menu);
            }
        }

        // TODO update text with current axis
    });

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

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

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

    sm_auto.setTitle("Filling Menu");
    sm_auto.setChild(&sm_menu);

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

    sm_config.setTitle("Configuration");
    sm_auto.setChild(&sm_menu);

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

    states_go_to(&sm_ask_homing);
}

void states_run(StateMachineInput smi) {
    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();
    }
}