#include <Ultrasonic.h>
#include <U8g2_for_Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Adafruit_Sensor.h>
#include <SPI.h>
#include <Adafruit_MPU6050.h>
#include "driver/gpio.h"
// for echo
#define BUTTON_PIN 27
#define TFT_DC 2
#define TFT_CS 15
Adafruit_MPU6050 mpu;
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
Ultrasonic ultrasonic(12, 13); // trigger pin, echo pin
int buzzerPin = 14;

void setup() {
pinMode(buzzerPin, OUTPUT);
tft.begin(); // initialize tft
Serial.println("");
delay(100);
Serial.begin(9600);
pinMode(BUTTON_PIN, INPUT_PULLUP);
pinMode (26, OUTPUT);
pinMode(25, OUTPUT);
Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

  Serial.println("");
  delay(100);



}

void loop() {
  int buttonState = digitalRead(BUTTON_PIN);
  if (buttonState == LOW) {
    tone(buzzerPin, 100, 1000);
  tft.fillScreen(ILI9341_BLACK);
  }
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");

  Serial.println("");
  delay(1500);
int distance = ultrasonic.read();
Serial.println(distance);
delay(500);
tft.setCursor(0, 0); // set cursor to top left corner of tft
tft.setTextColor(ILI9341_WHITE); // set text color to white
tft.setTextSize(2); // set text size to 2
tft.print("Distance: "); // print "Distance: "
tft.println(distance); // print measured distance on next line

tft.setCursor(0, 20);
tft.print("Rotation X: "); //
tft.println(g.gyro.x);
tft.print("Y: "); //
tft.println(g.gyro.y);
tft.print("Z: "); //
tft.println(g.gyro.z);
if (g.gyro.x>=-0.52 && g.gyro.x<=0.52)
  {Serial.println("takin");
  if (g.gyro.y>=-0.52 && g.gyro.y<=0.52)
    {Serial.println("takin");
      if (g.gyro.z>=-0.52 && g.gyro.z<=0.52)
        {Serial.println("takin");
        digitalWrite(26,HIGH);
        delay(1000);
        digitalWrite(26,LOW);
        }    
      else 
      {
      Serial.println("lo takin");
      digitalWrite(25,HIGH);
      delay(500);
     tone(buzzerPin, 100, 1000);
      digitalWrite(25,LOW);
      }
    
    }
  else 
    {
    Serial.println("lo takin");
    digitalWrite(25,HIGH);
    delay(500);
    tone(buzzerPin, 100, 1000);
    digitalWrite(25,LOW);
    }
  }
else 
{
Serial.println("lo takin");
digitalWrite(25,HIGH);
delay(1000);
tone(buzzerPin, 100, 1000);
digitalWrite(25,LOW);
}
if (distance<30) {
digitalWrite(25,HIGH);
delay(1000);
tone(buzzerPin, 100, 1000);
digitalWrite(25,LOW);
}
else{
digitalWrite(26,HIGH);
delay(1000);
digitalWrite(26,LOW);
}

delay(1500); // delay 500ms

tft.fillScreen(ILI9341_BLACK); // set screen color to black

}