#include <Servo.h>
#include <Wire.h>
#include <MPU6050_light.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

MPU6050 mpu(Wire);

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

long timer = 0;
long timer2 = 0;
long timer3 = 0;

int ledPin[] = {3,4,5,6,7,8,9,10};
const int ledPin2 = 4;
const int ledPin3 = 5;
const int ledPin4 = 6;
const int ledPin5 = 7;
const int ledPin6 = 8;
const int ledPin7 = 9;
const int ledPin8 = 10;

const int servoPin = 11;

const int trigPin = 13;
const int echoPin = 12;
// defines variables
long duration;
int distance;

Servo myServo;

void setup() {
  pinMode(ledPin[0], OUTPUT);
  pinMode(ledPin[1], OUTPUT);
  pinMode(ledPin[2], OUTPUT);
  pinMode(ledPin[3], OUTPUT);
  pinMode(ledPin[4], OUTPUT);
  pinMode(ledPin[5], OUTPUT);
  pinMode(ledPin[6], OUTPUT);
  pinMode(ledPin[7], OUTPUT);
  myServo.attach(servoPin);
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  Serial.begin(9600); // Starts the serial communication
  Wire.begin();
  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  mpu.calcOffsets(true,true); // gyro and accelero
  Serial.println("Done!\n");

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  //display.display();

  // Clear the buffer
  display.clearDisplay();

  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.println(F("Hello, world!"));

  display.display();
}

void loop() {

  if (millis() - timer3 > 800) {
    for (int i = 0; i < 8; i++) {
      digitalWrite(ledPin[i], HIGH);
      delay(100);
    }

    for (int i = 7; i > -1; i--) {
      digitalWrite(ledPin[i], LOW);
      delay(100);
    }

  }
  
  if (millis() - timer2 > 500){
    myServo.write(0);
    delay(500);
    myServo.write(180);
    delay(500);
  }

  mpu.update();

  if(millis() - timer > 1000){ // print data every second
    Serial.print(("TEMPERATURE: ")); Serial.println(mpu.getTemp());
    Serial.print("\tX: "); Serial.print(mpu.getAngleX());
    Serial.print("\tY: "); Serial.print(mpu.getAngleY());
    Serial.print("\tZ: "); Serial.println(mpu.getAngleZ());

    // Clears the trigPin
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(echoPin, HIGH);
    // Calculating the distance
    distance = duration * 0.034 / 2;
    // Prints the distance on the Serial Monitor
    Serial.print("Distance: ");
    Serial.println(distance);
    timer = millis();
  }
}