#include "Wire.h"
#include <MPU6050_light.h>
#include <ESP32Servo.h>
#include <U8g2lib.h>

// SerialPrint als Debug
#define DEBUG 1

#if DEBUG == 1
#define debug(x) Serial.print(x)
#define debugg(x, y) Serial.print(x, y)
#define debugln(x) Serial.println(x)
#define debuggln(x, y) Serial.println(x, y)
#else
#define debug(x)
#define debugg(x, y)
#define debugln(x)
#define debugg(x, y)
#endif

// Bitmaps Camper
// Front - Mitte linker Reifen bei px 22, Mitte rechter Reifen bei px 42
// Seite - Mitte linker Reifen bei px 14, Mitte rechter Reifen bei px 51
#define XBMP_Camper_Breite  64
#define XBMP_Camper_Hoehe 35
static unsigned char PROGMEM XBMP_CamperFront[] =  {
  0x00, 0x00, 0xf8, 0xff, 0xff, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x00,
  0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x20, 0x00, 0x00,
  0x00, 0x00, 0x84, 0xff, 0xff, 0x21, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00,
  0x00, 0x22, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x00, 0x22, 0x00, 0x00,
  0x00, 0x00, 0x84, 0xff, 0xff, 0x21, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00,
  0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x20, 0x00, 0x00,
  0x00, 0x00, 0xfc, 0xff, 0xff, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00,
  0x00, 0x24, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x24, 0x00, 0x00,
  0x00, 0x00, 0x14, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00,
  0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x28, 0x00, 0x00,
  0x00, 0x00, 0x14, 0x00, 0xf8, 0x28, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00,
  0x20, 0x28, 0x00, 0x00, 0x00, 0x00, 0xf4, 0xff, 0xff, 0x2f, 0x00, 0x00,
  0x00, 0x00, 0x34, 0x00, 0x00, 0x2c, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00,
  0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x28, 0x00, 0x00,
  0x00, 0x00, 0x94, 0xf8, 0x1f, 0x29, 0x00, 0x00, 0x00, 0x00, 0xd4, 0x01,
  0x80, 0x2b, 0x00, 0x00, 0x00, 0x00, 0x94, 0xf8, 0x1f, 0x29, 0x00, 0x00,
  0x00, 0x00, 0x14, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x14, 0xf8,
  0x1f, 0x28, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x28, 0x00, 0x00,
  0x00, 0x00, 0x14, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x34, 0xe0,
  0x07, 0x2c, 0x00, 0x00, 0x00, 0x00, 0xf8, 0xff, 0xff, 0x1f, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x70, 0x00,
  0x00, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x0e, 0x00, 0x00,
  0x00, 0x00, 0x70, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x00, 0xf0, 0xff, 0xff,
  0xff, 0xff, 0x0f, 0x00
};

static unsigned char PROGMEM XBMP_CamperSeite[] = {
  0xe0, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x30, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x80, 0x03, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06,
  0x10, 0xfe, 0x7f, 0x00, 0x00, 0xe0, 0x0f, 0x18, 0x10, 0x01, 0x80, 0x00,
  0x00, 0x10, 0x10, 0x30, 0x10, 0x01, 0x80, 0x00, 0x00, 0x10, 0x10, 0x20,
  0x10, 0xfe, 0x7f, 0xe0, 0x3f, 0xe0, 0x0f, 0x20, 0x10, 0x00, 0x00, 0x30,
  0x60, 0x00, 0x00, 0x30, 0x10, 0x00, 0x00, 0x10, 0x40, 0x00, 0x00, 0x18,
  0x10, 0x00, 0x00, 0x90, 0x4f, 0xfc, 0xff, 0x0f, 0x10, 0x00, 0x00, 0xd0,
  0x58, 0x0c, 0xc0, 0x00, 0x10, 0xfe, 0xff, 0x50, 0x50, 0xe4, 0x9f, 0x01,
  0x10, 0x03, 0x80, 0x51, 0x50, 0x24, 0x30, 0x03, 0x10, 0x01, 0x00, 0x51,
  0x50, 0x24, 0x60, 0x06, 0x10, 0x01, 0x00, 0x51, 0x50, 0x24, 0xc0, 0x0c,
  0x10, 0x01, 0x00, 0xd1, 0x58, 0x24, 0x80, 0x19, 0x10, 0x01, 0x00, 0x91,
  0x4f, 0x24, 0x00, 0x33, 0x10, 0x03, 0x80, 0x11, 0x40, 0x24, 0x00, 0x26,
  0x10, 0xfe, 0xff, 0x10, 0x40, 0xe4, 0xff, 0x27, 0x10, 0x00, 0x00, 0x10,
  0x40, 0x04, 0x00, 0x20, 0x10, 0x00, 0x00, 0xd0, 0x40, 0x64, 0x00, 0x20,
  0x30, 0x00, 0x00, 0x10, 0x40, 0x04, 0x00, 0x30, 0x30, 0x00, 0x00, 0x10,
  0x40, 0x04, 0x00, 0x30, 0x10, 0x00, 0x00, 0x10, 0x40, 0x04, 0x00, 0x20,
  0x10, 0x00, 0x00, 0x10, 0x40, 0x04, 0x00, 0x20, 0x10, 0xf0, 0x01, 0x10,
  0x40, 0x04, 0x3e, 0x20, 0x10, 0xf8, 0x03, 0x10, 0x40, 0x04, 0x7f, 0x20,
  0x10, 0x1c, 0x07, 0x10, 0x40, 0x84, 0xe3, 0x30, 0x30, 0x0c, 0x06, 0x10,
  0x40, 0x84, 0xc1, 0x18, 0xe0, 0x0f, 0xfe, 0xff, 0xff, 0xff, 0xc1, 0x0f,
  0x00, 0x0c, 0x06, 0x00, 0x00, 0x80, 0xc1, 0x00, 0x00, 0x1c, 0x07, 0x00,
  0x00, 0x80, 0xe3, 0x00, 0x00, 0xf8, 0x03, 0x00, 0x00, 0x00, 0x7f, 0x00,
  0x00, 0xf0, 0x01, 0x00, 0x00, 0x00, 0x3e, 0x00, 0xfc, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff
};

// MPU
MPU6050 MPU(Wire);
// OLED
#define OLED_Breite 128
#define OLED_Hoehe 64
U8G2_SSD1306_128X64_NONAME_F_HW_I2C OLED(U8G2_R0, U8X8_PIN_NONE);
// Joystick
#define JOY_V 32
#define JOY_H 33
#define JOY_SEL 19
// Servos
#define SERVOvert_Pin 26
Servo SERVO_V;
#define ServoHorz_Pin 25
Servo SERVO_H;
// LED
#define LED 18
// Timer
unsigned long PrevMillis_JOY_SEL = 0, Millis_JOY_SEL_press, Millis_JOY_SEL_release, PrevMillisServo, PrevMillis_OLED_Update_Cursor, PrevMillis_OLED_Update, Millis_Temp_Start;
const int Interval = 100, JOY_SEL_ShortPressInterval = 350, JOY_SEL_LongPressInterval = 1000, IntervalTemperature = 3000, IntervalServo = 15;

// Funktionen (= Programme) aktivieren = 1, deaktivieren = 0
#define SEARCHLIGHT_IS_ACTIVE 1
#define BALANCE_IS_ACTIVE 1

// HomeMenu - Reihenfolge im ENUM, ARRAY & SwitchCase im Loop müssen immer gleich sein!!!!!!
#define HomeMenuSize 4 // Anzahl Einträge im ENUM/ Array
const char *HomeMenu [HomeMenuSize] = {"Home", "Searchlight", "Balance", "Temperature"};
const byte OLED_Zeile [] = {0, 11, 22, 33, 44, 55}, MenuEinzug = 10;

// ProgrammEbenen
enum enumHomeMenu {Home, Searchlight, Balance, Temperature};
enum SearchlightMenu {SL_Initialisierung, SL_Nutzung, SL_Beenden};
enum SearchlightBrightness {SL_BrightnessSet = 1, SL_BrightnessChange};
enum BalanceMenu {BAL_Initialisierung, BAL_AuswahlModus, BAL_auto, BAL_man};
enum BalanceAuswahl {CBAauto, CBAman, CBAhome, enumBalAuswahl_Anzahl};
enum BalanceManuell {BALMAN_Initialisierung, BALMAN_Nutzung, BALMAN_Beenden};

// Flag für OLED-Inhalt, Joysticktaster & Joystick-Funktion
byte CursorMainProg = Home, CursorSearchlightProg = SL_Initialisierung, CursorBalanceProg = BAL_Initialisierung, CursorBalanceManuell = BALMAN_Initialisierung, CursorSearchlightBrightness = SL_BrightnessSet;
// MenüCursor
int CursorHome, CursorBalanceAuswahl;

// Variablen
byte JOY_SEL_curr, JOY_SEL_prev = HIGH;
int JOY_horz, JOY_vert, SERVO_horz = 0, SERVO_vert = 0, SERVOmin = 0, SERVOmax = 180, SL_brightness, SL_brightnessMin = 15, SL_brightnessMax = 255, SL_brightnessProz;
float temp, winkel_x = 99, winkel_y = 99;

void setup()   {
  Serial.begin(115200);
  Wire.begin();

  pinMode(JOY_V, INPUT);
  pinMode(JOY_H, INPUT);
  pinMode(JOY_SEL, INPUT_PULLUP);
  pinMode(LED, OUTPUT);
  digitalWrite(LED, LOW);

  OLED.begin();
  OLED.setFontMode(0);
  OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss
  OLED.setFont(u8g2_font_6x10_tf);

  // Begrüßung zentriert ausgeben
  OLED.setFontPosCenter();
  char Servas[12] = "Habe d'aar!";
  OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(Servas) / 2), OLED_Hoehe / 2);
  OLED.print(Servas);
  OLED.sendBuffer();
  delay(1000);
  OLED.clearDisplay();
  OLED.setFontPosTop();
  // MPU starten
  byte status = MPU.begin();
  debug("MPU6050 status: ");
  debugln(status);
  while (status != 0) { } // stop everything if could not connect to MPU6050
  // Menü anzeigen
  ShowMenu();
}

void UpdateTitelleiste() {
  // alten Titel mit schwarzer Box überschreiben
  OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
  OLED.drawBox(0, OLED_Zeile[0], OLED_Breite, OLED.getAscent() - OLED.getDescent());
  OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

  // neuen Titel schreiben
  OLED.setCursor(0, OLED_Zeile[0]);
  OLED.print(HomeMenu[CursorMainProg]);
  OLED.drawHLine(0, OLED.getAscent() - OLED.getDescent(), OLED_Breite);
  OLED.sendBuffer();
}

void ShowMenu() {
  // zurücksetzen
  CursorMainProg = Home;
  debugln("CursorMainProg zurückgesetzt");

  OLED.clearBuffer();
  // Titelleiste
  OLED.setCursor(0, OLED_Zeile[0]);
  OLED.print(HomeMenu[Home]);
  // Temp in Titelleiste
  temp = MPU.getTemp();
  char tempStr[10];
  snprintf(tempStr, sizeof(tempStr), "%.1f °C", temp);
  OLED.setCursor((OLED_Breite) - (OLED.getStrWidth(tempStr)), OLED_Zeile[0]);
  OLED.print(tempStr);
  temp = 99;
  // Linie
  OLED.drawHLine(0, OLED.getAscent() - OLED.getDescent(), OLED_Breite);
  // Menüpunkte
  CursorHome = 1;
  for (byte i = 1; i < HomeMenuSize; i ++) {
    if (i == 1) {
      OLED.setCursor(1, OLED_Zeile[i]);
      OLED.print(">");
    }
    OLED.setCursor(MenuEinzug, OLED_Zeile[i]);
    OLED.print(HomeMenu[i]);
  }

  OLED.sendBuffer();
  delay(100);
  debugln("Menü vollständig geladen");
}

void FunctionNotAvailable() {
  char textNV [14] = "Function n/a!";
  OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(textNV) / 2), OLED_Hoehe / 2);
  OLED.print(textNV);
  OLED.sendBuffer();
  delay(2000);
}

void loop() {
  MPU.update();

  //JOY_vert = map(analogRead(JOY_V), 0, 4095, -100, 100);
  //JOY_horz = map(analogRead(JOY_H), 0, 4095, -100, 100);

/*debug("JOY_vert: ");
debug(JOY_vert);
debug(", JOY_horz: ");
debugln(JOY_horz);
*/
  if (millis() - PrevMillis_JOY_SEL > Interval) {
    ////////// Druck auf Joystick-Taster abfragen
    // Entprellen
    JOY_SEL_curr = digitalRead(JOY_SEL);
    delay(10);
    JOY_SEL_curr = digitalRead(JOY_SEL);

    if (JOY_SEL_curr == LOW && JOY_SEL_prev == HIGH) {
      Millis_JOY_SEL_press = millis();
      debug(Millis_JOY_SEL_press);
      debugln("Taster gedrückt");
    } else if (JOY_SEL_curr == HIGH && JOY_SEL_prev == LOW) {
      Millis_JOY_SEL_release = millis();
      debug(Millis_JOY_SEL_release);
      debugln("Taster losgelassen");

      if ((Millis_JOY_SEL_release - Millis_JOY_SEL_press) < JOY_SEL_ShortPressInterval) {
        debugln("ShortPress erkannt.");
        switch (CursorMainProg) {
          case Home: {
              CursorMainProg = CursorHome;
              switch (CursorMainProg) {
                case Searchlight: {
                    CursorSearchlightProg = SL_Initialisierung;
                    SL_brightness = SL_brightnessMax;
                  }
                  break;
                case Balance: {
                    CursorBalanceProg = BAL_Initialisierung;
                    CursorBalanceAuswahl = CBAauto;
                  }
                  break;
                case Temperature: {
                    Millis_Temp_Start = millis();
                  }
                  break;
              }
              OLED.clearBuffer();
              UpdateTitelleiste();
            }
            break;
          case Searchlight: {
              if (CursorSearchlightProg == SL_Nutzung && CursorSearchlightBrightness == SL_BrightnessSet) {
                CursorSearchlightBrightness = SL_BrightnessChange;
              } else if (CursorSearchlightProg == SL_Nutzung && CursorSearchlightBrightness == SL_BrightnessChange)
                CursorSearchlightBrightness = SL_BrightnessSet;
            }
            break;
          case Balance: {
              switch (CursorBalanceAuswahl) {
                case CBAauto: {
                    CursorBalanceProg = BAL_auto;
                  }
                  break;
                case CBAman: {
                    CursorBalanceProg = BAL_man;
                    CursorBalanceManuell = BALMAN_Initialisierung;
                  }
                  break;
                case CBAhome: {}
                  ShowMenu();
                  break;
              } // end switch(CursorBalanceAuswahl)
            }
            break;
          case Temperature: {
              // nix machen
            }
            break;
        } // end switch(CursorMainProg)
      } // end JOY_SEL_ShortPressInterval
      else if ((Millis_JOY_SEL_release - Millis_JOY_SEL_press) > JOY_SEL_LongPressInterval) {
        debugln("LongPress erkannt.");
        switch (CursorMainProg) {
          case Home: {
              // nix machen
            }
            break;
          case Searchlight: {
              if (CursorSearchlightProg == SL_Nutzung) {
                CursorSearchlightProg = SL_Beenden;
              }
            }
            break;
          case Balance: {
              if (CursorBalanceProg == BAL_man && CursorBalanceManuell == BALMAN_Nutzung) {
                CursorBalanceManuell = BALMAN_Beenden;
              }
            }
            break;
          case Temperature: {
              // nix machen
            }
            break;
        } // end switch(CursorMainProg)
      }
    } // end if JOY_SEL_LongPressInterval
    JOY_SEL_prev = JOY_SEL_curr;

    // Timer aktualisieren
    PrevMillis_JOY_SEL = millis();
  }


  ////////// jeweiliges Progamm durchlaufen & OLED aktualisieren
  switch (CursorMainProg) {
    case Home: {// Joystick-Bewegung abfragen
        JOY_vert = map(analogRead(JOY_V), 0, 4095, -100, 100);
        if (JOY_vert != 0) {
          // alten Cursor mit schwarzer Box überschreiben
          OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
          OLED.drawBox(0, OLED_Zeile[CursorHome], MenuEinzug - 1, OLED_Hoehe - OLED_Zeile[CursorHome]);
          OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

          // neue CursorPosition ermitteln
          if (JOY_vert > 0) { // Joystick nach oben
            CursorHome --;
            if (CursorHome < 1) {
              CursorHome = HomeMenuSize - 1;
            }
            debug("CursorHome: "); debugln(CursorHome);
          }
          if (JOY_vert < 0) { // Joystick nach unten
            CursorHome ++;
            if (CursorHome > HomeMenuSize - 1) {
              CursorHome = 1;
            }
            debug("CursorHome: "); debugln(CursorHome);
          }
          // Cursor an neue Position schreiben
          OLED.setCursor(0, OLED_Zeile[CursorHome]);
          OLED.print(">");
          OLED.sendBuffer();

          // Timer aktualisieren
          PrevMillis_OLED_Update_Cursor = millis();
        }
      }
      break;
    case Searchlight: {
        if (SEARCHLIGHT_IS_ACTIVE == 0) {
          FunctionNotAvailable();
        } else {
          switch (CursorSearchlightProg) {
            case SL_Initialisierung: {
                debug("CursorSearchlightProg ");
                debugln(CursorSearchlightProg);

                // hier jetzt Suchlicht ausfahren rein
                SERVO_vert = SERVOmax / 2;
                SERVO_horz = SERVOmax / 2;

                OLED.setCursor(0, OLED_Zeile[1]);
                OLED.print("Start searchlight...");
                OLED.sendBuffer();

                SERVO_V.attach(SERVOvert_Pin, 500, 2500);
                SERVO_V.write(SERVO_vert);
                SERVO_H.attach(ServoHorz_Pin, 500, 2500);
                SERVO_H.write(SERVO_horz);
                delay(250);

                // Licht anschalten
                digitalWrite(LED, SL_brightness);

                OLED.setCursor(0, OLED_Zeile[2]);
                OLED.print("Searchlight ready!");
                OLED.sendBuffer();
                delay(250);

                OLED.clearDisplay();

                // hier jetzt irgendein XBMP rein oder so

                // Searchlight-Dimmstufe
                SL_brightnessProz = map(SL_brightness, 0, 255, 0, 100);
                char brightnessStr[17];
                snprintf(brightnessStr, sizeof(brightnessStr), "Brightness: %d%%", SL_brightnessProz);
                OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(brightnessStr) / 2), OLED_Zeile[4]);
                OLED.print(brightnessStr);

                // Text für Ende SL_Nutzung noch hinschreiben, dass der dann nicht dauernd aktualisiert wird
                char servoStr [21] = "LongPress for >home<";
                OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(servoStr) / 2), OLED_Zeile[5]);
                OLED.print(servoStr);
                OLED.sendBuffer();

                CursorSearchlightProg = SL_Nutzung;
                debug("CursorSearchlightProg ");
                debugln(CursorSearchlightProg);
              }
              break;
            case SL_Nutzung: {
                switch (CursorSearchlightBrightness) {
                  case SL_BrightnessSet: {
                      JOY_vert = map(analogRead(JOY_V), 0, 4095, -100, 100);
                      JOY_horz = map(analogRead(JOY_H), 0, 4095, -100, 100);
                      if ((JOY_vert != 0 || JOY_horz != 0) && millis() - PrevMillisServo >= IntervalServo) {
                        ////////////////////////// SERVO_V rumfahren
                        if (JOY_vert < 0) {
                          SERVO_vert --;
                          if (SERVO_vert < SERVOmin) {
                            SERVO_vert = SERVOmin;
                          }
                        } else if (JOY_vert > 0) {
                          SERVO_vert ++;
                          if (SERVO_vert > SERVOmax) {
                            SERVO_vert = SERVOmax;
                          }
                        }
                        SERVO_V.write(SERVO_vert);

                        ////////////////////////// SERVO_H rumfahren
                        if (JOY_horz < 0) {
                          SERVO_horz ++;
                          if (SERVO_horz > SERVOmax) {
                            SERVO_horz = SERVOmax;
                          }
                        } else if (JOY_horz > 0) {
                          SERVO_horz --;
                          if (SERVO_horz < SERVOmin) {
                            SERVO_horz = SERVOmin;
                          }
                        }
                        SERVO_H.write(SERVO_horz);

                        // Timer aktualisieren
                        PrevMillisServo = millis();
                      }

                      if (millis() - PrevMillis_OLED_Update > Interval) {
                        // alten Wert mit schwarzer Box übermalen
                        OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                        OLED.drawBox(0, OLED_Zeile[4], OLED_Breite, OLED_Zeile[5] - OLED_Zeile[4]);
                        OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss
                        // Licht anschalten
                        digitalWrite(LED, SL_brightness);
                        // neuen Wert ausgeben
                        char brightnessStr[17];
                        snprintf(brightnessStr, sizeof(brightnessStr), "Brightness: %d%%", SL_brightnessProz);
                        OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(brightnessStr) / 2), OLED_Zeile[4]);
                        OLED.print(brightnessStr);
                        OLED.sendBuffer();

                        // Timer aktualisieren
                        PrevMillis_OLED_Update = millis();
                      }
                    } break;
                  case SL_BrightnessChange: {
                      if ((JOY_vert != 0 || JOY_horz != 0) && millis() - PrevMillisServo >= IntervalServo) {
                        if (JOY_vert > 0) {
                          SL_brightness ++;
                          if (SL_brightness > SL_brightnessMax) {
                            SL_brightness = SL_brightnessMin;
                          }
                        } else if (JOY_vert < 0) {
                          SL_brightness --;
                          if (SL_brightness < SL_brightnessMin) {
                            SL_brightness = SL_brightnessMax;
                          }
                        }
                      }

                      if (millis() - PrevMillis_OLED_Update > Interval) {
                        // alten Wert mit weisser Box übermalen
                        OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss
                        OLED.drawBox(0, OLED_Zeile[4], OLED_Breite, OLED_Zeile[5] - OLED_Zeile[4]);

                        // neuen Wert ermitteln
                        SL_brightnessProz = map(SL_brightness, 0, 255, 0, 100);
                        char brightnessStr[17];
                        snprintf(brightnessStr, sizeof(brightnessStr), "Brightness: %d%%", SL_brightnessProz);
                        OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(brightnessStr) / 2), OLED_Zeile[4]);
                        // Schriftfarbe umdrehen & Wert ausgeben
                        OLED.setDrawColor(0);
                        OLED.print(brightnessStr);
                        OLED.sendBuffer();
                        // Schriftfarbe zurücksetzen
                        OLED.setDrawColor(1);
                        // Licht dimmen
                        digitalWrite(LED, SL_brightness);
                        debug("SL Brightness: ");
                        debugln(SL_brightness);

                        // Timer aktualisieren
                        PrevMillis_OLED_Update = millis();
                      }
                    }
                    break;
                }
              }
              break;
            case SL_Beenden: {
                debug("CursorSearchlightProg ");
                debugln(CursorSearchlightProg);
                // Licht ausschalten & einfahren
                digitalWrite(LED, LOW);
                SERVO_V.attach(SERVOvert_Pin, 500, 2500);
                SERVO_V.write(SERVOmin);
                SERVO_H.attach(ServoHorz_Pin, 500, 2500);
                SERVO_H.write(SERVOmin);

                OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                OLED.drawBox(0, OLED_Zeile[5], OLED_Breite, OLED_Hoehe - OLED_Zeile[5]);
                OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

                OLED.setCursor(0, OLED_Zeile[5]);
                OLED.print("Searchlight off!");
                OLED.sendBuffer();
                delay(1000);

                ShowMenu();
              }
              break;
          }
        }
      }
      break;
    case Balance: {
        if (BALANCE_IS_ACTIVE == 0) {
          FunctionNotAvailable();
        } else {
          switch (CursorBalanceProg) {
            case BAL_Initialisierung: {
                OLED.setCursor(0, OLED_Zeile[1]);
                OLED.print("Calibrating...");
                OLED.setCursor(1,  OLED_Zeile[2]);
                OLED.print("Do not move MPU!");
                OLED.sendBuffer();
                //////////////////////////////////////////////////// wozu dieses Delay???
                //delay(1000);
                MPU.calcOffsets(true, true); // gyro and accelero
                OLED.setCursor(0,  OLED_Zeile[3]);
                OLED.print("Offset calculated...");
                OLED.sendBuffer();
                OLED.setCursor(0,  OLED_Zeile[4]);
                OLED.print("MPU ready!");
                OLED.sendBuffer();
                delay(250);
                OLED.clearDisplay();
                // XBMPs Camper Front & Seite auf OLED ausgeben
                OLED.drawXBMP(0, 0, XBMP_Camper_Breite, XBMP_Camper_Hoehe, XBMP_CamperFront);
                OLED.drawXBMP(XBMP_Camper_Breite, 0, XBMP_Camper_Breite, XBMP_Camper_Hoehe, XBMP_CamperSeite);

                CursorBalanceProg = BAL_AuswahlModus;
                CursorBalanceAuswahl = CBAauto;
              }
              break;
            case BAL_AuswahlModus: {
                if (millis() - PrevMillis_OLED_Update > Interval) {
                  // alten Winkelx & Pfeil mit schwarzer Box übermalen
                  OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                  OLED.drawBox(0, OLED_Breite + 1, XBMP_Camper_Breite, OLED_Zeile[5] - XBMP_Camper_Hoehe);
                  OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss
                  // neue Winkel ermitteln & ausgeben
                  winkel_x = MPU.getAngleX(); // Roll
                  char winkelxStr[12];
                  snprintf(winkelxStr, sizeof(winkelxStr), "R: %.2f°", winkel_x);
                  OLED.setCursor((OLED_Breite / 4) - (OLED.getStrWidth(winkelxStr) / 2), OLED_Zeile[4]);
                  OLED.print(winkelxStr);
                  winkel_y = MPU.getAngleY(); // Pitch
                  char winkelyStr[12];
                  snprintf(winkelyStr, sizeof(winkelyStr), "P: %.2f°", winkel_y);
                  OLED.setCursor(3 * (OLED_Breite / 4) - (OLED.getStrWidth(winkelyStr) / 2), OLED_Zeile[4]);
                  OLED.print(winkelyStr);

                  // Pfeile ausgeben
                  char BALpfeil [2] = "^";
                  int Versatz_x = (int)winkel_x;
                  if (Versatz_x > 5) {
                    Versatz_x = 5;
                  } else if (Versatz_x < -5) {
                    Versatz_x = -5;
                  }
                  OLED.setCursor((OLED_Breite / 4) - (OLED.getStrWidth(BALpfeil) / 2) + Versatz_x * 2, XBMP_Camper_Hoehe + 1);
                  OLED.print(BALpfeil);
                  int Versatz_y = (int)winkel_y;
                  if (Versatz_y > 5) {
                    Versatz_y = 5;
                  } else if (Versatz_y < -5) {
                    Versatz_y = -5;
                  }
                  OLED.setCursor(3 * (OLED_Breite / 4) - (OLED.getStrWidth(BALpfeil) / 2) + Versatz_y * 4, XBMP_Camper_Hoehe + 1);
                  OLED.print(BALpfeil);

                  OLED.sendBuffer();
                  // Timer aktualisieren
                  PrevMillis_OLED_Update = millis();
                }

                // JOY_horz = map(analogRead(JOY_H), 0, 4095, -100, 100);
                if (JOY_horz != 0 && millis() - PrevMillis_OLED_Update_Cursor > Interval) {
                  if (JOY_horz < 0) {
                    CursorBalanceAuswahl ++;
                    if (CursorBalanceAuswahl == enumBalAuswahl_Anzahl) {
                      CursorBalanceAuswahl = 0;
                    }
                    debug("CursorBalanceAuswahl: ");
                    debugln(CursorBalanceAuswahl);
                  }
                  if (JOY_horz > 0) {
                    CursorBalanceAuswahl --;
                    if (CursorBalanceAuswahl < 0) {
                      CursorBalanceAuswahl = enumBalAuswahl_Anzahl - 1;
                    }
                    debug("CursorBalanceAuswahl: ");
                    debugln(CursorBalanceAuswahl);
                  }
                  // Timer aktualisieren
                  PrevMillis_OLED_Update_Cursor = millis();
                }
                // alten Text schwarz übermalen
                OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                OLED.drawBox(0, OLED_Zeile[5], OLED_Breite, OLED_Hoehe - OLED_Zeile[5]);
                OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

                char BALmode [18] = ">Auto< Man  Home ";
                OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(BALmode) / 2), OLED_Zeile[5]);
                switch (CursorBalanceAuswahl) {
                  case CBAauto: {
                      OLED.print(">Auto< Man  Home ");
                    }
                    break;
                  case CBAman:
                    {
                      OLED.print(" Auto >Man< Home ");
                    }
                    break;
                  case CBAhome: {
                      OLED.print(" Auto  Man >Home<");
                    }
                    break;
                }
                OLED.sendBuffer();
              }
              break;
            case BAL_auto: {
                debugln("AutomatischesBalanceProgramm");
                ShowMenu();
              }
              break;
            case BAL_man: {
                switch (CursorBalanceManuell) {
                  case BALMAN_Initialisierung: {
                      // Balance-Menü (auto - man - home) schwarz übermalen
                      OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                      OLED.drawBox(0, OLED_Zeile[5], OLED_Breite, OLED_Hoehe - OLED_Zeile[5]);
                      OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

                      OLED.setCursor(0, OLED_Zeile[5]);
                      OLED.print("Start servos...");
                      OLED.sendBuffer();

                      SERVO_V.attach(SERVOvert_Pin, 500, 2500);
                      SERVO_V.write(SERVO_vert);
                      SERVO_H.attach(ServoHorz_Pin, 500, 2500);
                      SERVO_H.write(SERVO_horz);
                      delay(250);

                      OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                      OLED.drawBox(0, OLED_Zeile[5], OLED_Breite, OLED_Hoehe - OLED_Zeile[5]);
                      OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss
                      OLED.setCursor(0, OLED_Zeile[5]);
                      OLED.print("Servos ready!");
                      OLED.sendBuffer();
                      delay(250);

                      OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                      OLED.drawBox(0, OLED_Zeile[5], OLED_Breite, OLED_Hoehe - OLED_Zeile[5]);
                      OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

                      // Text für Ende BALMAN_Nutzung noch hinschreiben, dass der dann nicht dauernd aktualisiert wird
                      char servoStr3 [21] = "LongPress for >home<";
                      OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(servoStr3) / 2), OLED_Zeile[5]);
                      OLED.print(servoStr3);
                      OLED.sendBuffer();

                      CursorBalanceManuell = BALMAN_Nutzung;
                    }
                    break;
                  case BALMAN_Nutzung: {
                      if (millis() - PrevMillis_OLED_Update > Interval) {
                        // alte Winkel mit schwarzer Box übermalen
                        OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                        OLED.drawBox(0, XBMP_Camper_Hoehe + 1, OLED_Breite, OLED_Zeile[5] - XBMP_Camper_Hoehe);
                        OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

                        // Winkel ermitteln & ausgeben
                        winkel_x = MPU.getAngleX(); // Roll
                        char winkelxStr[12];
                        snprintf(winkelxStr, sizeof(winkelxStr), "R: %.2f°", winkel_x);

                        OLED.setCursor((OLED_Breite / 4) - (OLED.getStrWidth(winkelxStr) / 2), OLED_Zeile[4]);
                        OLED.print(winkelxStr);
                        winkel_y = MPU.getAngleY(); // Pitch
                        char winkelyStr[12];
                        snprintf(winkelyStr, sizeof(winkelyStr), "P: %.2f°", winkel_y);
                        OLED.setCursor(3 * (OLED_Breite / 4) - (OLED.getStrWidth(winkelyStr) / 2), OLED_Zeile[4]);
                        OLED.print(winkelyStr);

                        // Pfeile ausgeben
                        char BALpfeil [2] = "^";
                        int Versatz_x = (int)winkel_x;
                        if (Versatz_x > 10) {
                          Versatz_x = 10;
                        } else if (Versatz_x < -10) {
                          Versatz_x = -10;
                        }
                        OLED.setCursor((OLED_Breite / 4) - (OLED.getStrWidth(BALpfeil) / 2) + Versatz_x, XBMP_Camper_Hoehe + 1);
                        OLED.print(BALpfeil);
                        int Versatz_y = (int)winkel_y;
                        if (Versatz_y > 10) {
                          Versatz_y = 10;
                        } else if (Versatz_y < -10) {
                          Versatz_y = -10;
                        }
                        OLED.setCursor(3 * (OLED_Breite / 4) - (OLED.getStrWidth(BALpfeil) / 2) + Versatz_y * 2, XBMP_Camper_Hoehe + 1);
                        OLED.print(BALpfeil);

                        OLED.sendBuffer();
                        // Timer aktualisieren
                        PrevMillis_OLED_Update = millis();
                      }
                      
                      JOY_vert = map(analogRead(JOY_V), 0, 4095, -100, 100);
                      JOY_vert = map(analogRead(JOY_V), 0, 4095, -100, 100);
                      if (millis() - PrevMillisServo >= IntervalServo) {
                        ////////////////////////// SERVO_V rumfahren
                        if (JOY_vert < 0) {
                          SERVO_vert --;
                          if (SERVO_vert < SERVOmin) {
                            SERVO_vert = SERVOmin;
                          }
                        } else if (JOY_vert > 0) {
                          SERVO_vert ++;
                          if (SERVO_vert > SERVOmax) {
                            SERVO_vert = SERVOmax;
                          }
                        }
                        SERVO_V.write(SERVO_vert);

                        ////////////////////////// SERVO_H rumfahren
                        if (JOY_horz < 0) {
                          SERVO_horz ++;
                          if (SERVO_horz > SERVOmax) {
                            SERVO_horz = SERVOmax;
                          }
                        } else if (JOY_horz > 0) {
                          SERVO_horz --;
                          if (SERVO_horz < SERVOmin) {
                            SERVO_horz = SERVOmin;
                          }
                        }
                        SERVO_H.write(SERVO_horz);

                        PrevMillisServo = millis();
                      }
                    }
                    break;
                  case BALMAN_Beenden:
                    {
                      // Servos bzw. Relais ausschalten
                      OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
                      OLED.drawBox(0, OLED_Zeile[5], OLED_Breite, OLED_Hoehe - OLED_Zeile[5]);
                      OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

                      OLED.setCursor(0, OLED_Zeile[5]);
                      OLED.print("Servos deactivated!");
                      OLED.sendBuffer();
                      delay(1000);

                      ShowMenu();
                    }
                    break;
                }
                break;
              }
              break;
          }
        }
      }
      break;
    case Temperature: { // hier den Code für Temperature anzeigen rein
        if (millis() - Millis_Temp_Start < IntervalTemperature) {
          if (temp != MPU.getTemp()) {
            temp = MPU.getTemp();
            char tempStr[20];
            snprintf(tempStr, sizeof(tempStr), "Es hat %.2f °C", temp);
            OLED.setCursor((OLED_Breite / 2) - (OLED.getStrWidth(tempStr) / 2), OLED_Hoehe / 2);
            OLED.print(tempStr);
            OLED.sendBuffer();
          }
        } else {
          debugln("TempEnde");
          ShowMenu();
        }
      }
      break;
  } // end switch (CursorMainProg)
} // end loop