#include<Wire.h>
const int MPU=0x68; 
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
const int trigPin = 9;
const int echoPin = 10;
const int trigPin2 = 12;
const int echoPin2 = 13;
const int trigPin3 = 6;
const int echoPin3 = 7;
int position = 0;
char bigbtread[] = {0};
char btread;
String readString;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); 
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(115200);
  Serial1.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
}
void accandgyro(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,12,true);
  GyX=Wire.read()<<8|Wire.read();
  GyY=Wire.read()<<8|Wire.read();
  GyZ=Wire.read()<<8|Wire.read();

  if(2000 >= GyX){
    if(GyY <= 100){
      if(GyZ <= 2000){
        return;
        }
        else{Serial1.println("1");}
      } 
     else{
      if(GyY > 5000){
        if(GyZ > 1000){
            Serial1.println("1");
          }
        }
      if(GyZ <= 7000){return;}
      else{Serial1.println("1");}
      }
    }
  else{
    Serial1.println("1");
    }
  delay(100);

}
//Left
//ground
void Sensors(){
// Clears the trigPin
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
  long duration2 = pulseIn(echoPin2, HIGH);
// Calculating the distance
  long distance2= (duration2*0.034/2) + 3;
    delay(100);
  // Clears the trigPin
  digitalWrite(trigPin3, LOW);
  delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin3, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin3, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
  long duration3 = pulseIn(echoPin3, HIGH);
// Calculating the distance
  long distance3= (duration3*0.034/2) + 3;
    delay(100);
// 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
  long duration = pulseIn(echoPin, HIGH);
// Calculating the distance
  long distance= (duration*0.034/2) + 2;
   while (Serial1.available()) {
    delay(10);  //small delay to allow input buffer to fill

    char c = Serial1.read();  //gets one byte from serial buffer
    if (c == ',') {
      break;
    }  //breaks out of capture loop to print readstring
    readString += c; 
  } //makes the string readString  

  if (readString.length() >0) {
    Serial.println(readString.toInt()); //prints string to serial port out

  int thong = readString.toInt();
  long height= thong;
  long minh= height - 12;
  long maxh= height - 20;
  long maxhd= height + 20;
  long minhd= height + 12;
  if(distance > 100000){
    return;
    }
    //distance = left - 4
    //distance2 = stairs up - 5
    //stairs down - 0
    //distance3 = right - 6
    //front - 7
    //left and mid - 8
    //right and mid - 9
    //all of the above - 2
    else{
     if (distance < 40 && distance2 < 40 && distance3 < 40){
      
      Serial1.println('2');//all
      }
     else if (distance < 40 && distance2 < 40 && distance3 >= 40){
      Serial1.println('8');
      
      }
     else if (distance >= 40 && distance2 < 40 && distance3 < 40){
      
      Serial1.println('9');
      }
     else if (distance < 40 && distance2 >= 40 && distance3 < 40){
      
      Serial1.println('7');
      }
      else if (distance < 40 && distance2 >= 40 && distance3 >= 40){
      
      Serial1.println('4');
      }
      else if (distance >= 40 && distance2 >= 40 && distance3 < 40){
      
      Serial1.println('6');
      }
      else if (maxh < distance2 && distance2 < minh){
      
      Serial1.println('5');
      }
      else if (maxhd > distance2 && distance2 > minhd){
      
      Serial1.println('0');
      }       
      }
    
  }
  
}

void loop(){
  accandgyro();
  Sensors();
}