/********************************************************
  Connections:
  -------------------------------------------------------
  [Arduino Uno] --> [L298N Red Board]:
     9          -->   ENA
     6          -->   IN1
     7          -->   IN2
     4          -->   IN3
     5          -->   IN4
     10         -->   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] --> [Ultrasonic Sensor HC-SR04]:
     GND        -->   GND
     5V         -->   VCC
     A3         -->   Trig
     A2         -->   Echo
  [Arduino Uno] --> [LCD 16x2 I2C]:
     GND        -->   GND
     5V         -->   VCC
     SDA        -->   SDA
     SCL        -->   SCL
  -------------------------------------------------------
*********************************************************/

#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

int maxDistance = 10; // 10cm
#define tr A3
#define ec A2
long duration;
int distance;
int counter = 0;

#define SensorLeft    2
#define SensorMiddle  8
#define SensorRight   3
unsigned char SL;
unsigned char SM;
unsigned char SR;

int mSpeed = 90; //0-255
int pinLB = 5;//IN4
int pinLF = 4;//IN3
int pinRB = 7;//IN2
int pinRF = 6;//IN1
int ENA   = 9;
int ENB   = 10;

void IO_config(void)
{
  pinMode(tr, OUTPUT);
  pinMode(ec, INPUT);
  pinMode(SensorLeft, INPUT);
  pinMode(SensorMiddle, INPUT);
  pinMode(SensorRight, INPUT);
  pinMode(pinLB, OUTPUT);
  pinMode(pinLF, OUTPUT);
  pinMode(pinRB, OUTPUT);
  pinMode(pinRF, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  analogWrite(ENA, mSpeed); //0-255
  analogWrite(ENB, mSpeed); //0-255
}

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);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Hello, world!");
  IO_config();
  stopp();
  delay(5000);// milli Second
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Counter:");
  lcd.setCursor(0, 1);
  lcd.print(counter);
}

void Sensor_Scan(void)
{
  SL = digitalRead(SensorLeft);
  SM = digitalRead(SensorMiddle);
  SR = digitalRead(SensorRight);
  Serial.print(SL);
  Serial.print(SM);
  Serial.println(SR);
}

void loop()
{
  Sensor_Scan();
  US();
  showDataOnLCD();
  if (distance > maxDistance && counter < 7) {
    ///////////////////////////////////////////////////////////////////////////////
    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 if (SR == 0 & SL == 0) {// all sensors BLACK
        counter = counter + 1;
        if (counter == 7) {
          stopp() ;
        }
        else {
          advance();
          delay(200);
        }
      }
      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 or counter is equal to 7, stop
      {
        stopp() ;
        delay(100);
      }
    }
    ///////////////////////////////////////////////////////////////////////////////
  }

  else {
    stopp();
    delay(2000);
  }

}//end of LOOP


void US() {
  digitalWrite(tr, LOW);
  delayMicroseconds(5);
  digitalWrite(tr, HIGH);
  delayMicroseconds(10);
  digitalWrite(tr, LOW);
  duration = pulseIn(ec, HIGH);
  distance = duration * 0.034 / 2;
  Serial.print("Distance = ");
  Serial.print(distance);
  Serial.println(" cm");
}

void showDataOnLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Counter: ");
  lcd.print(counter);
  //lcd.setCursor(0, 1);
  //lcd.print("Distance: ");
  //lcd.print(distance);
  //lcd.print(" cm");
}