#include <SoftwareSerial.h>

int enA = 10;
int in1 = 9;
int in2 = 8;
// motor two
int enB = 5;
int in3 = 7;
int in4 = 6;
int Z = 0;

int trigPin = 11;
int echoPin = 12;
int trigPin1 = 3;
int echoPin1 = 4;
int trigPin2 = 2;
int echoPin2 = 13;

void setup()
{
  // set all the motor control pins to outputs
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  SoftwareSerial Camera(1,0);
  Serial.begin(9600);   
}

void Forward()
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  analogWrite(enA, 240);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enB, 255);

}

void Back()
{
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  analogWrite(enA, 240);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enB, 255);
}

void Left()
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  analogWrite(enA, 240);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enB, 255);
}

void Right()
{
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  analogWrite(enA, 240);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enB, 255);
}


void Stop()
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);  
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

void SwingLeft()
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  analogWrite(enA, 100);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enB, 255);
}

void SwingRight()
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  analogWrite(enA, 255);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enB, 100);
}

void LeftTurn()
{
  Left();
  delay(1350);
  Forward();
}


void RightTurn()
{
  Right();
  delay(1400);
  Forward();
}

void UTurn()
{
  Right();
  delay(2800);
  Forward();
}

void loop()
{
  long duration, Frontdistance;
  digitalWrite(trigPin, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(trigPin, HIGH);
  duration = pulseIn(echoPin, HIGH);
  Frontdistance = (duration/58.2);
  delay(50);
  

  long duration1, Leftdistance;
  digitalWrite(trigPin1, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(trigPin1, HIGH);
  duration1 = pulseIn(echoPin1, HIGH);
  Leftdistance = (duration1/58.2);
  delay(50);

  long duration2, Rightdistance;
  digitalWrite(trigPin2, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(trigPin2, HIGH);
  duration2 = pulseIn(echoPin2, HIGH);
  Rightdistance = (duration2/58.2);
  Serial.print("RighttDistance: ");
  Serial.println(Rightdistance);
  delay(50);
  
  int b=0;
    if (Serial.available()>0){
      b=Serial.read();
      if(b=310){
        Z=300;
      }
      if(b=410){
      Z=400;   
      }
  }


  if((Frontdistance < 8 && Rightdistance < 8 && Leftdistance < 8)) //1 x - x - x 
  {
    UTurn();
  }

  if((Frontdistance < 8 && Rightdistance > 8 && Leftdistance < 8))//2 x - o - x
  {
    RightTurn();
  }

  if((Frontdistance < 8 && Rightdistance < 8 && Leftdistance > 8))//3 x - x - 0
  {
   LeftTurn();
  }

  if((Frontdistance > 8 && Rightdistance > 8 && Leftdistance > 8)) //4 o - o - o
  {
   Forward();
  }

  if((Frontdistance > 8 && Rightdistance < 8 && Leftdistance < 8)) //5 o - x - x
  {
   Forward();
  }

  if((Frontdistance < 8 && Rightdistance > 8 && Leftdistance < 8)) //6 o - x - o
  {
   Forward();
  }

  if((Frontdistance > 8 && Rightdistance > 8 && Leftdistance < 8)) //7 o - o - x
  {
   Forward();
  }

  if((Frontdistance < 8 && Rightdistance < 8 && Leftdistance > 8)) //8 x - x - o
  {
   Forward();
  }

  if (Z=300)
  {
    RightTurn();
    }
    
  if (Z=400){
    LeftTurn();
    }
}