#include <Arduino.h>
// I2C / TWI devices
#include <Wire.h>
// OLED (AdaFruit SSD)
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
// IMU
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include "U8glib.h"

U8GLIB_SSD1306_128X64 u8g; // create an instant of OLED display

/* function prototypes */
void begin_OLED();
void begin_IMU();

// for SSD1306 declaration
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     4 // Reset pin #
// Declaration for SSD1306 OLED display
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// create an 'mpu' object
Adafruit_MPU6050 mpu;

#define ssdAddress 0x3C   // I2C address of the SSD1306
#define mpuAddress 0x68   // I2C address of the MPU-6050
float x = 0;
float y = 0;
float z = 0;

void setup() {
    // setup serial
    Serial.begin(9600);
    // initialize as I2C master
    Wire.begin();
    // try to connect to the OLED display
    begin_OLED();
    // Try to connect to IMU
    begin_IMU();
    // Show Adafruit logo on screen on startup.
    display.display();
    delay(1000);
    // Clear the buffer
    display.clearDisplay();
    u8g.setFont(u8g_font_7x13); // set the font for text
  //u8g.setColorIndex(1); set the display colour can be done outside picture loop

}

sensors_event_t event;

void loop() 
{
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.println(g.gyro.z);
  x = (g.gyro.x)*20;
  y = (g.gyro.y)*20;
  z = (g.gyro.z)*20;
  delay(100);
  LCD_display();
}

void LCD_display() 
{
  u8g.firstPage(); //marks the beginning of the picture loop.
  do {
    u8g.setColorIndex(1); // set the colour to white
    u8g.drawStr(0, 10, "X-axis"); //display "hello world" text (0,50) 
    u8g.drawFrame(0, 10, 128, 20); //draw frame at (0,10) with width = 128, height = 20
    u8g.drawBox(10, 15, x, 10); //draw box at (10,15) with width = x, height = 10
    u8g.drawStr(0, 30, "Y-axis"); //display "hello world" text (0,50) 
    u8g.drawFrame(0, 30, 128, 20); //draw frame at (0,10) with width = 128, height = 20
    u8g.drawBox(10, 35, y, 10); //draw box at (10,15) with width = x, height = 10
    u8g.drawStr(0, 50, "Z-axis"); //display "hello world" text (0,50) 
    u8g.drawFrame(0, 50, 128, 20); //draw frame at (0,10) with width = 128, height = 20
    u8g.drawBox(10, 55, z, 10); //draw box at (10,15) with width = x, height = 10
  } while ( u8g.nextPage() ); //marks the end of the body of the picture loop
}

void begin_OLED() 
{
    if(!display.begin(SSD1306_SWITCHCAPVCC, ssdAddress)) {  // default address is 0x3C
        Serial.println(F("SSD1306 connection failed"));
        while(1);   // SSD1306 did not connect, loop forever
    }
    // device connected successfully
    Serial.println("SSD1306 Connected!");   
}

void begin_IMU() 
{
    if (!mpu.begin(mpuAddress)) {     // default address is 0x68
        Serial.println("MPU6050 connection failed");
        while (1);  // IMU didn't connect, loop forever
    }
    // device connected successfully
    Serial.println("MPU6050 Connected!");   
}