// Poernomo Maulana Rofif Aqilla
// Semar electrical assigmment

// Adding needed library, such library for SSD1306 display and MPU6050 sensor
#include <Adafruit_GFX.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050.h>
#include <Wire.h>

// Define screen size
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// define reset pin for the display
#define OLED_RESET -1
// Create an instance for the display
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// Create an instace for MPU6050 sensor
MPU6050 mpu;
// Creating necessary variable that we will use in the program
const int graph_height = 20;
const int graph_width = 128;
const int graph_top = SCREEN_HEIGHT - graph_height;
const int graph_left = 4;
const int graph_color = SSD1306_WHITE;
// variable for interval update of the graph
const int graph_interval = 10; 

int16_t prevAccelX = 0, prevAccelY = 0, prevAccelZ = 0;
int16_t graphData[graph_width] = {0};
void setup() {
  // put your setup code here, to run once:
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();

  // initialize the display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3c)) //Address 0x3C for 128x64
  {
    Serial.println("Display allocation invalid");
    // loop if display failed or invalid
    while(1);
  }

  // clear the display buffer
  display.clearDisplay();
}


void loop() {
  // put your main code here, to run repeatedly:
  // declare variables for accelaration and gyro
  int16_t accelX, accelY, accelZ;
  int16_t gyroX, gyroY, gyroZ;
  // read accelerometer and gyro data from sensor
  mpu.getAcceleration(&accelX , &accelY, &accelZ);
  mpu.getRotation(&gyroX , &gyroY, &gyroZ);

  // display the data
  display.setTextSize(1);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  // display for acceleration data
  display.setCursor(0,0);
  display.print("Accel :");
  display.setCursor(0,10);
  display.print("X : ");
  display.println(accelX);
  display.setCursor(0,20);
  display.print("Y : ");
  display.println(accelY);
  display.setCursor(0,30);
  display.print("Z : ");
  display.println(accelZ);

  // display for gyro data
  display.setCursor(SCREEN_WIDTH/2 ,0);
  display.print("Gyro :");
  display.setCursor(SCREEN_WIDTH/2, 10);
  display.print("X : ");
  display.println(gyroX);
  display.setCursor(SCREEN_WIDTH/2, 20);
  display.print("Y : ");
  display.println(gyroY);
  display.setCursor(SCREEN_WIDTH/2, 30);
  display.print("Z : ");
  display.println(gyroZ);

  // displaying the X-axis graph into the display
  lineGraph(accelX-prevAccelX);
  display.display();

  // storing the current value
  prevAccelX = accelX;
  prevAccelY = accelY;
  prevAccelZ = accelZ;

  // display the graph
  delay(graph_interval);
  display.display();
}

// function for Accelaration X-axis graph
void lineGraph(int X){
  static int graphPosition = 0;
  // shift graph data to the left
  for(int i = 0; i < graph_width-1; i++){
    graphData[i] = graphData[i + 1];
  }

  // adding new data point to right side of the graph
  graphData[graph_width - 1] = map(X , -32768, 32767, 0, graph_height);

  // showing the graph line
  for(int i = 0; i < graph_width-1; i++){
    int x0 = graph_left + i;
    int y0 = graph_top + graph_height - graphData[i];
    int x1 = graph_left + i + 1;
    int y1 = graph_top + graph_height - graphData[i+1];
  // function to draw X-axis graph line
    display.drawLine(x0, y0, x1, y1, graph_color);
  }
}