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


const int PWMB = 11;
const int BIN2 = 2;
const int BIN1 = 5;

const int trigPinLeft=3;
const int echoPinLeft=4;
const int trigPinRight=6;
const int echoPinRight=7;
const int trigPin = 9;
const int echoPin = 8;
//int switchPin = 7;

float distance = 0;
float distanceRight=0;
float distanceLeft=0;
/********************************************************************************/
void setup()
{
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  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();
  Serial.println(distance);
    if(distance < 12){
      stop();
      delay(100);
      goLeft();
      delay(100);
      distanceLeft=getDistanceLeft();
      //Serial.println(distanceLeft);
      distanceRight=getDistanceRight();
      //Serial.println(distanceRight);
      delay(100);
      while(distanceLeft<1||distanceRight<1){

        if(distanceLeft<1){
          goRight();
        }
        else if(distanceRight<1){
          goLeft();
        }
        distanceLeft=getDistanceLeft();

        distanceRight=getDistanceRight();

        delay(5);
      }
    }
    else{
      goForwards(255);
    }
  delay(5);

 //goForwards(255);

}

/********************************************************************************/
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-127);
}
/********************************************************************************/
void goBackwards(int speed){
  rightMotor(speed*-1);
  leftMotor(speed*-1);
}
/********************************************************************************/
void goRight(){
  rightMotor(-155);
  leftMotor(155);
}
/********************************************************************************/
void goLeft(){
  rightMotor(155);
  leftMotor(-155);
}
/********************************************************************************/
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;
}