#include <Arduino.h>

#include "config.h"
#include "config_pins.h"
#include "common.h"
#include "encoder.h"
#include "lcd.h"
#include "steppers.h"
#include "states.h"

void setup() {
    pinMode(LED_PIN, OUTPUT);
    digitalWrite(LED_PIN, HIGH);

    pinMode(BEEPER, OUTPUT);
    blocking_beep(100, 1000);
    
    Serial.begin(115200);
    Serial.println(F("Initializing Fuellfix v2"));
    Serial.print(F("Version: "));
    Serial.println(FIRMWARE_VERSION);

    Serial.println(F("Init encoder"));
    encoder_init();

    Serial.println(F("Init LCD"));
    lcd_init();

    Serial.println(F("Init stepper motors"));
    steppers_init();

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

    Serial.println(F("XY:"));

    Serial.print(F("\t X: "));
    Serial.print(X_AXIS_MIN);
    Serial.print(F(" mm - "));
    Serial.print(X_AXIS_MAX);
    Serial.println(F(" mm"));

    Serial.print(F("\t Y: "));
    Serial.print(Y_AXIS_MIN);
    Serial.print(F(" mm - "));
    Serial.print(Y_AXIS_MAX);
    Serial.println(F(" mm"));

    Serial.print(F("\t"));
    Serial.print(XY_STEPS_PER_MM);
    Serial.println(F(" steps/mm"));

    Serial.print(F("\t"));
    Serial.print(XY_MAX_SPEED);
    Serial.println(F(" mm/s"));

    Serial.print(F("\t"));
    Serial.print(XY_MAX_ACCEL);
    Serial.println(F(" mm/s^2"));

    Serial.println();

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

    Serial.println(F("Z:"));

    Serial.print(F("\t"));
    Serial.print(Z_AXIS_MIN);
    Serial.print(F(" mm - "));
    Serial.print(Z_AXIS_MAX);
    Serial.println(F(" mm"));

    Serial.print(F("\t"));
    Serial.print(Z_STEPS_PER_MM);
    Serial.println(F(" steps/mm"));

    Serial.print(F("\t"));
    Serial.print(Z_MAX_SPEED);
    Serial.println(F(" mm/s"));

    Serial.print(F("\t"));
    Serial.print(Z_MAX_ACCEL);
    Serial.println(F(" mm/s^2"));

    Serial.println();

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

    Serial.println(F("E:"));

    Serial.print(F("\t"));
    Serial.print(E_AXIS_MIN);
    Serial.print(F(" mm - "));
    Serial.print(E_AXIS_MAX);
    Serial.println(F(" mm"));

    Serial.print(F("\t"));
    Serial.print(E_STEPS_PER_MM);
    Serial.println(F(" steps/mm"));

    Serial.print(F("\t"));
    Serial.print(E_MAX_SPEED);
    Serial.println(F(" mm/s"));

    Serial.print(F("\t"));
    Serial.print(E_MAX_ACCEL);
    Serial.println(F(" mm/s^2"));

    Serial.println();

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

    Serial.println(F("ready, showing splash screen"));
    digitalWrite(LED_PIN, LOW);
    blocking_beep(100, 2000);

    // wait some time to show splash screen
    delay(2000);

    Serial.println(F("Init state machine"));
    states_init();

    blocking_beep(100, 2000);
    Serial.println(F("starting main loop"));
}

void loop() {
    unsigned long t = millis();
    common_run(t);
    bool still_running = steppers_run();
    encoder_run();

    int click = encoder_click();
    int kill = kill_switch();

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

    struct StateMachineInput smi(click, encoder_change(), kill, !still_running);
    states_run(smi);
}