#define m1 5  // Right Motor MA1
#define m2 4  // Right Motor MA2
#define m3 2  // Left Motor MB1
#define m4 3  // Left Motor MB2
#define e1 11 // Right Motor Enable Pin EA
#define e2 10 // Left Motor Enable Pin EB

//**********5 Channel IR Sensor Connection**********//
#define ir1 A1
#define ir2 A2
#define ir3 A3
#define ir4 A4
#define ir5 A5
//*************************************************//

int speed = 150;

void setup()
{
  pinMode(m1, OUTPUT);
  pinMode(m2, OUTPUT);
  pinMode(m3, OUTPUT);
  pinMode(m4, OUTPUT);
  pinMode(e1, OUTPUT);
  pinMode(e2, OUTPUT);

  pinMode(ir1, INPUT);
  pinMode(ir2, INPUT);
  pinMode(ir3, INPUT);
  pinMode(ir4, INPUT);
  pinMode(ir5, INPUT);

  Serial.begin(9600);
  Serial.println("Start");
  delay(1000);
}

void loop()
{

  // CarTest();

  // //Reading Sensor Values
  int s1 = digitalRead(ir1); // Left Most Sensor
  int s2 = digitalRead(ir2); // Left Sensor
  int s3 = digitalRead(ir3); // Middle Sensor
  int s4 = digitalRead(ir4); // Right Sensor
  int s5 = digitalRead(ir5); // Right Most Sensor

  Serial.print(s1);
  Serial.print(" ");
  Serial.print(s2);
  Serial.print(" ");
  Serial.print(s3);
  Serial.print(" ");
  Serial.print(s4);
  Serial.print(" ");
  Serial.print(s5);
  Serial.print(" ");

  // delay(500);

  // print Froward , Right , Left in the serial monitor
  if((s1 == 1) && (s2 == 1) && (s3 == 0) && (s4 == 1) && (s5 == 1))
  {
    Serial.println("Forward");
  }
  if((s1 == 1) && (s2 == 0) && (s3 == 0) && (s4 == 0) && (s5 == 1))
  {
    Serial.println("Forward");
  }

  else if((s1 == 1) && (s2 == 0) && (s3 == 1) && (s4 == 1) && (s5 == 1))
  {
    Serial.println("Right");
  }
  else if((s1 == 0) && (s2 == 1 && (s3 == 1) && (s4 == 1) && (s5 == 1)))
  {
    Serial.println("Right");
  }
  else if((s1 == 0) && (s2 == 0) && (s3 == 1) && (s4 == 1) && (s5 == 1))
  {
    Serial.println("Right");
  }
  else if ((s1 == 1) && (s2 == 0) && (s3 == 0) && (s4 == 1) && (s5 == 1))
  {
    Serial.println("Right");
  }
  
  else if((s1 == 1) && (s2 == 1) && (s3 == 1) && (s4 == 0) && (s5 == 1))
  {
    Serial.println("Left");
  }
  else if((s1 == 1) && (s2 == 1) && (s3 == 1) && (s4 == 1) && (s5 == 0))
  {
    Serial.println("Left");
  }
  else if((s1 == 1) && (s2 == 1) && (s3 == 1) && (s4 == 0) && (s5 == 0))
  {
    Serial.println("Left");
  }
  else if((s1 == 1) && (s2 == 1) && (s3 == 0) && (s4 == 0) && (s5 == 1))
  {
    Serial.println("Left");
  }
  
  else if((s1 == 1) && (s2 == 1) && (s3 == 1) && (s4 == 0) && (s5 == 1))
  {
    Serial.println("On All White");
  }
  else
  {
    Serial.println("On All Black");
  }

  delay(500);

  // //if only middle sensor detects black line
  // if((s1 == 1) && (s2 == 1) && (s3 == 0) && (s4 == 1) && (s5 == 1))
  // {
  //   //going forward with full speed
  //   Forward(speed);
  // }

  // //if only left sensor detects black line
  // if((s1 == 1) && (s2 == 0) && (s3 == 1) && (s4 == 1) && (s5 == 1))
  // {
  //   //going right with full speed
  //   Right(speed);
  // }

  // //if only left most sensor detects black line
  // if((s1 == 0) && (s2 == 1) && (s3 == 1) && (s4 == 1) && (s5 == 1))
  // {
  //   //going right with full speed
  //   Right(speed);
  // }

  // //if only right sensor detects black line
  // if((s1 == 1) && (s2 == 1) && (s3 == 1) && (s4 == 0) && (s5 == 1))
  // {
  //   //going left with full speed
  //   Left(speed);
  // }

  // //if only right most sensor detects black line
  // if((s1 == 1) && (s2 == 1) && (s3 == 1) && (s4 == 1) && (s5 == 0))
  // {
  //   //going left with full speed
  //   Left(speed);
  // }

  // //if middle and right sensor detects black line
  // if((s1 == 1) && (s2 == 1) && (s3 == 0) && (s4 == 0) && (s5 == 1))
  // {
  //   //going left with full speed
  //   Left(speed);
  // }

  // //if middle and left sensor detects black line
  // if((s1 == 1) && (s2 == 0) && (s3 == 0) && (s4 == 1) && (s5 == 1))
  // {
  //   //going right with full speed
  //   Right(speed);
  // }

  // //if middle, left and left most sensor detects black line
  // if((s1 == 0) && (s2 == 0) && (s3 == 0) && (s4 == 1) && (s5 == 1))
  // {
  //   //going right with full speed
  //   Right(speed);
  // }

  // //if middle, right and right most sensor detects black line
  // if((s1 == 1) && (s2 == 1) && (s3 == 0) && (s4 == 0) && (s5 == 0))
  // {
  //   //going left with full speed
  //   Left(speed);
  // }

  // //if all sensors are on a black line
  // if((s1 == 0) && (s2 == 0) && (s3 == 0) && (s4 == 0) && (s5 == 0))
  // {
  //   //stop
  //   Stop();
  // }

  // delay(100);
}

void Forward(int speed)
{
  analogWrite(e1, speed); // you can adjust the speed of the motors from 0-255
  analogWrite(e2, speed); // you can adjust the speed of the motors from 0-255

  digitalWrite(m1, HIGH);
  digitalWrite(m2, LOW);
  digitalWrite(m3, HIGH);
  digitalWrite(m4, LOW);
}

void Backward(int speed)
{
  analogWrite(e1, speed); // you can adjust the speed of the motors from 0-255
  analogWrite(e2, speed); // you can adjust the speed of the motors from 0-255

  digitalWrite(m1, LOW);
  digitalWrite(m2, HIGH);
  digitalWrite(m3, LOW);
  digitalWrite(m4, HIGH);
}

void Right(int speed)
{
  analogWrite(e1, speed); // you can adjust the speed of the motors from 0-255
  analogWrite(e2, speed); // you can adjust the speed of the motors from 0-255

  digitalWrite(m1, HIGH);
  digitalWrite(m2, LOW);
  digitalWrite(m3, LOW);
  digitalWrite(m4, LOW);
}

void Left(int speed)
{
  analogWrite(e1, speed); // you can adjust the speed of the motors from 0-255
  analogWrite(e2, speed); // you can adjust the speed of the motors from 0-255

  digitalWrite(m1, LOW);
  digitalWrite(m2, LOW);
  digitalWrite(m3, HIGH);
  digitalWrite(m4, LOW);
}

void Stop()
{
  digitalWrite(m1, LOW);
  digitalWrite(m2, LOW);
  digitalWrite(m3, LOW);
  digitalWrite(m4, LOW);
}

void CarTest()
{
  Forward(150);
  delay(1000);

  Backward(150);
  delay(1000);

  Right(150);
  delay(1000);

  Left(150);
  delay(1000);

  Stop();
  delay(1000);
}