#include <Servo.h>
int Left = 45; //Degrees the legs move at
int Right = 135;
int Default = 90;
int z = 0;
int y = 0;
int tempPin = 7;
const int Trigpin = 5;
const int Echopin = 6;
const int distance_max = 50;
Servo Mservo;  // create servo object to control a servo
Servo Lservo;
Servo Rservo;
float duration_us, distance_cm;
float temp;

void setup() {
  pinMode(Trigpin, OUTPUT);
  pinMode(Echopin, INPUT);
  Mservo.attach(2);  // attaches servos to pins
  Lservo.attach(3);
  Rservo.attach(4);

    //Adjusting to default
  Mservo.write(Default);
  Lservo.write(Default);
  Rservo.write(Default);

}

void loop() {
  z = 0;
  y = 0;

//Reading Temperature
temp = analogRead(tempPin);
temp = temp * 0.48828125;
if(temp > 60){
Mservo.write(Left);
    delay(500);
    Lservo.write(Default);
    delay(500);
    Mservo.write(Right);
    delay(500);
    Rservo.write(Default);
    delay(500);
    Mservo.write(Default);
    while(temp > 60){
  
    temp = analogRead(tempPin);
    temp = temp * 0.48828125;
}
}

  digitalWrite(Trigpin, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trigpin, LOW);

  duration_us = pulseIn(Echopin, HIGH);

  distance_cm = 0.017 * duration_us;
  if (distance_cm > distance_max) {
    //Each step taken
  Mservo.write(Left);
  delay(500);
  Lservo.write(Default);
  delay(500);
  Rservo.write(Right);
  delay(500);
  Mservo.write(Right);
  delay(500);
  Lservo.write(Left);
  delay(500);
  Rservo.write(Default);
  } else{


    //Servo's setting to default position
    Mservo.write(Left);
    delay(500);
    Lservo.write(Default);
    delay(500);
    Mservo.write(Right);
    delay(500);
    Rservo.write(Default);
    delay(500);
    Mservo.write(Default);
    //Hexapod Turning Left 90 degrees
    for (int x = 0; x < 3; x++) {
    Mservo.write(Left);
    delay(500);
    Rservo.write(Right);
    delay(500);
    Mservo.write(Right);
    delay(500);
    Rservo.write(Default);
    }

while((z == 0) && (temp < 60)) {
//check temperature
temp = analogRead(tempPin);
temp = temp * 0.48828125;

if(temp<60) {
  //Hexapod takes 2 steps
  for (int x = 0; x < 2; x++) {
  Mservo.write(Left);
  delay(500);
  Lservo.write(Default);
  delay(500);
  Rservo.write(Right);
  delay(500);
  Mservo.write(Right);
  delay(500);
  Lservo.write(Left);
  delay(500);
  Rservo.write(Default);
  }

  //Hexapod Readjusts
  Mservo.write(Left);
    delay(500);
    Lservo.write(Default);
    delay(500);
    Mservo.write(Right);
    delay(500);
    Rservo.write(Default);
    delay(500);
    Mservo.write(Default);
    //Hexapod Turns Right 90 degrees
    for (int x = 0; x < 3; x++) {
    Mservo.write(Right);
    delay(500);
    Lservo.write(Left);
    delay(500);
    Mservo.write(Left);
    delay(500);
    Lservo.write(Default);
    }

    //Checks for object
     digitalWrite(Trigpin, HIGH);
     delayMicroseconds(10);
     digitalWrite(Trigpin, LOW);

    duration_us = pulseIn(Echopin, HIGH);

    distance_cm = 0.017 * duration_us;

    if (distance_cm > distance_max){
      z++;
    } else{

      //Turns Left 90 degrees
    for (int x = 0; x < 3; x++) {
    Mservo.write(Left);
    delay(500);
    Rservo.write(Right);
    delay(500);
    Mservo.write(Right);
    delay(500);
    Rservo.write(Default);
    }
    }

    y++;

    //Testing the right side
    while(y == 10){
//Turning the opposite direction
    for (int x = 0; x < 6; x++) {
    Mservo.write(Right);
    delay(500);
    Lservo.write(Left);
    delay(500);
    Mservo.write(Left);
    delay(500);
    Lservo.write(Default);
    }

//Going back to initial wall
  for (int x = 0; x < 20; x++) {
  Mservo.write(Left);
  delay(500);
  Lservo.write(Default);
  delay(500);
  Rservo.write(Right);
  delay(500);
  Mservo.write(Right);
  delay(500);
  Lservo.write(Left);
  delay(500);
  Rservo.write(Default);
  }

//Repeating previous steps but to the right

//Taking 2 steps
  for (int x = 0; x < 2; x++) {
  Mservo.write(Left);
  delay(500);
  Lservo.write(Default);
  delay(500);
  Rservo.write(Right);
  delay(500);
  Mservo.write(Right);
  delay(500);
  Lservo.write(Left);
  delay(500);
  Rservo.write(Default);
  }

  //Hexapod Readjusts
  Mservo.write(Left);
    delay(500);
    Lservo.write(Default);
    delay(500);
    Mservo.write(Right);
    delay(500);
    Rservo.write(Default);
    delay(500);
    Mservo.write(Default);

    //Hexapod Turns Left 90 degrees
    for (int x = 0; x < 3; x++) {
    Mservo.write(Left);
    delay(500);
    Rservo.write(Right);
    delay(500);
    Mservo.write(Left);
    delay(500);
    Rservo.write(Default);
    }

    //Checks for object
     digitalWrite(Trigpin, HIGH);
     delayMicroseconds(10);
     digitalWrite(Trigpin, LOW);

    duration_us = pulseIn(Echopin, HIGH);

    distance_cm = 0.017 * duration_us;

    if (distance_cm < distance_max){
      z++;
    } else{

      //Turns Right 90 degrees
    for (int x = 0; x < 3; x++) {
    Mservo.write(Right);
    delay(500);
    Lservo.write(Left);
    delay(500);
    Mservo.write(Left);
    delay(500);
    Lservo.write(Default);
    }
    }
    }
   }
  }
  }

}