// C++ code
//
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;
int sensor_left,sensor_right;
int value1,value2,value3;

void setup()
{
  
  for(int i=9;i<=11;i++){
    pinMode(i,INPUT);
  }
  pinMode(8, OUTPUT);
  pinMode(12,OUTPUT);
  pinMode(13,OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(3,OUTPUT);

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

void loop(){
  for(int i=0; i<256;i++){
    digitalWrite(5,HIGH);
    digitalWrite(6,LOW);
    analogWrite(3,i);
    
    Serial.println(analogRead(A0));
    delay(100);
  }
  delay(100000);
  digitalWrite(5,HIGH);
  digitalWrite(6,HIGH);
  delay(1000);
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
  delay(1000);
  digitalWrite(5,HIGH);
  digitalWrite(6,LOW);
  delay(1000);
  digitalWrite(5,LOW);
  digitalWrite(6,HIGH);
  delay(1000);

  value1 = distance_measure(8,9);
  value2 = distance_measure(12,10);
  value3 = distance_measure(13,11);
  Serial.println(value1);
  Serial.println(value2);
  Serial.println(value3);

sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    /* Print out the values */
    Serial.print("AccelX:");
    Serial.print(a.acceleration.x);
    Serial.print(",");
    Serial.print("AccelY:");
    Serial.print(a.acceleration.y);
    Serial.print(",");
    Serial.print("AccelZ:");
    Serial.print(a.acceleration.z);
    Serial.print(", ");
    Serial.print("GyroX:");
    Serial.print(g.gyro.x);
    Serial.print(",");
    Serial.print("GyroY:");
    Serial.print(g.gyro.y);
    Serial.print(",");
    Serial.print("GyroZ:");
    Serial.print(g.gyro.z);
    Serial.println("");
    delay(1000);

  if (value2<25){
    if(value1>25){
     left(); 
     Serial.println("Left");
    }
    else if(value3>25){
      right();
      Serial.println("Right");
    }
    else{
      back();
      Serial.println("back");
      if(value1>25){
       left(); 
       Serial.println("Leftt");
      }
      else if(value3>25){
        right();
        Serial.println("Right");
      }
    }
  }
  else{
    front();
    Serial.println("Front");
  }
  delay(10);
}

void front(){
  digitalWrite(2,LOW);
  digitalWrite(4,HIGH);
  analogWrite(3,255);
  digitalWrite(5,LOW);
  digitalWrite(7,HIGH);
  analogWrite(6,255);
}
void left(){
  digitalWrite(2,LOW);
  digitalWrite(4,HIGH);
  analogWrite(3,255);
  digitalWrite(5,HIGH);
  digitalWrite(7,LOW);
  analogWrite(6,255);
}
void right(){
  digitalWrite(2,HIGH);
  digitalWrite(4,LOW);
  analogWrite(3,255);
  digitalWrite(5,LOW);
  digitalWrite(7,HIGH);
  analogWrite(6,255);
}

void back(){
  digitalWrite(2,HIGH);
  digitalWrite(4,LOW);
  analogWrite(3,255);
  digitalWrite(5,HIGH);
  digitalWrite(7,LOW);
  analogWrite(6,255);
}
int distance_measure(int j,int i){
  digitalWrite(j,HIGH);
  delayMicroseconds(1);
  digitalWrite(j,LOW);
  int duration1=pulseIn(i,HIGH);
//  int duration2=pulseIn(10,HIGH);
//  int duration3=pulseIn(11,HIGH);
  int distance1=duration1*0.034/2;
//  float distance2=duration2*0.034/2;
//  float distance3=duration3*0.034/2;
//Serial.println(distance2);
//  Serial.println(distance3);
  return distance1;
}