#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <SPI.h>
#include <Adafruit_GFX.h>



#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4
#define GRAPH_WIDTH 60 // OLED display width, in pixels
#define GRAPH_HEIGHT 22 // OLED display height, in pixels


Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);Adafruit_MPU6050 mpu;
// float Xaccel[100];
// float Yaccel[100];
// float Zaccel[100];
int current=-1;
float time[100];
float X;
float Y;
float Z;
float dispX;
float dispY;
float dispZ;
float  circleX, circleY, circleZ;
unsigned long last_t = 0 ;
unsigned long new_t;
unsigned long dt;
int _circularBuffer[GRAPH_WIDTH];
int _curWriteIndex = 0;


void drawLine(int , int );
//int _graphHeight = SCREEN_HEIGHT;
void setup(void) {
  Serial.begin(115200);
   display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  
  
  
   display.display();
   delay(1000);

  
   display.clearDisplay();

   display.setTextSize(1);
  display.setTextColor(WHITE);

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");
}



void loop() {
display.clearDisplay();
  new_t = millis();
  dt = (new_t -last_t)*0.001;
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float Xaccel=a.acceleration.x;
  float Yaccel=a.acceleration.y;
  float Zaccel=a.acceleration.z;
     dispX= dispX + (0.5*Xaccel*0.005*dt*dt)*0.1 ;
     dispY= dispY + (0.5*Yaccel*0.005*dt*dt)*0.1 ;   
     dispZ= dispZ + (0.5*Zaccel*0.005*dt*dt)*0.1 ; 
  X = dispX;
  Y = dispY;
  Z = dispZ;    
    if (X > 120){
      X=120;
      dispX=120;
    }
    else if (X < -120){
      X=-120;
      dispX=-120;
    }

    if (Y > 120){
      Y =120;
            dispY=120;

    }
    else if (Y < -120){
      Y =-120;
            dispY=-120;

    }

    if (Z > 30){
      Z =30;
      dispZ = 30;
    }
    else if (Z < -30){
      Z =-30;
      dispZ=-30;
    }
circleX=map(X,-120,120,5,60);
circleY=map(Y,-120,120,5,60);
circleZ=map(Z,-30,30,0,20);

long GraphX = map(X*100,-12000,12000,0,1024);

_circularBuffer[_curWriteIndex++] = GraphX;
if(_curWriteIndex >= GRAPH_WIDTH){
    _curWriteIndex = 0;
  }

int xPos = 0; 
for (int i = _curWriteIndex; i < GRAPH_WIDTH; i++){
  int analogVal = _circularBuffer[i];
  drawLine(xPos, analogVal);
  xPos++;
}

for(int i = 0; i < _curWriteIndex; i++){
  int analogVal = _circularBuffer[i];
  drawLine(xPos, analogVal);
  xPos++;;
}
display.display();


display.drawCircle(circleX,circleY,circleZ,SSD1306_WHITE);






display.drawLine(60,10, 128,10,SSD1306_BLACK);
display.display();
  // Serial.print("[");
  // Serial.print(time[current]);
  // Serial.print("] Xaccel: ");
  // Serial.print(Xaccel);
  Serial.print(" disp X: ");
  Serial.print(X);
    Serial.print(" disp Y: ");
  Serial.print(Y);
  Serial.print(" disp Z: ");
  Serial.println(Z);
  // Serial.print(", Yaccel: ");
  // Serial.print(Yaccel);
  // Serial.print(", Zacell: ");
  // Serial.print(Zaccel);
  // Serial.println(" m/s^2");
  // Serial.print("[");
  // Serial.print(time[current]);
  // Serial.print("] Xrot: ");
  // Serial.print(g.gyro.x);
  // Serial.print(",   Yrot: ");
  // Serial.print(g.gyro.y);
  // Serial.print(",   Zrot: ");
  // Serial.print(g.gyro.z);
  // Serial.println(" rad/s");
  delay(5);
};

void drawLine(int xPos, int analogVal){
  int lineHeight = map(analogVal, 0, 1024, 0,GRAPH_HEIGHT);
  int yPos = display.height() - lineHeight;
  
  display.drawFastVLine(xPos+60, yPos-44, lineHeight, SSD1306_WHITE);
  display.drawFastVLine(xPos+60, yPos-22, lineHeight,SSD1306_WHITE);
  display.drawFastVLine(xPos+60, yPos, lineHeight,SSD1306_WHITE );
  
}