#include <SPI.h>  // SPI für die Kommunikation
#include <SD.h>
#include <TFT_eSPI.h>  // Graphics and font library for ST7735 driver chip
#include <FS.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include "esp_attr.h"  // für interrupt

/* TFT Display ST7735:
  Pinnung
    Display (Pin) - ESP32
    GND  (1) - GND
    VCC  (2) - 5V
    SCK  (3) - G18
    SDA  (4) - G23
    RES  (5) - G14
    RS/A0(6) - G2
    CS   (7) - G17
    LEDA (8) - 3.3V

    SD Kartenleser Pinning
    (Pin) - ESP32
    SCK   - G18
    MISO  - G19
    MOSI  - G23
    CS    - G05

  Beschleunigugssensor GY-521 IIC Interface n ESP32
  VCC 3.3V / 5V
  GND Masse
  SCL I2C-Clock       G22
  SDA I2C-Data        G21
  XDA I2C-Master-Data
  XCL I2C-Master-Clock
  AD0 high / low (Adress-Select) LOW=0x68, HIGH=0x69
  INT Interrupt (optional)          G2 ???
*/

TFT_eSPI tft = TFT_eSPI();  // Invoke library, pins defined in User_Setup.

// SD Karte
#define SD_CS 5  // Chip Select SD-Karte
String FName = "/init.cfg";
File myFile;
String textin = "Wir haben nix";

// Ruettelsensor MPU050
Adafruit_MPU6050 mpu;

// 0-Punkte für jusierung
float ax, ay, az;  //Beschleinigung
float gx, gy, gz;  //Rotation
float lasttemp = 0;

// Korrektur der Farbfehler im Include File ST7735.h im 565 (16 Bit) System
#define C_RED 0x001F      //ST7735_BLUE
#define C_BLUE 0xF800     //ST7735_RED
#define C_GREEN 0x07E0    //ST7735_GREEN
#define C_CYAN 0xFFE0     //ST7735_YELLOW
#define C_ORANGE 0x03bf   //ST7735_ORANGE
#define C_MAGENTA 0xF81F  //ST7735_MAGENTA
#define C_YELLOW 0x07FF   //ST7735_CYAN
#define C_WHITE 0xFFFF    //ST7735_WHITE
#define C_BLACK 0x0000    //ST7735_BLACK

// TFT Position der headlines
#define Spos 0    // Überschrift Speed
#define Rpos 72   // Überschrift Roration
#define Tpos 145  // Position  Temperatur und Meldungen
#define Delta 16
#define dhead 27      // abstand von Überschrift
#define dbalk 12      // Abstand Balken
#define balken 10     // höhe balken
#define colx C_RED    //  Balken  für X
#define coly C_GREEN  // Balken für Y
#define colz C_BLUE   // Balken für Z

// Farben, nur so zum Spass
unsigned int farbe[] = { C_RED, C_GREEN, C_BLUE, C_CYAN, C_ORANGE, C_MAGENTA, C_YELLOW };
int anzFarbe = 6;
int aktFarbe = 0;
int Zeile = 0;  // Zeile auf TFT

// Funktionsauswahl
#define losgehts 33  // Start Routine
// Reset Offsetwerte 0-Setzen
#define Rset 32
int WOffset = 0;

// Interruptroutine, sete Varialble Offset auf true
void IRAM_ATTR SetOffset() {
  WOffset = 1;
}

void setup() {
  Serial.begin(115200);
  Serial.println("MPU6050 Sensor");
  // für Funktionsauswahl
  pinMode(losgehts, INPUT);     // Start routine
  digitalWrite(losgehts, LOW);  // Start routine

  // Offsetwerte setzen
  pinMode(Rset, INPUT_PULLUP);  // wenn 0 dann Offsetwerte einlesen
                                //digitalWrite(Rset, HIGH);
                                // attachInterrupt(GPIOPin, Routine, Mode) - Interruptroutionn aufruf
  attachInterrupt(Rset, SetOffset, RISING);

  //Init  Screen
  tft.init();
  tft.setRotation(1);
  tft.fillScreen(C_BLACK);
  tft.setRotation(2);
  // setTextSize(groesse);
  tft.setTextSize(2);
  // setCursor(links,oben);
  tft.setCursor(5, 0);
  // setTextColor(farbe);
  tft.setTextColor(C_WHITE);
  // print(text);
  tft.print("Guten Tag");
  tft.setTextSize(1);
  tft.setCursor(0, 20);
  tft.println("TFT-SPI Test");
  tft.println((String) "File:" + FName);
  tft.drawLine(0, 40, 127, 40, C_BLUE);
  tft.setCursor(0, 50);

  // SD Karte
  Serial.print("Initializing SD card...");
  digitalWrite(SD_CS, HIGH);  // SD card chips select, must use GPIO 5 (ESP32 SS)
  delay(100);
  if (!SD.begin()) {
    Serial.println("initialization failed!");
    tft.setTextColor(C_RED);
    tft.setTextSize(2);
    tft.println("SD-Fehler");
    //while (1);
  }
  Serial.println("initialization done.");
  listDir(SD, "/", 1);
  tft.setTextSize(1);
  tft.setTextColor(C_GREEN);
  tft.println("SD fertig");
  delay(10);

  // Gyroskop
  if (!mpu.begin()) {
    Serial.println("Gyroskop MPU6050 nicht gefunden");
    tft.setTextSize(2);
  tft.setTextColor(C_RED);
  tft.println("No Sensor");
    while (1) {
      delay(10);
    }
  }
  tft.println("MPU6050 ok");
  Serial.println("Gyroskop MPU6050 gefunden");
  SetUpGyro();

  tft.println("alles fertig");
  Serial.println("Inizialisierung fertig");

  // SetUp TFT
  tft.fillScreen(C_BLACK);
  tft.setRotation(2);
  tft.setTextSize(2);
  tft.setCursor(0, Spos);
  tft.setTextColor(C_YELLOW);
  //tft.print("Speed up");
  tft.print("Beschl.");
  //tft.setTextSize(1);
  //tft.println(" (m/s^2)");


  tft.setTextColor(C_ORANGE);
  tft.setCursor(0, Rpos);
  tft.setTextSize(2);
  tft.print("Rotation");

  Serial.println("");
}

//*****************************************************************
//  Hauptprogramm
//*****************************************************************
void loop() {
  // MPU6050 Routinen
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Falls gedrückt, Offsetwerte einlesen (von Interrupt)
  if (WOffset == 1) {
    Serial.println("Offsetwerte setzen");
    tft.fillRect(0, 145, 128, 20, C_BLACK);
    tft.setCursor(0, 145);
    tft.setTextSize(2);
    tft.setTextColor(C_RED);
    tft.print("Set Offset");
    lasttemp = 0;
    while (digitalRead(Rset) == LOW) {  // gegen Prellen
      delay(10);
    }
    WOffset = 0;  // Flag Rückseten, wird in Interruptroutine gesetzt
    ax = a.acceleration.x;
    ay = a.acceleration.y;
    az = a.acceleration.z;
    gx = g.gyro.x;
    gy = g.gyro.y;
    gz = g.gyro.z;
    delay(1000);
  }
  // Ausgabe auf Serial Monitor
  /* Print out the values */
  Serial.print("Beschleunigung X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" Grad/s");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");

  //******************************************************************
  // Zeichnen Balken Beschleunigung
  // Aufruf: void zeichnen(float xwert, float ywert, float zwert, int faktor, int position, int offset, long farbe)
  //******************************************************************
  // Beschleunigung
  zeichnen(a.acceleration.x - ax, a.acceleration.y - ay, a.acceleration.z - az, 10, Spos, Delta, C_YELLOW);
  //Rotatoion
  zeichnen(g.gyro.x - gx, g.gyro.y - gy, g.gyro.z - gz, 26, Rpos, Delta, C_ORANGE);
  //Temperatur
  // falls neue Temeratur
  //if (lasttemp != temp.temperature) 
  if (abs(lasttemp - temp.temperature)>0.1) 
  {
    tft.fillRect(0, Tpos, 128, 20, C_BLACK);
    tft.setTextColor(C_CYAN);
    tft.setTextSize(2);
    tft.setCursor(0, Tpos);
    //sprintf(TBuff, "Temp: %5.2f", temp.temperature);
    lasttemp = temp.temperature;
    tft.print((String) "Temp:" + lasttemp);
  }

  //delay(500);
}

//**********************************************************************************
// Balkenwerte zeichnen und Werte auf TFT
//************************************************************************************
void zeichnen(float xwert, float ywert, float zwert, int faktor, int position, int offset, long farbe) {
  int hwert;
  char RBuff[40];
  // Rotation Zeichnen
  // zeichnen x-Wert
  tft.fillRect(0, position + dhead, 128, balken, C_BLACK);
  hwert = ((xwert)*faktor);
  if (hwert < 0) {
    tft.fillRect(64 + hwert, position + dhead, -hwert, balken, colx);
  } else {
    tft.fillRect(64, position + dhead, hwert, balken, colx);
  }
  // zeichnen y-Wert
  tft.fillRect(0, position + dhead + dbalk, 128, balken, C_BLACK);
  hwert = (ywert * faktor);
  if (hwert < 0) {
    tft.fillRect(64 + hwert, position + dhead + dbalk, -hwert, balken, coly);
  } else {
    tft.fillRect(64, position + dhead + dbalk, hwert, balken, coly);
  }
  // zeichnen z-Wert
  tft.fillRect(0, position + dhead + 2 * dbalk, 128, balken, C_BLACK);
  hwert = (zwert * faktor);
  if (xwert < 0) {
    tft.fillRect(64 + hwert, position + dhead + 2 * dbalk, -hwert, balken, colz);
  } else {
    tft.fillRect(64, position + dhead + 2 * dbalk, hwert, balken, colz);
  }
  tft.setCursor(0, position + Delta);
  tft.fillRect(0, position + Delta, 128, 10, C_BLACK);
  tft.setTextColor(farbe);
  tft.setTextSize(1);
  tft.setCursor(0, position + Delta);
  sprintf(RBuff, "%5.2fx %5.2fy %5.fz", xwert, ywert, zwert);
  tft.print(RBuff);
}



//*****************************************************************
//  Gyroskop initialisieren
//*****************************************************************
void SetUpGyro() {
  Serial.println("Initialsierung GY-521");
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+- 250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+- 500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+- 1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+- 2000 deg/s");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
  }

  Serial.println("Gyroskop und Beschleunigugssenor initialisiert");
}

//*****************************************************************
// Ausgabe Zeile auf TFT
void tft_raus(String tfttext) {

  Zeile += 1;
  // Löschen Display
  if (Zeile > 10) {
    tft.fillRect(0, 50, 127, 110, C_BLACK);
    tft.setCursor(0, 50);
    Zeile = 0;
  }
  aktFarbe = (aktFarbe < anzFarbe) ? aktFarbe + 1 : 0;
  tft.setTextColor(farbe[aktFarbe]);

  tft.println(tfttext);
}

//*****************************************************************
// Directory der SD-Karte

void listDir(fs::FS &fs, const char *dirname, uint8_t levels) {
  Serial.printf("Verzeichnis: %s\n", dirname);

  tft.setTextColor(C_YELLOW);

  File root = fs.open(dirname);
  if (!root) {
    Serial.println("Failed to open directory");
    return;
  }
  if (!root.isDirectory()) {
    Serial.println("Not a directory");
    return;
  }
  File file = root.openNextFile();
  while (file) {
    if (file.isDirectory()) {
      Serial.print("  DIR : ");
      Serial.println(file.name());
      if (levels) {
        listDir(fs, file.name(), levels - 1);
      }
    } else {
      Serial.print("  FILE: ");
      Serial.print(file.name());
      Serial.print("  SIZE: ");
      Serial.println(file.size());
      tft.print("File:");
      tft.println(file.name());
    }
    file = root.openNextFile();
  }
}
//*****************************************************************
// Inhalt SD-Karten File
void listSD() {

  // Lesen SD Karte
  myFile = SD.open(FName);
  delay(100);
  if (myFile) {
    while (myFile.available()) {
      textin = myFile.readStringUntil('\n');
      Serial.println(textin);
    }
    myFile.close();
  } else {
    Serial.println("File not found: " + FName);
  }
  // Ausgabe Serial
  Serial.println(textin);
  tft_raus(textin);
}
