#define L1 A5 // defining the left sensors and stating the corresponding pins
#define L2 A4
#define L3 3
#define R1 4 // defining the right sensors and stating corresponding pins
#define R2 5
#define R3 6
#define RM_0 10 // defining right motors
#define RM_1 8
#define LM_0 A0 // defining left motors
#define LM_1 12
#define PWM_0 11 // power of the motors
#define PWM_1 9

int s1l; // inputting all sensors
int s2l;
int s3l;
int s1r;
int s2r;
int s3r;

void setup()
{
    pinMode(L1, INPUT); // setting the sensors as inputs
    pinMode(L2, INPUT);
    pinMode(L3, INPUT);
    pinMode(R1, INPUT);
    pinMode(R2, INPUT);
    pinMode(R3, INPUT);
    pinMode(RM_0, OUTPUT); // setting the motors as outputs
    pinMode(RM_1, OUTPUT);
    pinMode(LM_0, OUTPUT);
    pinMode(LM_1, OUTPUT);
    Serial.begin(9600);
}

//ACTIONS
void botStop(){
    digitalWrite(LM_0, LOW); // sets the motors to the low so that it stops
    digitalWrite(LM_1, LOW);
    digitalWrite(RM_0, LOW);
    digitalWrite(RM_1, LOW);

    analogWrite(PWM_0, 0); // power of the motors to be 0 so that it does not move
    analogWrite(PWM_1, 0);
}
void botGo(){
    digitalWrite(LM_0, HIGH); // sets each motor to the desired value so that it can move forward
    digitalWrite(LM_1, LOW);
    digitalWrite(RM_0, HIGH);
    digitalWrite(RM_1, LOW);

    analogWrite(PWM_0, 100); // power of the motor to be at maximum for fastest speed
    analogWrite(PWM_1, 100);
}
void botRight(){
    digitalWrite(LM_0, LOW); // sets only the right motor ot high so it is able to turn right
    digitalWrite(LM_1, LOW);
    digitalWrite(RM_0, HIGH);
    digitalWrite(RM_1, LOW);

    analogWrite(PWM_0, 75); // desired power of the motor so that it can turn right
    analogWrite(PWM_1, 150);
}
void botLeft(){
    digitalWrite(LM_0, HIGH); // sets only the left motor to high so that it can turn left
    digitalWrite(LM_1, LOW);
    digitalWrite(RM_0, LOW);
    digitalWrite(RM_1, LOW);

    analogWrite(PWM_0, 150); // desired power of the motor so that it can turn left
    analogWrite(PWM_1, 75);
}
void bot360(){
      digitalWrite(LM_0, LOW);
      digitalWrite(LM_1, HIGH);
      digitalWrite(RM_0, HIGH);
      digitalWrite(RM_1, LOW);

      analogWrite(PWM_0, 100);
      analogWrite(PWM_1, 100);
      delay(3000);
}



void loop()
{
  s1l = digitalRead(L1); // reading the inputs
  s2l = digitalRead(L2);
  s3l = digitalRead(L3);
  s1r = digitalRead(R1);
  s2r = digitalRead(R2);
  s3r = digitalRead(R3);


//statements with description of what bot is seeing
//A STRAIGHT LINE AHEAD FROM LEFT TO RIGHT
 if(s1l == HIGH && s2l == HIGH && s3l == HIGH && s1r == HIGH && s2r == HIGH && s3r == HIGH)
  {
    botStop();
  }
//LINE IN THE DIRECTION OF TRAVEL (NO OBSTRUCTION)
  if(s1l == LOW && s2l == LOW && s3l == HIGH && s1r == HIGH && s2r == LOW && s3r == LOW)// forward
  {
    botGo();
  }
//A STRAIGHT LINE AHEAD FROM RIGHT TO CENTER
  if(s1l == LOW && s2l == LOW && s3l == HIGH && s1r == HIGH && s2r == HIGH && s3r == LOW)//soft right
  {
    botRight();
  }
//A STRAIGHT LINE AHEAD FROM LEFT TO CENTER
  if(s1l == LOW && s2l == HIGH & s3l == HIGH && s1r == HIGH && s2r == LOW && s3r == LOW) //soft left
  {
    botLeft();
  }

  if(s1l == HIGH && s2l == HIGH && s3l == HIGH && s1r == HIGH && s2r == LOW && s3r == LOW) //hard left
  {
    botLeft();
  }

  if(s1l == LOW && s2l == LOW && s3l == HIGH && s1r == HIGH && s2r == HIGH && s3r == HIGH && (millis() < 13000) && (millis() > 10000)) //hard right (first)
  {

    bot360();   //do 360 for 3 seconds

  }

    if(s1l == LOW && s2l == LOW && s3l == HIGH && s1r == HIGH && s2r == HIGH && s3r == HIGH && (millis() > 13000) && (millis() < 13000+1000)) //hard right (second)
  {
    botGo();  //continue after 360
  }

      if(s1l == LOW && s2l == LOW && s3l == HIGH && s1r == HIGH && s2r == HIGH && s3r == HIGH && (millis() > 13000+1000) && (millis() < 13000+10000)) //hard right (third)
  {
    botStop(); //5 second stop

  }


      if(s1l == LOW && s2l == LOW && s3l == HIGH && s1r == HIGH && s2r == HIGH && s3r == HIGH && (millis() > 13000+10000) && (millis() < 13000+11000)) //hard right (fourth)
  {
    botGo();  //continue after stop
  }

  if(s1l == HIGH && s2l == HIGH && s3l == HIGH && s1r == HIGH && s2r == HIGH && s3r == HIGH && (millis() < 10000)) //stop, instead turn left
  {
    botLeft();
  }
    if(s1l == HIGH && s2l == HIGH && s3l == HIGH && s1r == HIGH && s2r == HIGH && s3r == HIGH && (millis() > 10000)) //stop, this time stop
  {
    botStop();
  }

}