int s1 = 14, s2 = 15, s3 = 16, s4 = 17, s5 = 18, clp = 19, left, right, left_most, right_most, center, bump;
int right_motor_1 = 4, right_motor_2 = 7, left_motor_1 = 12, left_motor_2 = 8;
int right_speed = 3, left_speed = 5;
const int optSpeed = 100, sharp = 80;
int flagLast = 0;
void setup() {
  // put your setup code here, to run once:
  pinMode(s1, INPUT);
  pinMode(s2, INPUT);
  pinMode(s3, INPUT);
  pinMode(s4, INPUT);
  pinMode(s5, INPUT);
  pinMode(clp, INPUT);

  pinMode(right_motor_1, OUTPUT);
  pinMode(right_motor_2, OUTPUT);
  pinMode(right_speed, OUTPUT);
  pinMode(left_motor_1, OUTPUT);
  pinMode(left_motor_2, OUTPUT);
  pinMode(left_speed, OUTPUT);
  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:
  left_most = digitalRead(s1);
  left = digitalRead(s2);
  center = digitalRead(s3);
  right = digitalRead(s4);
  right_most = digitalRead(s5);
  bump = digitalRead(clp);

  if (bump == 1 ) {
    stopRun();
  }
  else if (left == 0 || left_most == 0) {
    move_left();
  }
  else if (right == 0 || right_most == 0) {
    move_right();
  }
  else if (center == 0 ) {
    forward();
  }
  else if ((left == 1 && right == 1 && left_most == 1 && right_most == 1 && center == 1)) {

    revese();

    delay(200);
  } else {
    revese();
    delay(200);
  }

}

void stopRun() {
  stop_();
  Serial.println("stop");
}

void forward() {
  analogWrite(right_speed, optSpeed);
  analogWrite(left_speed, optSpeed);
  digitalWrite(right_motor_1, HIGH);
  digitalWrite(right_motor_2, LOW);
  digitalWrite(left_motor_1, HIGH);
  digitalWrite(left_motor_2, LOW);
  Serial.println("going forward");
}
void move_left_sharp() {
  analogWrite(right_speed, optSpeed);
  analogWrite(left_speed, optSpeed);
  digitalWrite(right_motor_1, HIGH);
  digitalWrite(right_motor_2, LOW);

  digitalWrite(left_motor_1, LOW);
  digitalWrite(left_motor_2, HIGH);
  Serial.println("taking sharp left");
}
void move_right_sharp() {
  analogWrite(right_speed, optSpeed);
  analogWrite(left_speed, optSpeed);
  digitalWrite(right_motor_1, LOW);
  digitalWrite(right_motor_2, HIGH);
  digitalWrite(left_motor_1, HIGH);
  digitalWrite(left_motor_2, LOW);
  Serial.println("taking sharp right");
}

void move_left() {
  analogWrite(right_speed, sharp);
  analogWrite(left_speed, optSpeed);
  digitalWrite(right_motor_1, HIGH);
  digitalWrite(right_motor_2, LOW);

  digitalWrite(left_motor_1, LOW);
  digitalWrite(left_motor_2, LOW);
  Serial.println("taking left");
}
void move_right() {
  analogWrite(right_speed, optSpeed);
  analogWrite(left_speed, sharp);
  digitalWrite(right_motor_1, LOW);
  digitalWrite(right_motor_2, LOW);
  digitalWrite(left_motor_1, HIGH);
  digitalWrite(left_motor_2, LOW);
  Serial.println("taking right");
}
void stop_() {
  analogWrite(right_speed, optSpeed);
  analogWrite(left_speed, optSpeed);
  digitalWrite(right_motor_1, LOW);
  digitalWrite(right_motor_2, LOW);
  digitalWrite(left_motor_1, LOW);
  digitalWrite(left_motor_2, LOW);

}

void revese() {
  digitalWrite(right_motor_1, LOW);
  digitalWrite(right_motor_2, HIGH);
  digitalWrite(left_motor_1, LOW);
  digitalWrite(left_motor_2, HIGH);

}