#include <Wire.h>                                    //Bibl. für Kommunikation über KABEL (allgemein) d.h. auch für Motor o.ä. 
#include <Adafruit_Sensor.h>                        //für allgemeine Kommunikation (egal welcher Sensor/Kontroller) ("übergeordnete Bib")
#include <Adafruit_MPU6050.h>                      //spezifisch für MPU5060 (hängt von beiden anderen ab, daher vorher inkludieren)

Adafruit_MPU6050 mpu;                               //umbenennen d. Bib, damit in Code nicht immer voller Name geschrieben werden muss

sensors_event_t messung_a, messung_g, messung_t;   //legt Variablentyp + -namen (hier 3 Stk. je für Beschleun., Winkel und Temp. (für uns weiter nicht relevant)) an, in der Messwerte von Sensor gespeichert werden = Messung alle x Sekunden
float start_ax, start_ay, start_az, start_gx, start_gy, start_gz;     //Variablen, in die Ausgangslage des Sensors gespeichert werden (für Kalibrierung)
float v_x = 0, v_y = 0, v_z = 0, pos_x = 0, pos_y = 0, pos_z = 0, drehung = 0;
unsigned long lastTime = 0;

// Orientierung (Winkel) in Radiant
float angle_alpha = 0, angle_beta = 0, angle_gamma = 0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while (!Serial); // Warten, bis die serielle Verbindung hergestellt ist

  Serial.println("Hello, ESP32!");
   //Sensor starten (als Loop. falls bei Start was nicht funktioniert)
  while (!mpu.begin()) {
    Serial.println("noch nicht gestartet...");      // Loop, falls Start nicht funktioniert
    delay(1000);                                    //Angabe in ms
  }
  Serial.println("Start erfoglreich!");

  // Initiale Einstellungen für den MPU6050
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  //mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);

  //Werte zu Startzeitpunkt messen (später für Kalibrierung nötig)
  mpu.getEvent(&messung_a, &messung_g, &messung_t); 
  start_ax = messung_a.acceleration.x;
  start_ay = messung_a.acceleration.y;
  start_az = messung_a.acceleration.z;
  start_gx = messung_g.gyro.x;
  start_gy = messung_g.gyro.y;
  start_gz = messung_g.gyro.z;
  Serial.println("Ausgangswerte:");
  Serial.print(" Beschleunigung in x: ");
  Serial.print(start_ax);
  Serial.print(" in y: ");
  Serial.print(start_ay);
  Serial.print(" in z: ");
  Serial.println(start_az);
  Serial.print("  Verdrehung um x: ");
  Serial.print(start_gx);
  Serial.print(" um y: ");
  Serial.print(start_gy);
  Serial.print(" um z: ");
  Serial.println(start_gz);
  
  lastTime = millis();

}


void loop() {
  // put your main code here, to run repeatedly:
  mpu.getEvent(&messung_a, &messung_g, &messung_t); //alle Werte von Sensor auslesen

  // Zeitdifferenz berechnen
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0; // Delta t in Sekunden
  lastTime = currentTime;
  
  // Gyroskopwerte (gz in rad/s)
  float gz = messung_g.gyro.z - start_gz;
  
  // Winkelintegration (drehung in rad)
  drehung += gz * dt;   // (Drehung um Z-Achse)

  // Beschleunigungswerte lokal (in m/s²)
  float ax_local = messung_a.acceleration.x - start_ax;
  float ay_local = messung_a.acceleration.y - start_ay;
  float az_local = messung_a.acceleration.z - start_az;

  // Beschleunigungswerte global (in m/s²)
  float ax_global = ax_local * cos(-drehung) - ay_local * sin(-drehung);
  float ay_global = ax_local * sin(-drehung) + ay_local * cos(-drehung);
  float az_global = az_local;

  // Geschwindigkeitswerte (in m/s)
  v_x += ax_global * dt;
  v_y += ay_global * dt;
  v_z += az_global * dt;

  // Position in (m)
  pos_x += v_x * dt;
  pos_y += v_y * dt;
  pos_z += v_z * dt;

  // Beschleunigung ausgeben (=anzeigen), Angabe in m/s2 (Erdbeschleunigung beachten!)
  Serial.print(pos_x);
  Serial.print(" ");
  Serial.print(pos_y);
  Serial.print(" ");
  Serial.print(pos_z);
  Serial.print(" ");
  Serial.println(drehung);
  delay(10);
}