// Example sketch for the MPU-6050.
//
// Version 1, 9 August 2021, by Koepel with help from Wokwi community.
//
// Start the simulation and click on the MPU-6050 module to change the values.
//
// Note:
//   This example does not use a library with quaternations for the 3D orientation.
//   The values are directly read from the MPU-6050 and are used to rotate a cube.
//   It is just a fun example. The rotation of the cube is not according to the
//   physical values of the accelerometer and the gyro.
//
// Based on:
//   (1)
//     3D_Cube for Arduino OLED module by Colin Ord, 9/1/2015
//     A port of my (Colin Ord) original JustBasic Cube_3D demo to the Arduino Uno using U8G library.
//     http://colinord.blogspot.com/2015/01/arduino-oled-module-with-3d-demo.html
//     (no known copyrights)
//
//   (2)
//     MPU-6050 Short Example Sketch
//     By Arduino User JohnChi
//     August 17, 2014
//     Public Domain
//     https://playground.arduino.cc/Main/MPU-6050/#short
// 
//   (3)
//     The Adafruit GFX library with the SSD1306 driver.
//

#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>

Adafruit_SSD1306 display( 128, 64);   // 128 pixels width, 64 pixels height

#define LED_PIN   6
#define LED_COUNT 1

// Set the origin in the middle of the display
const int xOrigin = 64;
const int yOrigin = 32;

int x=1;
int y=1;
//int buf =0 ;

// Vertices for a cube
// A cube has 8 corners and each coordinate has x,y,z values.
// The wirefram is to display the lines on the OLED display
// It contains the corners of the shape in 2D coordinates


void setup()
{
  Serial.begin( 115200);
  // Initialize the OLED display and test if it is connected.
  if( !display.begin( SSD1306_SWITCHCAPVCC, 0x3C)) 
  {
    Serial.println(F( "SSD1306 allocation failed"));
    for(;;);                                   // halt the sketch if error encountered
  }


  
}


void loop()
{
  // Start with a empty buffer
  display.clearDisplay();
  
   x = 1;
    for (int i = 1; i < (318 * 20); i++)  //draw 20 iterations
    {
        uint16_t color = SSD1306_BLACK;           //for rubout
        x++;
        if (x == 319){     //start again
            x = 1;}
          
        if (i > 319)       //rub out the previous sinewave 
        {
            if ((x == 159) || (buf[x - 1] == 119)){ // ?axes 
                color = SSD1306_WHITE;  //yes, draw BLUE axis
            display.drawPixel(x, buf[x - 1], SSD1306_WHITE);
            } //replace the previous point
        }
        y = 119 + (sin(((i * 1.1) * 3.14) / 180) * (90 - (i / 100)));
        display.drawPixel(x, y, SSD1306_WHITE);              //plot the new sinewave point
        buf[x - 1] = y;                      //remember the point for next pass
    }

  display.display();
}