#include <Arduino.h>
#include <TimerOne.h>

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

const static float hz = min(max(max(XY_MAX_HZ, Z_MAX_HZ), E_MAX_HZ), MAX_SAMPLE_FREQ);
const static float us = (1000.0 * 1000.0) / hz;

void print_data(void) {
    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_PERCENT);
    Serial.println(F(" steps/percent"));

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

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

    Serial.println();

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

    Serial.print(F("XY max move freq: "));
    Serial.print(XY_MAX_HZ);
    Serial.println(F("hz"));
    Serial.print(F("Z max move freq: "));
    Serial.print(Z_MAX_HZ);
    Serial.println(F("hz"));
    Serial.print(F("E max move freq: "));
    Serial.print(E_MAX_HZ);
    Serial.println(F("hz"));

    Serial.print(F("Sample Freq: "));
    Serial.print(hz);
    Serial.print(F("Hz = "));
    Serial.print(us);
    Serial.println(F("us"));
}

void fast_loop(void) {
    steppers_run();
    encoder_run_fast();
}

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 data"));
    data_init();

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

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

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

    //print_data();

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

    // wait some time to show splash screen
    while (millis() < 1500) { }

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

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

    Timer1.initialize(us);
    Timer1.attachInterrupt(fast_loop);
}

void loop() {
    common_run();
    encoder_run();

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

    CLEAR_STORE_INTERRUPTS();

    int change = encoder_change();
    int rpm = encoder_rpm();

    bool motor_done[AXIS_COUNT];
    bool all_done = true;
    for (int i = 0; i < AXIS_COUNT; i++) {
        motor_done[i] = stepper_reached_target(i);
        if (!motor_done[i]) {
            all_done = false;
        }
    }

    RESTORE_INTERRUPTS();

    // allow rudimentary control over serial
    // with enter and w/s or arrow up/down for encoder
    // and q/k/space keys for kill
    static int ser_state = 0;
    while (Serial.available()) {
        int c = Serial.read();
        if (ser_state == 0) {
            if (c == '\n') {
                click = 1;
            } else if ((c == 'q') || (c == 'Q') || (c == 'k') || (c == 'K') || (c == ' ')) {
                kill = 1;
            } else if ((c == 'w') || (c == 'W')) {
                change = 1;
            } else if ((c == 's') || (c == 'S')) {
                change = -1;
            } else if (c == '\e') {
                ser_state = 1;
            }
        } else if (ser_state == 1) {
            if (c == '[') {
                ser_state = 2;
            } else {
                ser_state = 0;
            }
        } else if (ser_state == 2) {
            if (c == 'A') {
                change = 1;
            } else if (c == 'B') {
                change = -1;
            }
            ser_state = 0;
        }
    }

    struct StateMachineInput smi(click, change, rpm, kill, motor_done, all_done);
    states_run(smi);

    lcd_loop();
}