#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_SSD1306.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

// MPU6050
Adafruit_MPU6050 MPU; // Sensor initialisieren
sensors_event_t a, g, temp; // Event-Variablen
// X-Achse Bildschirm-Waagrechte, - = oberer Rand kippt Richtung Wand, + = unterer Rand kippt Richtung Wand
// Y-Achse Bildschirm-Senkrechte, - = linker Rand kippt Richtung Wand, + = rechter Rand kippt Richtung Wand
// Z-AChse Bildschirm ins Gesicht!!!11!!1!!elf

// LEDs
#define LED_ObenLinks 17
#define LED_ObenRechts 19
#define LED_UntenLinks 5
#define LED_UntenRechts 18
byte Brightness = 255;

// OLED
#define OLED_Breite 128
#define OLED_Hoehe 64
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 OLED(OLED_Breite, OLED_Hoehe, &Wire, OLED_RESET);

// Timer
unsigned long CurrMillis, PrevMillis_x = 0, PrevMillis_y = 0;

// Variablen
float G = 9.81, Gmax;
float acc_x_prev = 0, acc_y_prev = 0;
int Linie_x_prev, Linie_y_prev;

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

  pinMode(LED_ObenLinks, OUTPUT);
  pinMode(LED_ObenRechts, OUTPUT);
  pinMode(LED_UntenLinks, OUTPUT);
  pinMode(LED_UntenRechts, OUTPUT);

  // LEDs einschalten
  digitalWrite(LED_ObenLinks, HIGH);
  digitalWrite(LED_ObenRechts, HIGH);
  digitalWrite(LED_UntenLinks, HIGH);
  digitalWrite(LED_UntenRechts, HIGH);

  // MPU6050 initialisieren
  while (!MPU.begin()) {
    debugln("MPU6050 not connected!");
  }
  debugln("MPU6050 connected!");

  // Beschleunigungssensor: Auflösung einstellen
  MPU.setAccelerometerRange(MPU6050_RANGE_2_G);
  debug("Accelerometer range set to: ");
  switch (MPU.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Gmax = G * 2;
      debug("+-2G, Gmax = ");
      debugln(Gmax);
      break;
    case MPU6050_RANGE_4_G:
      Gmax = G * 4;
      debug("+-4G, Gmax = ");
      debugln(Gmax);
      break;
    case MPU6050_RANGE_8_G:
      Gmax = G * 8;
      debug("+-8G, Gmax = ");
      debugln(Gmax);
      break;
    case MPU6050_RANGE_16_G:
      Gmax = G * 16;
      debug("+-16G, Gmax = ");
      debugln(Gmax);
      break;
  }

  // Lagesensor: Auflösung einstellen
  MPU.setGyroRange(MPU6050_RANGE_250_DEG);
  debug("Gyro range set to: ");
  switch (MPU.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      debugln("+- 250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      debugln("+- 500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      debugln("+- 1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      debugln("+- 2000 deg/s");
      break;
  }

  // Filter einstellen
  MPU.setFilterBandwidth(MPU6050_BAND_44_HZ);
  debug("Filter bandwidth set to: ");
  switch (MPU.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      debugln("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      debugln("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      debugln("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      debugln("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      debugln("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      debugln("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      debugln("5 Hz");
      break;
  }

  // MPU6050 bereit
  debugln("MPU6050 ready!");
  delay(100);

  //// OLED initialisieren
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    debugln("SSD1306 allocation failed");
    for (;;); // Don't proceed, loop forever
  }
  OLED.clearDisplay();
  OLED.setRotation(1); // Portrait-Modus, x = kurze Seite, y = lange Seite mit Pins
  OLED.setTextSize(1);

  // Mittelkreis
  OLED.drawCircle((OLED.width() / 2) - 1, (OLED.height() / 2) - 1, 2, SSD1306_WHITE); // Mittelpunkt-x, Mittelpunkt-y, Radius, Farbe
  // X-Linie
  Linie_x_prev = map(-acc_x_prev, -Gmax, Gmax, 0, OLED_Breite - 1); // a.acceleration.x invertieren
  OLED.drawFastHLine(0, Linie_x_prev, OLED_Hoehe, SSD1306_WHITE); // x0, y0, Länge, Farbe
  // Y-Linie
  Linie_y_prev = map(acc_y_prev, -Gmax, Gmax, 0, OLED_Hoehe - 1);
  OLED.drawFastVLine(Linie_y_prev, 0, OLED_Breite, SSD1306_WHITE); // x0, y0, Länge, Farbe
  OLED.display();

  // LEDs ausschalten
  digitalWrite(LED_ObenLinks, LOW);
  digitalWrite(LED_ObenRechts, LOW);
  digitalWrite(LED_UntenLinks, LOW);
  digitalWrite(LED_UntenRechts, LOW);
}

void Datenausgabe() {
  debug("Acceleration X: ");
  debug(a.acceleration.x);
  debug(", Y: ");
  debug(a.acceleration.y);
  //debug(", Z: ");
  //debug(a.acceleration.z);
  debugln(" m/s^2");

  debug("Rotation X: ");
  debug(g.gyro.x);
  debug(", Y: ");
  debug(g.gyro.y);
  //debug(", Z: ");
  //debug(g.gyro.z);
  debugln(" rad/s");

  debug("Temperature: ");
  debug(temp.temperature);
  debugln(" °C");

  debugln("");
}


void loop() {
  MPU.getEvent(&a, &g, &temp);

  if (a.acceleration.x != acc_x_prev || a.acceleration.y != acc_y_prev) {
    //// vorherige Linien schwarz malen
    // X-Linie
    Linie_x_prev = map(-acc_x_prev, -Gmax, Gmax, 0, OLED_Breite - 1); // a.acceleration.x invertieren
    OLED.drawFastHLine(0, Linie_x_prev, OLED_Hoehe, SSD1306_BLACK); // x0, y0, Länge, Farbe
    // Y-Linie
    Linie_y_prev = map(acc_y_prev, -Gmax, Gmax, 0, OLED_Hoehe - 1);
    OLED.drawFastVLine(Linie_y_prev, 0, OLED_Breite, SSD1306_BLACK); // x0, y0, Länge, Farbe

    //// neue Linien weiß malen
    // X-Linie
    int Linie_x = map(-a.acceleration.x, -Gmax, Gmax, 0, OLED_Breite - 1); // a.acceleration.x invertieren
    OLED.drawFastHLine(0, Linie_x, OLED_Hoehe, SSD1306_WHITE); // x0, y0, Länge, Farbe
    // Y-Linie
    int Linie_y = map(a.acceleration.y, -Gmax, Gmax, 0, OLED_Hoehe - 1);
    OLED.drawFastVLine(Linie_y, 0, OLED_Breite, SSD1306_WHITE); // x0, y0, Länge, Farbe

    // Mittelkreis zeichnen
    OLED.drawCircle((OLED.width() / 2) - 1, (OLED.height() / 2) - 1, 2, SSD1306_WHITE); // Mittelpunkt-x, Mittelpunkt-y, Radius, Farbe
    OLED.display();

    acc_x_prev = a.acceleration.x;
    acc_y_prev = a.acceleration.y;
  }

  /*
    CurrMillis = millis();

    // LEDs x blinken
    int BlinkInterval_x = map(abs(a.acceleration.x), 0, Gmax, 0, 1000);

    if (CurrMillis - PrevMillis_x >= BlinkInterval_x) {
    if (a.acceleration.x < 0) {
      digitalWrite(LED_ObenLinks, HIGH);
      digitalWrite(LED_ObenRechts, HIGH);

      digitalWrite(LED_UntenLinks, LOW);
      digitalWrite(LED_UntenRechts, LOW);
    } else if (a.acceleration.x == 0) {
      digitalWrite(LED_ObenLinks, LOW);
      digitalWrite(LED_ObenRechts, LOW);

      digitalWrite(LED_UntenLinks, LOW);
      digitalWrite(LED_UntenRechts, LOW);
    } else if (a.acceleration.x > 0) {
      digitalWrite(LED_ObenLinks, LOW);
      digitalWrite(LED_ObenRechts, LOW);

      digitalWrite(LED_UntenLinks, HIGH);
      digitalWrite(LED_UntenRechts, HIGH);
    }
    PrevMillis_x = CurrMillis;
    }

    byte BlinkInterval_y = map(abs(a.acceleration.y), 0, 19.61, 0, 255);

    if (a.acceleration.y < 0) {
    digitalWrite(LED_ObenLinks, HIGH);
    digitalWrite(LED_UntenLinks, HIGH);

    digitalWrite(LED_ObenRechts, LOW);
    digitalWrite(LED_UntenRechts, LOW);

    } else if (a.acceleration.y == 0) {
    digitalWrite(LED_ObenLinks, LOW);
    digitalWrite(LED_UntenLinks, LOW);

    digitalWrite(LED_ObenRechts, LOW);
    digitalWrite(LED_UntenRechts, LOW);
    } else if
    (a.acceleration.y > 0) {
    digitalWrite(LED_ObenLinks, LOW);
    digitalWrite(LED_UntenLinks, LOW);

    digitalWrite(LED_ObenRechts, HIGH);
    digitalWrite(LED_UntenRechts, HIGH);
    }
  */

  Datenausgabe();

}