/*
***************************
  Connections:
  -----------------------------
  [Arduino Uno] --> [L298N Red Board]:
     5V         -->   ENA
     4          -->   IN1
     5          -->   IN2
     6          -->   IN3
     7          -->   IN4
     5V         -->   ENB
  [Arduino Uno] --> [IR Sensor (Left)]:
     GND        -->   GND
     5V         -->   VCC
     2          -->   OUT
  [Arduino Uno] --> [IR Sensor (Center)]:
     GND        -->   GND
     5V         -->   VCC
     8          -->   OUT
  [Arduino Uno] --> [IR Sensor (Right)]:
     GND        -->   GND
     5V         -->   VCC
     3          -->   OUT
  [Arduino Uno] --> [IR Sensor (Front)]:
     GND        -->   GND
     5V         -->   VCC
     9          -->   OUT
  -----------------------------
****************************
*/

#include <Servo.h>

Servo myservo;
int pos = 0;
#define SensorFront   9
#define SensorLeft    2   //input pin of left sensor
#define SensorMiddle  8   //input pin of middle sensor
#define SensorRight   3   //input pin of right sensor
unsigned char SL;        //state of left sensor
unsigned char SM;        //state of middle sensor
unsigned char SR;        //state of right sensor
unsigned char SF;

int pinLB = 7;        //IN4
int pinLF = 6;        //IN3
int pinRB = 5;        //IN2
int pinRF = 4;        //IN1

void M_Control_IO_config(void)
{
  pinMode(SensorFront, INPUT);
  pinMode(SensorLeft, INPUT);
  pinMode(SensorMiddle, INPUT);
  pinMode(SensorRight, INPUT);
  pinMode(pinLB, OUTPUT);
  pinMode(pinLF, OUTPUT);
  pinMode(pinRB, OUTPUT);
  pinMode(pinRF, OUTPUT);
}

void turnR()
{
  digitalWrite(pinRB, LOW);
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, LOW);
  digitalWrite(pinLF, HIGH);
  delay(10);
}
void back()
{
  digitalWrite(pinRB, LOW);
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, HIGH);
  digitalWrite(pinLF, LOW);
  delay(10);
}
void advance()
{
  digitalWrite(pinRB, HIGH);
  digitalWrite(pinRF, LOW );
  digitalWrite(pinLB, LOW);
  digitalWrite(pinLF, HIGH);
  delay(10);
}
void stopp()
{
  digitalWrite(pinRB, HIGH);
  digitalWrite(pinRF, HIGH);
  digitalWrite(pinLB, HIGH);
  digitalWrite(pinLF, HIGH);
  delay(10);
}
void turnL()
{
  digitalWrite(pinRB, HIGH);
  digitalWrite(pinRF, LOW);
  digitalWrite(pinLB, HIGH);
  digitalWrite(pinLF, LOW);
  delay(10);
}

void setup()
{
  Serial.begin(9600);
  myservo.attach(12);
  myservo.write(0);
  M_Control_IO_config();
  stopp();
  delay(5000);// milli Second
}

void Sensor_Scan(void)
{
  SF = digitalRead(SensorFront);// 0 / 1
  SL = digitalRead(SensorLeft);// 0 / 1
  SM = digitalRead(SensorMiddle);// 0 / 1
  SR = digitalRead(SensorRight);// 0 / 1
  Serial.print(SL);
  Serial.print(SM);
  Serial.println(SR);
  Serial.println(SF);
}

void loop()
{
  Sensor_Scan();

  if (SF == 1) {
    ///////////////////////////////////////////////////////////////////////////////
    if (SM == 0)// middle sensor in black area
    {
      if (SL == 1 & SR == 0) // black on left, white on right, turn left
      {
        turnR();
      }
      else if (SR == 1 & SL == 0) // white on left, black on right, turn right
      {
        turnL();
      }
      else // white on both sides, going forward
      {
        advance();
      }
    }
    ///////////////////////////////////////////////////////////////////////////////
    else // middle sensor on white area
    {
      if (SL == 1 & SR == 0) // black on left, white on right, turn left
      {
        turnR();

      }
      else if (SR == 1 & SL == 0) // white on left, black on right, turn right
      {
        turnL();


      }
      else // all white, stop
      {
        delay(100);
        stopp() ;
      }
    }
    ///////////////////////////////////////////////////////////////////////////////
  }

  else {
    stopp();
    myservo.write(90);
    delay(5000);
    myservo.write(0);
    delay(2000);
  }

}