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

#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels
#define OLED_RESET    -1  // Reset pin # (or -1 if sharing reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int MPU_addr=0x68;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

int minVal=265;
int maxVal=402;
unsigned long aktualnyCzas,Czas;
double xx,xp,xl;
double y;
double z;
int xd,xpz, xlz,x;

#define RST_PIN 12

void setup(){



  pinMode(RST_PIN, INPUT_PULLUP);


Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);

 display.clearDisplay();

 display.setTextSize(2);
display.setCursor(4,16);
display.print(".:Witaj:.");
display.display();*/
delay(3000);

}
void loop()
{

if (digitalRead(RST_PIN)==0){
  xpz=0;
  xlz=0;
}



Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);

x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);

y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

  aktualnyCzas = millis();
if (aktualnyCzas - Czas >= 50UL){
  Czas=aktualnyCzas;
  display.clearDisplay();




  if ((x>=0)&&(x<99)){
xp=x;
display.drawLine(64, 13, xp+64, 7, WHITE);
display.drawLine(64, 13, xp+64, 8, WHITE);
display.drawLine(64, 13, xp+64, 9, WHITE);
display.drawLine(64, 13, xp+64, 10, WHITE);
display.drawLine(64, 13, xp+64, 11, WHITE);
display.drawLine(64, 13, xp+64, 12, WHITE);
display.drawLine(64, 13, xp+64, 13, WHITE);
display.setTextSize(1);



display.setCursor(0,24);
display.print("Lewo:");
display.setTextSize(2);
display.setCursor(30,24);
display.println("0");

display.setTextSize(1);
display.setCursor(64,24);
display.print("Prawo:");
display.setTextSize(2);
display.setCursor(100,24);
display.println(xp,0);


 if (xp>=xpz){
  xpz=xp;

 }
  }

 if ((x>261)&&(x<=360)){
xd=x-360+64;
xl=360-x;
display.drawLine(64, 13, xd, 7, WHITE);
display.drawLine(64, 13, xd, 8, WHITE);
display.drawLine(64, 13, xd, 9, WHITE);
display.drawLine(64, 13, xd, 10, WHITE);
display.drawLine(64, 13, xd, 11, WHITE);
display.drawLine(64, 13, xd, 12, WHITE);
display.drawLine(64, 13, xd, 13, WHITE);



display.setTextSize(1);
display.setCursor(0,24);
display.print("Lewo:");
display.setTextSize(2);
display.setCursor(30,24);
display.println(xl,0);

display.setTextSize(1);
display.setCursor(64,24);
display.print("Prawo:");
display.setTextSize(2);
display.setCursor(100,24);
display.println("0");

if (xl>=xlz){
  xlz=xl;

 }
  }


  display.drawLine(4,0,4,15, WHITE);
display.drawLine(14,2,14,13, WHITE);
display.drawLine(24,0,24,15, WHITE);
display.drawLine(34,2,34,13, WHITE);
display.drawLine(45,0,45,15, WHITE);
display.drawLine(54,2,54,13, WHITE);

display.drawLine(63,0,63,15, WHITE);
display.drawLine(65,0,65,15, WHITE);

display.drawLine(74,2,74,13, WHITE);
display.drawLine(84,0,84,15, WHITE);
display.drawLine(94,2,94,13, WHITE);
display.drawLine(104,0,104,15, WHITE);
display.drawLine(114,2,114,13, WHITE);
display.drawLine(124,0,124,15, WHITE);

display.setTextSize(1);
display.setCursor(15,16);
 display.print(40);
display.setCursor(42,16);
 display.print(20);
display.setCursor(60,16);
 display.print(0);
display.setCursor(80,16);
 display.print(20);
display.setCursor(100,16);
 display.print(40);
display.setCursor(0,42);
 display.print(".::Najlepszy wynik::.");
 display.setCursor(0,50);
 display.print("Lewo:");
 display.setCursor(64,50);
 display.print("Prawo:");



  display.setTextSize(2);
display.setCursor(30,50);
 display.print(xlz);
display.setCursor(100,50);
 display.print(xpz);
display.display();

}
}