const int AIN1 = 13;
const int AIN2 = 12;
const int PWMA = 11;


const int PWMB = 8;
const int BIN2 = 9;
const int BIN1 = 10;

const int trigPinLeft=4;
const int echoPinLeft=3;
const int trigPinRight=2;
const int echoPinRight=1;
const int trigPin = 6;
const int echoPin = 5;
 int switchPin = 7;

float distance = 0;
float distanceRight=0;
float distanceLeft=0;

int backupTime = 300;
int turnTime = 200;

/********************************************************************************/
void setup()
{
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  //pinMode(switchPin, INPUT_PULLUP);



  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  Serial.begin(9600);
}

/********************************************************************************/
void loop()
{
  distance = getDistance();
    if(distance < 8){
      stop();
      delay(25);
      goLeft();
      delay(turnTime);
      distanceLeft=getDistanceLeft();
      distanceRight=getDistanceRight();
      while(distanceLeft<1||distanceRight<1){
        distanceLeft=getDistanceLeft();
        distanceRight=getDistanceRight();
        if(distanceRight<1&&distanceLeft<1){
          goForwards(255);
          break;
        }
        else if(distanceLeft<1){
          goRight();
        }
        else if(distanceRight<1){
          goLeft();
        }
        delay(50);
      }
    }
    else{
      goForwards(255);
      delay(50);
    }

}

/********************************************************************************/
void rightMotor(int motorSpeed)
{
  if (motorSpeed > 0)
    {
      digitalWrite(AIN1, HIGH);
      digitalWrite(AIN2, LOW);
    }
  else if (motorSpeed < 0)
    {
      digitalWrite(AIN1, LOW);
      digitalWrite(AIN2, HIGH);
    }
  else
    {
      digitalWrite(AIN1, LOW);
      digitalWrite(AIN2, LOW);
    }
  analogWrite(PWMA, abs(motorSpeed));
}

/********************************************************************************/
void leftMotor(int motorSpeed)
{
  if (motorSpeed > 0)
    {
      digitalWrite(BIN1, HIGH);
      digitalWrite(BIN2, LOW);
    }
  else if (motorSpeed < 0)
    {
      digitalWrite(BIN1, LOW);
      digitalWrite(BIN2, HIGH);
    }
  else
    {
      digitalWrite(BIN1, LOW);
      digitalWrite(BIN2, LOW);
    }
  analogWrite(PWMB, abs(motorSpeed));
}
/********************************************************************************/
void goForwards(int speed){
  rightMotor(speed);
  leftMotor(speed);
}
/********************************************************************************/
void goBackwards(int speed){
  rightMotor(speed*-1);
  leftMotor(speed*-1);
}
/********************************************************************************/
void goRight(){
  rightMotor(-255);
  leftMotor(255);
}
/********************************************************************************/
void goLeft(){
  rightMotor(255);
  leftMotor(-255);
}
/********************************************************************************/
void stop(){
  rightMotor(0);
  leftMotor(0);
}
/********************************************************************************/
float getDistance()
{
  float echoTime;
  float calculatedDistance;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  echoTime = pulseIn(echoPin, HIGH);
  calculatedDistance = echoTime / 148.0;
  return calculatedDistance;
}
/********************************************************************************/
float getDistanceRight()
{
  float echoTimeRight;
  float calculatedDistanceRight;

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

  echoTimeRight = pulseIn(echoPinRight, HIGH);
  calculatedDistanceRight = echoTimeRight / 148.0;

  return calculatedDistanceRight;
}
/********************************************************************************/
float getDistanceLeft()
{
  float echoTimeLeft;
  float calculatedDistanceLeft;

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

  echoTimeLeft = pulseIn(echoPinLeft, HIGH);
  calculatedDistanceLeft = echoTimeLeft/ 148.0;

  return calculatedDistanceLeft;
}