#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include "U8glib.h"
U8GLIB_SSD1306_128X64 u8g;
int spiritLevel = 0; 


Adafruit_MPU6050 mpu;

void setup() {
  u8g.setFont(u8g_font_7x14); 
  Serial.begin(115200);
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }  
}


void loop() {
  u8g.firstPage();

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float acc_total_vector = sqrt((a.acceleration.x*a.acceleration.x)+ (a.acceleration.y*a.acceleration.y) + (a.acceleration.z*a.acceleration.z));
  int angle_pitch_acc = asin((float)a.acceleration.y/acc_total_vector)* 57.296;
  int angle_roll_acc = asin((float)a.acceleration.x/acc_total_vector)* -57.296;
  do 
    {
      u8g.setPrintPos(1, 20); 
      u8g.print("Pitch angle: ");
      u8g.print(spiritLevel-angle_pitch_acc);
      u8g.setPrintPos(1,40);
      u8g.print("Roll angle: ");
      u8g.print(spiritLevel - angle_roll_acc);
    }while(u8g.nextPage());
  Serial.print(a.acceleration.x);
  Serial.print(",");
  Serial.print(a.acceleration.y);
  Serial.print(",");
  Serial.print(a.acceleration.z);
  Serial.print(", ");
  Serial.println("");
  
  
}