//#include <Adafruit_ILI9341.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <MPU6050_light.h>
#include <Wire.h>

//#define TFT_DC 2
//#define TFT_CS 15
//Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

MPU6050 mpu(Wire);

unsigned long timer = 0; 

void setup() {
  Serial.begin(115200);
  Serial.begin(115200);
  Wire.begin();
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE,0);
  display.setCursor(35,10);
  display.print("ESP32");
  display.setCursor(25,40);
  display.print("MPU6050");
  display.display();

  //tft.begin();
  //tft.setRotation(1);

  mpu.begin();
  //  if (!mpu.begin()) {
  //  Serial.println("Failed to find MPU6050 chip");
  //  while (1) {
  //    delay(10);
  //  }
  //}
  delay(1000);
  mpu.calcGyroOffsets();
}

void loop() {
  if (millis() - timer > 100) { 
    float x = mpu.getAngleX();
    float y = mpu.getAngleY();
    float z = mpu.getAngleZ();
    
    //tft.fillScreen(ILI9341_BLACK); 
    //tft.setTextSize(3); 
    //tft.setCursor(5, 15); 
    //tft.setTextColor(ILI9341_RED);  
    //tft.print("X: ");
    //tft.println(x);

    //tft.setCursor(5, 40); 
    //tft.print("Y: ");
    //tft.println(y);

    //tft.setCursor(5, 65);
    //tft.print("Z: ");
    //tft.println(z);

    Serial.print(mpu.getAngleX());
    Serial.print(", ");
    Serial.print(mpu.getAngleY());
    Serial.print(", ");
    Serial.println(mpu.getAngleZ());
    timer = millis();
        
    delay(1000);
  }
}