#include <Preferences.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>


#define EN_PIN    12  // Nano v3:  16 Mega:  38  //enable (CFG6)
#define DIR_PIN   13  //      19      55  //direction
#define STEP_PIN  14  //      18      54  //step
#define CS_PIN    5  //      17      64  //chip select

#define ACCEL 20000

#define MIN_TIME 1
#define MAX_TIME 250
#define MIN_SPEED 0.1
#define MAX_SPEED 20.0
#define MIN_LENGTH 1
#define MAX_LENGTH 90
#define MIN_ACCEL 0
#define MAX_ACCEL 100
#define MIN_DELAY 50
#define MAX_DELAY 10000

#define MSF 256 //Microstepping factor

const int steps_per_mm = 200 * MSF / 40;
const int mm_per_cm = 10;

long slider_position = 0;
boolean slider_move = false;
boolean slider_move_init = false;

#include <TMC2130Stepper.h>
TMC2130Stepper driver = TMC2130Stepper(EN_PIN, DIR_PIN, STEP_PIN, CS_PIN);

#include <AccelStepper.h>
AccelStepper stepper = AccelStepper(stepper.DRIVER, STEP_PIN, DIR_PIN);

Preferences preferences;

#define LEDL_PIN    16
#define LEDR_PIN    17

#define ENC_A 34
#define ENC_B 35
int enc_val = 0;

#define BTN1 33
#define BTN2 32

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1

const unsigned char Logo [] PROGMEM = {
  // 'logo_black_white_circle_6464, 64x64px
  0x00, 0x00, 0x00, 0x0f, 0xf2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x40, 0x03, 0xc0, 0x00, 0x00,
  0x00, 0x00, 0x0c, 0x3e, 0x60, 0x30, 0x00, 0x00, 0x00, 0x00, 0x61, 0xfe, 0x77, 0x86, 0x00, 0x00,
  0x00, 0x00, 0x83, 0xfe, 0x77, 0xf1, 0x00, 0x00, 0x00, 0x03, 0x33, 0xfe, 0x77, 0xf0, 0xc0, 0x00,
  0x00, 0x04, 0xf9, 0xfe, 0x77, 0xe7, 0x20, 0x00, 0x00, 0x19, 0xfc, 0xd6, 0x77, 0xcf, 0x98, 0x00,
  0x00, 0x27, 0xfc, 0xf6, 0x77, 0x9f, 0xe4, 0x00, 0x00, 0x4f, 0xfc, 0xd6, 0x77, 0x3f, 0xf2, 0x00,
  0x00, 0x9f, 0xfc, 0xce, 0x77, 0x3f, 0xf9, 0x00, 0x01, 0x3f, 0xfc, 0xee, 0x77, 0x3f, 0xfc, 0x80,
  0x01, 0x7f, 0xfc, 0xce, 0x63, 0x3f, 0xfe, 0x80, 0x02, 0xff, 0xfc, 0xc6, 0x63, 0x3f, 0xff, 0x40,
  0x04, 0xff, 0xfc, 0xc6, 0x63, 0x3f, 0xff, 0x20, 0x05, 0xff, 0xf8, 0xc6, 0x63, 0x3f, 0xff, 0xa0,
  0x0b, 0xff, 0x80, 0x00, 0x00, 0x01, 0xff, 0xd0, 0x11, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0xc8,
  0x10, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0xe8, 0x04, 0x7f, 0x00, 0x00, 0x00, 0x00, 0xff, 0xe0,
  0x26, 0x3f, 0x00, 0x00, 0x00, 0x00, 0xff, 0xf4, 0x27, 0x18, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x74,
  0x43, 0x80, 0x00, 0x00, 0x00, 0x00, 0x03, 0x72, 0x59, 0xe0, 0x0c, 0x9b, 0x5e, 0xe0, 0x03, 0x7a,
  0x5c, 0xff, 0x0f, 0x9b, 0xd8, 0xb0, 0xfe, 0xfa, 0x5e, 0x7f, 0x0f, 0xbb, 0x9c, 0xf0, 0xff, 0xfb,
  0x3f, 0x00, 0x0c, 0xbb, 0xd8, 0xe0, 0x00, 0x2c, 0x3f, 0x80, 0x0c, 0xbf, 0x58, 0xa0, 0x00, 0x6c,
  0xbf, 0xf8, 0x0c, 0xef, 0x7e, 0xb0, 0x1f, 0x8d, 0xbf, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0xfd,
  0xbc, 0xfe, 0x00, 0x00, 0x00, 0x00, 0xff, 0xfd, 0xbb, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0xbb, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xbc, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0xfd,
  0xbf, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0xfd, 0x9f, 0xf0, 0x32, 0xef, 0x5b, 0xbc, 0x1f, 0xfd,
  0x00, 0x00, 0x3f, 0xbf, 0x76, 0xf8, 0x00, 0x00, 0x1f, 0xf0, 0x3f, 0xbc, 0x76, 0xf0, 0x00, 0x00,
  0x5f, 0xff, 0x3f, 0xbe, 0x56, 0xf8, 0xff, 0xfb, 0x5f, 0xfe, 0x23, 0xbc, 0x5e, 0xf0, 0xff, 0xfa,
  0x40, 0x00, 0x22, 0xef, 0x4b, 0xbc, 0x00, 0xfa, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x72,
  0x2f, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0x34, 0x2f, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0x84,
  0x07, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0xc0, 0x17, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0xe8,
  0x13, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0xc8, 0x0b, 0xff, 0x80, 0x00, 0x00, 0x01, 0xff, 0xd0,
  0x05, 0xff, 0xf8, 0xc6, 0x63, 0x3f, 0xff, 0xa0, 0x04, 0xff, 0xfc, 0xc6, 0x63, 0x3f, 0xff, 0x20,
  0x02, 0xff, 0xfc, 0xc6, 0x63, 0x3f, 0xff, 0x40, 0x01, 0x7f, 0xfc, 0xce, 0x73, 0x3f, 0xfe, 0x80,
  0x01, 0x3f, 0xfc, 0xce, 0x73, 0x3f, 0xfc, 0x80, 0x00, 0x9f, 0xfc, 0xce, 0x73, 0x3f, 0xf9, 0x00,
  0x00, 0x4f, 0xfc, 0xce, 0x73, 0x9f, 0xf2, 0x00, 0x00, 0x27, 0xfc, 0xce, 0x73, 0x8f, 0xe4, 0x00,
  0x00, 0x19, 0xf9, 0xce, 0x73, 0xc7, 0x98, 0x00, 0x00, 0x04, 0xf3, 0xce, 0x73, 0xe3, 0x20, 0x00,
  0x00, 0x03, 0x07, 0xce, 0x79, 0xf0, 0xc0, 0x00, 0x00, 0x00, 0x87, 0xcf, 0x3c, 0xf1, 0x00, 0x00,
  0x00, 0x00, 0x61, 0xcf, 0x9e, 0x06, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x0f, 0xc4, 0x30, 0x00, 0x00,
  0x00, 0x00, 0x03, 0x40, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xf2, 0x00, 0x00, 0x00
};

const unsigned char tick [] PROGMEM = {
  // 'img_2050_16x16, 16x16px
  0x00, 0x0e, 0x00, 0x1f, 0x00, 0x1f, 0x00, 0x3f, 0x00, 0x3e, 0x00, 0x7e, 0x70, 0x7c, 0xf8, 0xfc,
  0xfd, 0xf8, 0xff, 0xf0, 0x7f, 0xf0, 0x3f, 0xe0, 0x1f, 0xe0, 0x0f, 0xc0, 0x07, 0xc0, 0x03, 0x80
};

const unsigned char arrow_right [] PROGMEM = {
  // 'img_465808_10x16, 10x16px
  0x60, 0x00, 0xf0, 0x00, 0xf8, 0x00, 0x7c, 0x00, 0x3e, 0x00, 0x1f, 0x00, 0x0f, 0x80, 0x07, 0xc0,
  0x07, 0xc0, 0x0f, 0x80, 0x1f, 0x00, 0x3e, 0x00, 0x7c, 0x00, 0xf8, 0x00, 0xf0, 0x00, 0x60, 0x00
};

const unsigned char arrow_left [] PROGMEM = {
  // 'img_465807_10x16, 10x16px
  0x01, 0x80, 0x03, 0xc0, 0x07, 0xc0, 0x0f, 0x80, 0x1f, 0x00, 0x3e, 0x00, 0x7c, 0x00, 0xf8, 0x00,
  0xf8, 0x00, 0x7c, 0x00, 0x3e, 0x00, 0x1f, 0x00, 0x0f, 0x80, 0x07, 0xc0, 0x03, 0xc0, 0x01, 0x80
};

const unsigned char cancel_x [] PROGMEM = {
  // 'img_293678_16x16, 16x16px
  0xe0, 0x07, 0xf0, 0x0f, 0xf8, 0x1f, 0x7c, 0x3e, 0x3e, 0x7c, 0x1f, 0xf8, 0x0f, 0xf0, 0x07, 0xe0,
  0x07, 0xe0, 0x0f, 0xf0, 0x1f, 0xf8, 0x3e, 0x7c, 0x7c, 0x3e, 0xf8, 0x1f, 0xf0, 0x0f, 0xe0, 0x07
};

const unsigned char placeholder [] PROGMEM = {
  // 'img_293678_16x16, 16x16px
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};


Adafruit_SSD1306 disp(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//MENU
#define MENU_START 0
#define MENU_MODE 1
#define MENU_SETTINGS 2
String menu_header[] = {"START", "MODE", "SETTINGS"};
int menupoint = 0;
const int n_menupoints = 3;

boolean main_menu = true;

boolean open_menu[n_menupoints] = {false};

int MODE = 0;
const int n_MODES = 2;
String MODE_STR[] = {"TIME", "LENGTH"};
#define MODE_TIME 0
#define MODE_LENGTH 1

int slider_dir = 0; //true: M->W
const int n_slider_dir = 3;
String slider_dir_str[] = {"M -> W", "M <- W", "M <-> W"};
#define DIR_MW 0
#define DIR_WM 1
#define DIR_ALT 2


#define CONFIGMENU_TIME 0
#define CONFIGMENU_LENGTH 1
#define CONFIGMENU_SPEED 2
#define CONFIGMENU_ACCEL 3
#define CONFIGMENU_DIR 4
#define CONFIGMENU_DELAY 5
String configmenu_header[] = {"Time", "Length", "Speed", "Accel.", "Direction", "Delay"};
int config_menupoint = 0;
const int config_n_menupoints = 6;


int TIME_MOVE = 10;
int LENGTH_MOVE = MAX_LENGTH;
float SPEED_MOVE  = 1.0;
int ACCEL_MOVE = 10;
boolean DIR_MOVE = true;
int DELAY_MOVE = 100;


#define PA_TIME 10
#define PA_LENGTH 11
#define PA_SPEED 12
#define PA_ACCEL 13
#define PA_DIR 14
#define PA_MODE 15
#define PA_DELAY 16
#define PV_DELAY 50

void readPREF() {
  preferences.begin("cameraslider", false);
  TIME_MOVE = preferences.getUInt("PA_TIME");
  LENGTH_MOVE = preferences.getUInt("PA_LENGTH");
  SPEED_MOVE = preferences.getFloat("PA_SPEED");
  ACCEL_MOVE = preferences.getUInt("PA_ACCEL");
  slider_dir = preferences.getUInt("PA_DIR");
  MODE = preferences.getUInt("PA_MODE");
  DELAY_MOVE = preferences.getUInt("PA_DELAY");
  preferences.end();
}

void writePREF(int ADDR) {
  preferences.begin("cameraslider", false);
  switch (ADDR) {
    case PA_TIME: preferences.putUInt("PA_TIME", TIME_MOVE); break;
    case PA_LENGTH: preferences.putUInt("PA_LENGTH", LENGTH_MOVE); break;
    case PA_SPEED: preferences.putFloat("PA_SPEED", SPEED_MOVE); break;
    case PA_ACCEL: preferences.putUInt("PA_ACCEL", ACCEL_MOVE); break;
    case PA_DIR: preferences.putUInt("PA_DIR", slider_dir); break;
    case PA_MODE: preferences.putUInt("PA_MODE", MODE); break;
    case PA_DELAY: preferences.putUInt("PA_DELAY", DELAY_MOVE); break;
  }
  preferences.end();
}

void writePREF_ALL() {
  preferences.begin("cameraslider", false);
  preferences.putUInt("PA_TIME", TIME_MOVE);
  preferences.putUInt("PA_LENGTH", LENGTH_MOVE);
  preferences.putFloat("PA_SPEED", SPEED_MOVE);
  preferences.putUInt("PA_ACCEL", ACCEL_MOVE);
  preferences.putUInt("PA_DIR", slider_dir);
  preferences.putUInt("PA_MODE", MODE);
  preferences.putUInt("PA_DELAY", DELAY_MOVE);
  preferences.end();
}


void setup() {
  Serial.begin(115200);
  Serial.println("start");

  pinMode(BTN1, INPUT);
  pinMode(BTN2, INPUT);
  pinMode(ENC_A, INPUT);
  pinMode(ENC_B, INPUT);

  pinMode(LEDL_PIN, OUTPUT);
  digitalWrite(LEDL_PIN, LOW);
  pinMode(LEDR_PIN, OUTPUT);
  digitalWrite(LEDR_PIN, LOW);

  SPI.begin();

  pinMode(CS_PIN, OUTPUT);
  digitalWrite(CS_PIN, HIGH);

  driver.begin();             // Initiate pins and registeries
  driver.rms_current(300);    // Set stepper current to 600mA. The command is the same as command TMC2130.setCurrent(600, 0.11, 0.5);
  driver.stealthChop(1);      // Enable extremely quiet stepping
  driver.stealth_autoscale(1);
  driver.microsteps(MSF);

  stepper.setMaxSpeed(MAX_SPEED);
  stepper.setAcceleration(ACCEL);
  stepper.setEnablePin(EN_PIN);
  stepper.setPinsInverted(false, false, true);
  stepper.disableOutputs();

  if (!disp.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed 1"));
    for (;;); // Don't proceed, loop forever
  }

  delay(200);
  disp.clearDisplay();
  disp.drawBitmap(64 - 32, 0, Logo, 64, 64, 1);
  disp.display();

  delay(1000);
  disp.clearDisplay();

  disp.setTextColor(WHITE);
  disp.setTextSize(1);

  readPREF();

  drawButtons(0, 0);

  drawMenu(menupoint);
}


unsigned long t_enc = millis();
unsigned long t_slider = millis();

void loop() {


  if (slider_move) {
    if (slider_move_init) {
      if (slider_dir == DIR_MW) DIR_MOVE = false;
      else if (slider_dir == DIR_WM) DIR_MOVE = true;
      else if (slider_dir == DIR_ALT) DIR_MOVE = false;

      int l = 0;
      int s = 0;
      int a = 0;

      drawMoveScreen(true, DIR_MOVE);

      if (MODE == MODE_LENGTH) {
        l = LENGTH_MOVE * steps_per_mm * mm_per_cm;
        s = SPEED_MOVE * steps_per_mm * mm_per_cm;
        a = ACCEL_MOVE * 5000;
      }
      else if (MODE == MODE_TIME) {
        l = LENGTH_MOVE * steps_per_mm * mm_per_cm;
        s = LENGTH_MOVE / TIME_MOVE * steps_per_mm * mm_per_cm;
        a = ACCEL_MOVE * 5000;
      }

      stepper.disableOutputs();
      stepper.setCurrentPosition(0);
      stepper.move(l);
      stepper.setMaxSpeed(s);
      stepper.setAcceleration(a);
      delay(100);
      stepper.enableOutputs();
      driver.shaft_dir(DIR_MOVE);

      slider_move_init = false;
    }

    if (stepper.distanceToGo() == 0) {
      if (slider_dir == DIR_ALT) {
        stepper.disableOutputs();
        stepper.setCurrentPosition(0);
        stepper.move(LENGTH_MOVE * steps_per_mm * mm_per_cm);
        delay(DELAY_MOVE);
        stepper.enableOutputs();
        DIR_MOVE = !DIR_MOVE;
        driver.shaft_dir(DIR_MOVE);

        drawMoveScreen(true, DIR_MOVE);
      }
      else {
        for (int i = 0; i < 5; i++) {
          digitalWrite(LEDL_PIN, HIGH);
          delay(100);
          digitalWrite(LEDL_PIN, LOW);
          delay(100);
        }

        slider_move = false;
        slider_move_init = true;
        drawMenu(menupoint);
      }
    }
    else {
      stepper.run();
    }

  }
  else {
    if (slider_move_init) {
      stepper.stop();
      stepper.disableOutputs();
      slider_move_init = false;
    }
  }




  if (readEncoder(enc_val)) {
    int f = 1;
    if (millis() - t_enc < 10) f = 5;

    if (enc_val == 1) {
      if (main_menu) {
        drawMenu(++menupoint);
      }
      else if (menupoint == MENU_SETTINGS) {
        IncConfigM(config_menupoint, true, f);
      }
      else {
        for (int i = 0; i < n_menupoints; i++)
          if (open_menu[i]) IncM(i, true);
      }
    }
    else if (enc_val == -1) {
      if (main_menu) {
        drawMenu(--menupoint);
      }
      else if (menupoint == MENU_SETTINGS) {
        IncConfigM(config_menupoint, false, f);
      }
      else {
        for (int i = 0; i < n_menupoints; i++)
          if (open_menu[i]) IncM(i, false);
      }
    }
    enc_val = 0;
    t_enc = millis();
  }


  if (digitalRead(BTN1) == 0) {
    drawButtons(1, 0);

    if (slider_move) {
      slider_move = false;
      slider_move_init = true;
      digitalWrite(LEDL_PIN, slider_move);
      delay(500);
      drawMenu(menupoint);
    }
    else if (main_menu) {
      menupoint = 0;
      drawMenu(menupoint);
    }
    else if (open_menu[MENU_SETTINGS]) {
      drawConfigMenu(++config_menupoint);
    }
    else {
      digitalWrite(LEDR_PIN, HIGH);
      for (int i = 0; i < n_menupoints; i++) open_menu[i] = false;
      main_menu = true;
      drawButtons(0, 0);
      drawMenu(menupoint);
      delay(30);
      digitalWrite(LEDR_PIN, LOW);
    }

    while (digitalRead(BTN1) == 0) delay(10);
    delay(100);
    drawButtons(0, 0);
  }
  else if (digitalRead(BTN2) == 0) {
    drawButtons(0, 1);

    if (main_menu) {
      if (menupoint == MENU_START) {
        slider_move = true;
        slider_move_init = true;
        delay(500);
        digitalWrite(LEDL_PIN, slider_move);
      }
      else {
        HandleMenu(menupoint);
      }
    }
    else {
      if (open_menu[MENU_SETTINGS]) {
        writePREF_ALL();
        for (int i = 0; i < 5; i++) {
          digitalWrite(LEDR_PIN, HIGH);
          delay(20);
          digitalWrite(LEDR_PIN, LOW);
          delay(20);
        }
      }
      digitalWrite(LEDR_PIN, HIGH);
      for (int i = 0; i < n_menupoints; i++) open_menu[i] = false;
      main_menu = true;
      drawButtons(0, 0);
      drawMenu(menupoint);
      delay(30);
      digitalWrite(LEDR_PIN, LOW);
    }

    while (digitalRead(BTN2) == 0) delay(10);
    delay(100);
    drawButtons(0, 0);
  }
}


float roundfloat(float val) {
  int temp = val * 10;
  temp = round(temp);
  return temp / 10;
}

void drawButtons(boolean b1, boolean b2) {
  disp.fillRect(0, 44, 20, 20, BLACK);
  disp.fillRect(106, 44, 20, 20, BLACK);

  disp.drawRoundRect(2, 44, 20, 20, 3, WHITE);
  disp.drawRoundRect(106, 44, 20, 20, 3, WHITE);

  if (b1) disp.fillRoundRect(4, 46, 16, 16, 3, WHITE);
  else if (open_menu[MENU_SETTINGS]) disp.drawBitmap(8, 54 - 8, arrow_right, 10, 16, 1);
  else disp.drawBitmap(4, 54 - 8, cancel_x, 16, 16, 1);

  if (b2) disp.fillRoundRect(108, 46, 16, 16, 3, WHITE);
  else disp.drawBitmap(108, 54 - 8, tick, 16, 16, 1);

  disp.display();
}

void drawPercbox(int perc, boolean up) {
  disp.fillRect(24, 44, 78, 20, BLACK);

  disp.drawRoundRect(23, 44, 82, 20, 3, WHITE);

  disp.setTextSize(1);
  disp.setCursor(29, 50);
  disp.print("-");
  disp.setCursor(95, 50);
  disp.print("+");

  disp.drawLine(38, 48, 38, 58, WHITE);
  disp.drawLine(90, 48, 90, 58, WHITE);

  disp.drawLine(41, 53, 87, 53, WHITE);
  disp.fillRect(41, 49, map(perc, 0, 100, 0, 47), 9, WHITE);

  if (up) disp.display();
}

void drawMovebox(boolean dir, boolean up) {
  disp.fillRect(24, 44, 78, 20, BLACK);

  disp.drawRoundRect(23, 44, 82, 20, 3, WHITE);

  disp.setTextSize(1);
  disp.setCursor(29, 50);
  disp.print("M");
  disp.setCursor(95, 50);
  disp.print("W");

  disp.drawLine(38, 48, 38, 58, WHITE);
  disp.drawLine(90, 48, 90, 58, WHITE);

  //disp.drawLine(41, 53, 87, 53, WHITE);
  //disp.fillRect(41, 49, map(perc, 0, 100, 0, 47), 9, WHITE);

  disp.setTextSize(1);
  disp.setCursor(X(1, 3), 50);
  if (dir) disp.print("<<<");
  else disp.print(">>>");

  if (up) disp.display();
}

void drawMenu(int &point) {
  disp.fillRect(0, 0, 128, 44, BLACK);
  disp.drawRoundRect(2, 2, 124, 41, 5, WHITE);

  if (point < 0) point = n_menupoints - 1;
  else if (point > n_menupoints - 1) point = 0;

  drawPercbox(0, false);

  int len = menu_header[point].length();
  disp.setTextSize(2);
  disp.setCursor(X(2, len), Y(2, 0.35));
  disp.print(menu_header[point]);
  disp.display();
}

void HandleMenu(int m) {
  open_menu[m] = !open_menu[m];

  if (open_menu[m]) {
    main_menu = false;
    drawMenuM(m);
  }
  else drawMenu(menupoint);
}


void drawMenuM(int m) {
  disp.fillRect(0, 0, 128, 44, BLACK);
  disp.drawRoundRect(2, 2, 124, 41, 5, WHITE);

  String str = "";
  String val = "";

  if (m == MENU_MODE) {
    str = "SET: " + menu_header[MENU_MODE];
    val = MODE_STR[MODE];
    int len = str.length();
    disp.setTextSize(1);
    disp.setCursor(X(1, len), Y(1, 0.18));
    disp.print(str);

    disp.setTextSize(2);
    len = val.length();
    disp.setCursor(X(2, len), Y(2, 0.45));
    disp.print(val);

    disp.display();
  }
  else if (m == MENU_SETTINGS) {
    drawConfigMenu(config_menupoint);
  }
  else {

  }
}

void drawConfigMenu(int &m) {
  disp.fillRect(0, 0, 128, 44, BLACK);
  disp.drawRoundRect(2, 2, 124, 41, 5, WHITE);

  if (m > config_n_menupoints - 1) m = 0;

  String str = "SET > ";
  String val = "";

  switch (m) {
    case CONFIGMENU_TIME:
      str += configmenu_header[CONFIGMENU_TIME] + " [s]";
      val = String(TIME_MOVE);
      drawPercbox(map(TIME_MOVE, MIN_TIME, MAX_TIME, 0, 100), false);
      break;
    case CONFIGMENU_LENGTH:
      str += configmenu_header[CONFIGMENU_LENGTH] + " [cm]";
      val = String(LENGTH_MOVE);
      drawPercbox(map(LENGTH_MOVE, MIN_LENGTH, MAX_LENGTH, 0, 100), false);
      break;
    case CONFIGMENU_SPEED:
      str += configmenu_header[CONFIGMENU_SPEED] + " [cm/s]";
      val = String(SPEED_MOVE);
      drawPercbox(map(SPEED_MOVE, MIN_SPEED, MAX_SPEED, 0, 100), false);
      break;
    case CONFIGMENU_DIR:
      str += configmenu_header[CONFIGMENU_DIR] + " [<>]";
      val = String(slider_dir_str[slider_dir]);
      drawPercbox(map(slider_dir, 0, n_slider_dir - 1, 0, 100), false);
      break;
    case CONFIGMENU_ACCEL:
      str += configmenu_header[CONFIGMENU_ACCEL] + " [cm/s2]";
      val = String(ACCEL_MOVE);
      drawPercbox(map(ACCEL_MOVE, MIN_ACCEL, MAX_ACCEL, 0, 100), false);
      break;
    case CONFIGMENU_DELAY:
      str += configmenu_header[CONFIGMENU_DELAY] + " [ms]";
      val = String(DELAY_MOVE);
      drawPercbox(map(DELAY_MOVE, MIN_DELAY, MAX_DELAY, 0, 100), false);
      break;
  }

  int len = str.length();
  disp.setTextSize(1);
  disp.setCursor(X(1, len), Y(1, 0.18));
  disp.print(str);

  disp.setTextSize(2);
  len = val.length();
  disp.setCursor(X(2, len), Y(2, 0.45));
  disp.print(val);

  disp.display();
}


void drawMoveScreen(boolean up, boolean dir) {
  if (up) {
    disp.fillRect(0, 0, 128, 44, BLACK);
    disp.drawRoundRect(2, 2, 124, 41, 5, WHITE);
  }

  String str = "MODE: " + MODE_STR[MODE];
  int len = str.length();
  disp.setTextSize(1);
  disp.setCursor(X(1, len), Y(1, 0.18));
  disp.print(str);

  drawMovebox(dir, false);
  if (MODE == MODE_LENGTH) str = String(SPEED_MOVE) + "cm/s, (" + String(int(LENGTH_MOVE / SPEED_MOVE)) + "s)";
  else if (MODE == MODE_TIME) str = "(" + String(LENGTH_MOVE / TIME_MOVE) + "cm/s), " + String(TIME_MOVE) + "s";
  len = str.length();
  disp.setCursor(X(1, len), Y(1, 0.38));
  disp.print(str);

  str = String(LENGTH_MOVE) + "cm, " + String(slider_dir_str[slider_dir]);
  len = str.length();
  disp.setCursor(X(1, len), Y(1, 0.57));
  disp.print(str);

  disp.display();
}


void IncM(int m, boolean inc) {
  switch (m) {
    case MENU_MODE:
      if (inc) MODE += 1;
      else MODE -= 1;
      if (MODE == n_MODES) MODE = 0;
      if (MODE == -1) MODE = n_MODES - 1;
      break;
  }

  drawMenuM(m);
}

void IncConfigM(int m, boolean inc, int inc_factor) {
  switch (m) {
    case CONFIGMENU_TIME:
      if (inc) TIME_MOVE += 1 * inc_factor;
      else TIME_MOVE -= 1 * inc_factor;
      TIME_MOVE = constrain(TIME_MOVE, MIN_TIME, MAX_TIME);
      break;
    case CONFIGMENU_SPEED:
      if (inc) SPEED_MOVE += 0.1 * float(inc_factor);
      else SPEED_MOVE -= 0.1 * float(inc_factor);
      SPEED_MOVE = constrain(SPEED_MOVE, MIN_SPEED, MAX_SPEED);
      break;
    case CONFIGMENU_LENGTH:
      if (inc) LENGTH_MOVE += 1 * inc_factor;
      else LENGTH_MOVE -= 1 * inc_factor;
      LENGTH_MOVE = constrain(LENGTH_MOVE, MIN_LENGTH, MAX_LENGTH);
      break;
    case CONFIGMENU_DIR:
      if (inc) slider_dir += 1;
      else slider_dir -= 1;
      slider_dir = constrain(slider_dir, 0, n_slider_dir - 1);
      break;
    case CONFIGMENU_ACCEL:
      if (inc) ACCEL_MOVE += 1 * inc_factor;
      else ACCEL_MOVE -= 1 * inc_factor;
      ACCEL_MOVE = constrain(ACCEL_MOVE, MIN_ACCEL, MAX_ACCEL);
      break;
    case CONFIGMENU_DELAY:
      if (inc) DELAY_MOVE += 100 * inc_factor;
      else DELAY_MOVE -= 100 * inc_factor;
      DELAY_MOVE = constrain(DELAY_MOVE, MIN_DELAY, MAX_DELAY);
      break;
  }

  drawConfigMenu(m);
}


boolean readEncoder(int &val) {
  boolean newEncode = false;
  static boolean last_aState = 0;

  boolean aState = digitalRead(ENC_A);
  boolean bState = digitalRead(ENC_B);

  if (aState != last_aState) {
    if (aState != bState) val--;
    else val++;

    last_aState = aState;
    newEncode = true;
  }
  return newEncode;
}


int X(int textgroesse, int n) {
  //gibt die X koordinate aus, damit text mit n zeichen mittig ist
  return (0.5 * (128 - textgroesse * (6 * n - 1)));
}//end int X

int X(int textgroesse, int n, int offset) {
  //gibt die X koordinate aus, damit text mit n zeichen mittig ist
  return (0.5 * (128 - textgroesse * (6 * n - 1))) + offset;
}//end int X

int Y(int textgroesse, float f) {
  //gibt die Y koordinate aus, damit text mittig ist
  return (f * 64 - (textgroesse * 4));
}//end int Y