// Choose your I2C implementation before including Tiny4kOLED.h
// The default is selected is Wire.h

// To use the Wire library:
//#include <Wire.h>

// To use the Adafruit's TinyWireM library:
//#include <TinyWireM.h>

// To use the TinyI2C library from https://github.com/technoblogy/tiny-i2c
//#include <TinyI2CMaster.h>


#include <Tiny4kOLED.h>
#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);

#include <SoftwareSerial.h>

SoftwareSerial bluetooth(4, 5); // RX, TX


void setup() {
 // set the data rate for the SoftwareSerial port
  bluetooth.begin(9600);
  
  // Send the initialization sequence to the oled. This leaves the display turned off
  oled.begin();

  // Two rotations are supported,
  // The begin() method sets the rotation to 1.
  //oled.setRotation(0);

  // Some newer devices do not contain an external current reference.
  // Older devices may also support using the internal curret reference,
  // which provides more consistent brightness across devices.
  // The internal current reference can be configured as either low current, or high current.
  // Using true as the parameter value choses the high current internal current reference,
  // resulting in a brighter display, and a more effective contrast setting.
  //oled.setInternalIref(true);

  // Two fonts are supplied with this library, FONT8X16 and FONT6X8
  // Other fonts are available from the TinyOLED-Fonts library
  oled.setFont(FONT8X16);

  // Clear the memory before turning on the display
  oled.clear();

  // Turn on the display
  oled.on();

  // Switch the half of RAM that we are writing to, to be the half that is non currently displayed
  oled.switchRenderFrame();
  Serial.begin(9600);
   Wire.begin();
 byte status = mpu.begin();
   Serial.print(F("MPU6050 status: "));
   Serial.println(status);
   while (status != 0) { } // stop everything if could not connect to MPU6050
 Serial.println(F("Calculating offsets, do not move MPU6050"));
   delay(1000);
   mpu.calcOffsets(); // gyro and accelero
   Serial.println("Done!\n");
}

void loop() {
  
  mpu.update();
 
//     Serial.print("X : ");
//     Serial.print(mpu.getAngleX());
//     Serial.print("\tY : ");
//     Serial.print();
//    Serial.print("\tZ : ");
//     Serial.println(mpu.getAngleZ());
  
  delay(50);
  float angle = mpu.getAngleY(); // declare the angle Y of the gyro on the variable angle
  updateDisplay(angle);
  Serial.println(angle);
 
}

void updateDisplay(float angleY) {
  // Clear the half of memory not currently being displayed.
  oled.clear();

  // Position the text cursor
  // In order to keep the library size small, text can only be positioned
  // with the top of the font aligned with one of the four 8 bit high RAM pages.
  // The Y value therefore can only have the value 0, 1, 2, or 3.
  // usage: oled.setCursor(X IN PIXELS, Y IN ROWS OF 8 PIXELS STARTING WITH 0);
  oled.setCursor(0, 1);

  // Write text to oled RAM (which is not currently being displayed).
  oled.print(F("ms: "));

  // Write the number of milliseconds since power on.
  oled.print(mpu.getAngleY());

  // Swap which half of RAM is being written to, and which half is being displayed.
  // This is equivalent to calling both switchRenderFrame and switchDisplayFrame.
  oled.switchFrame();
}