#include "Wire.h"
#include <MPU6050_light.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

// MPU
MPU6050 MPU(Wire);

// OLED
U8G2_SSD1306_128X64_NONAME_F_HW_I2C OLED(U8G2_R0, U8X8_PIN_NONE);
#define OLED_Breite 128
#define OLED_Hoehe 64

// XBM-Editor: https://xbm.jazzychad.net/
// Bitmap-Generator: https://marlinfw.org/tools/u8glib/converter.html

// 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
};

// Bitmaps Camper Zustandspfeile
// Z1 = BFS/ hinterer Reifen oben, FS/ vorderer unten
// Z2 = BFS/ hinterer Reifen unten, FS/ vorderer oben
// Z3 = im Wasser
#define XBMP_CamperZustand_Breite  64
#define XBMP_CamperZustand_Hoehe 11
static unsigned char PROGMEM XBMP_CamperFront_Z1[] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00,
  0x80, 0x20, 0x00, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x11, 0x00, 0x00,
  0x00, 0x00, 0x88, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x04, 0x01,
  0x00, 0x04, 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, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00
};

// Pfeile
/*
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x04, 0x00, 0x00,
  0x00, 0x00, 0x70, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x00,
  0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x24, 0x01, 0x00, 0x04, 0x00, 0x00,
  0x00, 0x00, 0x20, 0x00, 0x80, 0x24, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00,
  0x00, 0x15, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x0e, 0x00, 0x00,
  0x00, 0x00, 0x20, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00
  };
*/
static unsigned char PROGMEM XBMP_CamperSeite_Z1 [] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00,
  0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x50, 0x01, 0x00,
  0x00, 0x00, 0x08, 0x00, 0x00, 0x48, 0x02, 0x00, 0x00, 0x00, 0x08, 0x00,
  0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x49, 0x00, 0x00, 0x40, 0x00, 0x00,
  0x00, 0x00, 0x2a, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x00,
  0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00
};
static unsigned char PROGMEM XBMP_CamperFront_Z2[] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x04, 0x00, 0x00,
  0x00, 0x00, 0x20, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00,
  0x00, 0x15, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x80, 0x24, 0x00, 0x00,
  0x00, 0x00, 0x24, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x00,
  0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x04, 0x00, 0x00,
  0x00, 0x00, 0x20, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00
};
static unsigned char PROGMEM XBMP_CamperSeite_Z2 [] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00,
  0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x40, 0x00, 0x00,
  0x00, 0x00, 0x2a, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x49, 0x00,
  0x00, 0x48, 0x02, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x50, 0x01, 0x00,
  0x00, 0x00, 0x08, 0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00,
  0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00
};
static unsigned char PROGMEM XBMP_Camper_Z3 [] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xc0, 0xc7, 0xf1, 0x08, 0x04, 0x00,
  0x00, 0x00, 0x01, 0x21, 0x12, 0x09, 0x02, 0x00, 0x00, 0x00, 0x02, 0x11,
  0x14, 0x09, 0x01, 0x00, 0x00, 0xf8, 0x07, 0x11, 0xf4, 0x88, 0x7f, 0x00,
  0x00, 0x00, 0x02, 0x11, 0x14, 0x08, 0x01, 0x00, 0x00, 0x00, 0x01, 0x21,
  0x12, 0x00, 0x02, 0x00, 0x00, 0x80, 0x00, 0xc1, 0x11, 0x08, 0x04, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00
};

// Timer
unsigned long CurrMillis, PrevMillis = 0;
int Interval = 1000;

// Variablen
const byte Zeile [] = {48, 56, 64}, Zeilenhoehe = 8; // OLED-Textbereich ab Y = 46
float Winkel_x, Winkel_x_alt = 99, Winkel_y, Winkel_y_alt = 99;

void UpdateOLED_Text(String text, byte zeile) {
  // alten Text mit schwarzer Box übermalen
  OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
  OLED.drawBox(0, Zeile[zeile] - Zeilenhoehe, OLED_Breite, Zeilenhoehe);
  OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

  // Schriftart festlegen
  OLED.setFont(u8g2_font_6x10_tr);
  // Text zentrieren & ausgeben
  byte Textlaenge = OLED.getStrWidth(text.c_str());
  byte Texteinzug = (OLED_Breite / 2) - (Textlaenge / 2);
  OLED.setCursor(Texteinzug, Zeile[zeile]);
  OLED.print(text);
  OLED.sendBuffer();
  // OLED.updateDisplayArea(0, 6, 16, 3); // in tiles (= 8x8 px): tile_area_x_pos, tile_area_y_pos, tile_area_width, tile_area_height
}

void UpdateOLED_Wert(float winkel_x, float winkel_y) {
  // alte Werte mit schwarzer Box übermalen
  OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
  OLED.drawBox(0, OLED_Hoehe - Zeilenhoehe * 2, OLED_Breite, Zeilenhoehe * 2); // doppelte Zeilenhöhe bei großer Schrift
  OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

  // Schriftart festlegen
  OLED.setFont(u8g2_font_10x20_tr);
  // neue Werte ausgeben
  OLED.setCursor(0, OLED_Hoehe);
  OLED.print(winkel_y);
  OLED.setCursor(OLED_Breite / 2, OLED_Hoehe);
  OLED.print(winkel_x);
  OLED.sendBuffer();
  // OLED.updateDisplayArea(0, 6, 16, 3); // in tiles (= 8x8 px): tile_area_x_pos, tile_area_y_pos, tile_area_width, tile_area_height
}

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

  // OLED starten
  OLED.begin();

  // Backup für updateDisplayArea
  debug("TileWidth: ");
  debugln(OLED.getBufferTileWidth());
  debug("TileHeight: ");
  debugln(OLED.getBufferTileHeight());

  // XBMPs Camper Front & Seite auf OLED ausgeben
  // OLED.drawXBMP(0, 0, XBMP_SprinterSeite_Breite, XBMP_SprinterSeite_Hoehe, XBMP_SprinterSeite);
  // OLED.drawXBMP(0, 0, XBMP_T3Seite_Breite, XBMP_T3Seite_Hoehe, XBMP_T3Seite);
  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);
  OLED.sendBuffer();
  // Habe die Ehre
  UpdateOLED_Text("Habe die Ehre!", 0);
  delay(250);

  // MPU starten
  UpdateOLED_Text("Starte MPU6050", 2);

  byte status = MPU.begin();
  debug("MPU6050 Status: ");
  debugln(status);
  while (status != 0) {} // stop everything if could not connect to MPU6050

  // MPU kalibrieren
  UpdateOLED_Text("Kalibriere Offset", 2);
  MPU.calcOffsets(true, true); // gyro and accelero
  UpdateOLED_Text("Offset kalibriert", 2);

  // MPU bereit
  UpdateOLED_Text("MPU6050 bereit", 2);
  delay(250);

  // OLED Textbereich leeren
  OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
  OLED.drawBox(0, OLED_Hoehe - Zeilenhoehe * sizeof(Zeile), OLED_Breite, OLED_Hoehe - XBMP_Camper_Hoehe);
  OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss
  OLED.sendBuffer();
  delay(250);
}

void loop() {
  MPU.update();

  if (millis() - PrevMillis > Interval) {
    Winkel_x = MPU.getAngleX();
    Winkel_y = MPU.getAngleY();
    UpdateOLED_Wert(Winkel_x, Winkel_y);

    PrevMillis = millis();
  }

  // XBMPs Camper Zustand auf OLED ausgeben
  // Zustand 1
  OLED.drawXBMP(0, XBMP_Camper_Hoehe, XBMP_CamperZustand_Breite, XBMP_CamperZustand_Hoehe, XBMP_CamperFront_Z1);
  OLED.drawXBMP(XBMP_Camper_Breite, XBMP_Camper_Hoehe, XBMP_CamperZustand_Breite, XBMP_CamperZustand_Hoehe, XBMP_CamperSeite_Z1);
  OLED.sendBuffer();
  delay(1000);

  // Zustand 2
  OLED.drawXBMP(0, XBMP_Camper_Hoehe, XBMP_CamperZustand_Breite, XBMP_CamperZustand_Hoehe, XBMP_CamperFront_Z2);
  OLED.drawXBMP(XBMP_Camper_Breite, XBMP_Camper_Hoehe, XBMP_CamperZustand_Breite, XBMP_CamperZustand_Hoehe, XBMP_CamperSeite_Z2);
  OLED.sendBuffer();
  delay(1000);

  // Zustand 3
  OLED.drawXBMP(0, XBMP_Camper_Hoehe, XBMP_CamperZustand_Breite, XBMP_CamperZustand_Hoehe, XBMP_Camper_Z3);
  OLED.drawXBMP(XBMP_Camper_Breite, XBMP_Camper_Hoehe, XBMP_CamperZustand_Breite, XBMP_CamperZustand_Hoehe, XBMP_Camper_Z3);
  OLED.sendBuffer();
  delay(1000);



  /*
    MPU.update();

    Winkel_x = MPU.getAngleX();
    Winkel_y = MPU.getAngleY();

    if (Winkel_x != Winkel_x_alt || Winkel_y != Winkel_y_alt) {
    debugln("Winkel geändert.");

    if (Winkel_x < 0) {
      ZustandSeite = 1;
    } else if (Winkel_x > 0) {
      ZustandSeite = 2;
    } else {
      ZustandSeite = 3;
    }

    // schwarze Box über Text malen
    OLED.setDrawColor(0); // 0 = schwarz, 1 = weiss
    OLED.drawBox(0, OLED_Zeilenhoehe * 5, OLED_Zeilenhoehe + 2, OLED_Breite);
    OLED.sendBuffer();
    OLED.setDrawColor(1); // 0 = schwarz, 1 = weiss

    // neuen Text schreiben
    switch (ZustandSeite) {
      case 1 ... 2:
        OLED.setCursor(0, OLED_Zeilenhoehe * 6);
        OLED.print("WoMo steht schief :(");
        break;
      case 3:
        OLED.setCursor(0, OLED_Zeilenhoehe * 6);
        OLED.print("WoMo steht gerade :)");
        break;
    }

    OLED.sendBuffer();

    // Winkel speichern
    Winkel_x_alt = Winkel_x;
    Winkel_y_alt = Winkel_y;

    }

    if (millis() - PrevMillis > Interval) {
    debug(F("TEMPERATURE: ")); debugln(MPU.getTemp());

    debugln(F("Winkel"));
    debug("X: "); debug(MPU.getAngleX());
    debug("\tY: "); debugln(MPU.getAngleY());

    PrevMillis = millis();
    }
  */
}