#include "MPU6050.h"

MPU6050::MPU6050() {
  
}

void MPU6050::getAngularVelocity() {
  gyroX = gyroXPresent / 131.0;
  gyroY = gyroYPresent / 131.0; 
  gyroZ = gyroZPresent / 131.0;
}
void MPU6050::calculateAngle() {  
  winkelX = winkelX + ((timePresent - timePast)*(gyroXPresent + gyroXPast - 2*gyroXCalli)) * 0.00000382;
  winkelY = winkelY + ((timePresent - timePast)*(gyroYPresent + gyroYPast - 2*gyroYCalli)) * 0.00000382;
  winkelZ = winkelZ + ((timePresent - timePast)*(gyroZPresent + gyroZPast - 2*gyroZCalli)) * 0.00000382;
}
void MPU6050::printData() {
  Serial.print("3-Achsen-Gyroskop              ");
  Serial.print("\tX=");
  Serial.print(gyroX);
  Serial.print("°/s");
  Serial.print("\tY=");
  Serial.print(gyroY);
  Serial.print("°/s");
  Serial.print("\tZ=");
  Serial.print(gyroZ);
  Serial.print("°\r\n");

  Serial.print("Winkel der Achsen              ");
  Serial.print("\tX=");
  Serial.print(winkelX);
  Serial.print("° ");
  Serial.print("\tY=");
  Serial.print(winkelY);
  Serial.print("° ");
  Serial.print("\tZ=");
  Serial.print(winkelZ);
  Serial.print("°\r\n");
    
  Serial.print("3-Achsen-Beschleunigungsmesser ");
  Serial.print("\tX=");
  Serial.print(beschleunigungX);
  Serial.print("m/s² ");
  Serial.print("\tY=");
  Serial.print(beschleunigungY);
  Serial.print("m/s² ");
  Serial.print("\tZ=");
  Serial.print(beschleunigungZ);
  Serial.print("m/s²\r\n");
}

void MPU6050::setUpMPU() {
  // power management
  Wire.beginTransmission(0b1101000);
  Wire.write(0x6B);
  Wire.write(0b00000000);
  Wire.endTransmission();

  // configure gyro
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1B);
  Wire.write(0b00000000);
  Wire.endTransmission();

  // configure accelerometer
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1C);
  Wire.write(0b00000000);
  Wire.endTransmission();  
}

void MPU6050::callibrateGyroValues() {
    for (int i=0; i<5000; i++) {
      getGyroValues();
      gyroXCalli = gyroXCalli + gyroXPresent;
      gyroYCalli = gyroYCalli + gyroYPresent;
      gyroZCalli = gyroZCalli + gyroZPresent;
    }
    gyroXCalli = gyroXCalli/5000;
    gyroYCalli = gyroYCalli/5000;
    gyroZCalli = gyroZCalli/5000;
}

void MPU6050::readAndProcessAccelData() {
  Wire.beginTransmission(0b1101000); 
  Wire.write(0x3B); 
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); 
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read(); 
  accelY = Wire.read()<<8|Wire.read(); 
  accelZ = Wire.read()<<8|Wire.read(); 
  MPU6050::processAccelData();
}

void MPU6050::processAccelData() {
  beschleunigungX = (accelX/16384.0)*9.81;
  beschleunigungY = (accelY/16384.0)*9.81; 
  beschleunigungZ = (accelZ/16384.0)*9.81;
}

void MPU6050::readAndProcessGyroData() {
  gyroXPast = gyroXPresent;
  gyroYPast = gyroYPresent;
  gyroZPast = gyroZPresent;
  timePast = timePresent;
  timePresent = millis();
  
  MPU6050::getGyroValues();
  MPU6050::getAngularVelocity();
  MPU6050::calculateAngle();
}

void MPU6050::getGyroValues() {
  Wire.beginTransmission(0b1101000); 
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6);
  while(Wire.available() < 6);
  gyroXPresent = Wire.read()<<8|Wire.read();
  gyroYPresent = Wire.read()<<8|Wire.read();
  gyroZPresent = Wire.read()<<8|Wire.read();
}