#include<NewPing.h>
#include<AFMotor.h>
#include<MFRC522.h>
#include <LiquidCrystal_I2C.h>
#include<SPI.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);
#define RIGHT A2
#define LEFT A3
#define CENTRE A4
#define TRIGGER_PIN A1
#define ECHO_PIN A0
#define MAX_DISTANCE 100
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
AF_DCMotor Motor1(1,MOTOR12_1KHZ);
AF_DCMotor Motor2(2,MOTOR12_1KHZ);
AF_DCMotor Motor3(3,MOTOR34_1KHZ);
AF_DCMotor Motor4(4,MOTOR34_1KHZ);
MFRC522 mfrc522(53,5);


void setup() {
SPI.begin();  
Serial.begin(9600);
pinMode(RIGHT, INPUT);
pinMode(LEFT, INPUT);
pinMode(CENTRE, INPUT);
mfrc522.PCD_Init();
lcd.init();
lcd.backlight();
lcd.clear();
lcd.setCursor(1,0);
lcd.print("WELCOME TO ALF");
lcd.setCursor(3,1);
lcd.print("PROCEEDING");
delay(2000);
}


void loop() {
delay(50);
unsigned int distance = sonar.ping_cm();
Serial.print("distance");
Serial.println(distance);
int Right_Value = digitalRead(RIGHT);
int Left_Value = digitalRead(LEFT);
int Centre_Value = digitalRead(CENTRE);
Serial.print("RIGHT");
Serial.println(Right_Value);
Serial.print("LEFT");
Serial.println(Left_Value);
Serial.print("CENTRE");
Serial.println(Centre_Value);

if((Right_Value==0) && (distance>=5) && (Left_Value==0) && (Centre_Value==1) ){
  Motor1.setSpeed(70);
  Motor1.run(FORWARD);
  Motor2.setSpeed(70);
  Motor2.run(FORWARD);
  Motor3.setSpeed(70);
  Motor3.run(FORWARD);
  Motor4.setSpeed(70);
  Motor4.run(FORWARD);
}else if((Right_Value==0) && (Left_Value==1) && (Centre_Value==1)) {
  Motor1.setSpeed(120);
  Motor1.run(FORWARD);
  Motor2.setSpeed(120);
  Motor2.run(FORWARD);
  Motor3.setSpeed(100);
  Motor3.run(BACKWARD);
  Motor4.setSpeed(100);
  Motor4.run(BACKWARD);
}else if((Right_Value==1)&&(Left_Value==0) && (Centre_Value==1)) {
  Motor1.setSpeed(100);
  Motor1.run(BACKWARD);
  Motor2.setSpeed(100);
  Motor2.run(BACKWARD);
  Motor3.setSpeed(120);
  Motor3.run(FORWARD);
  Motor4.setSpeed(120);
  Motor4.run(FORWARD);
}else if((Right_Value==1)&&(Left_Value==1) && (Centre_Value==1)) {
  Motor1.setSpeed(0);
  Motor1.run(RELEASE);
  Motor2.setSpeed(0);
  Motor2.run(RELEASE);
  Motor3.setSpeed(0);
  Motor3.run(RELEASE);
  Motor4.setSpeed(0);
  Motor4.run(RELEASE);
}else if(distance > 1 && distance < 10) {
  Motor1.setSpeed(0);
  Motor1.run(RELEASE);
  Motor2.setSpeed(0);
  Motor2.run(RELEASE);
  Motor3.setSpeed(0);
  Motor3.run(RELEASE);
  Motor4.setSpeed(0);
  Motor4.run(RELEASE);
  }
  if(!mfrc522.PICC_IsNewCardPresent())
  {
    return;
  }
  if(!mfrc522.PICC_ReadCardSerial())
  {
    return;
  }
  String uid="";
  Serial.println();
  Serial.print("UID=");
  for(int i=0;i<mfrc522.uid.size;i++)
  {
    Serial.print(mfrc522.uid.uidByte[i]<0x10 ? "0" : "");
    Serial.print(mfrc522.uid.uidByte[i],HEX);
    uid.concat(String(mfrc522.uid.uidByte[i]<0x10 ? "0" : ""));
    uid.concat(String(mfrc522.uid.uidByte[i],HEX));
  }
uid.toUpperCase(); 
String tagUID1 = "03C4DD0A"; 
String tagUID3 = "5066960B";
String tagUID5 = "404A6C0B";
  if(uid == tagUID1){
  {Motor1.setSpeed(0);
  Motor1.run(RELEASE);
  Motor2.setSpeed(0);
  Motor2.run(RELEASE);
  Motor3.setSpeed(0);
  Motor3.run(RELEASE);
  Motor4.setSpeed(0);
  Motor4.run(RELEASE);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("REACHED MD CABIN");
  delay(3000);
  lcd.clear();};
  Motor1.setSpeed(70);
  Motor1.run(FORWARD);
  Motor2.setSpeed(70);
  Motor2.run(FORWARD);
  Motor3.setSpeed(70);
  Motor3.run(FORWARD);
  Motor4.setSpeed(70);
  Motor4.run(FORWARD);
 }
  else if(uid == tagUID3){
  {Motor1.setSpeed(0);
  Motor1.run(RELEASE);
  Motor2.setSpeed(0);
  Motor2.run(RELEASE);
  Motor3.setSpeed(0);
  Motor3.run(RELEASE);
  Motor4.setSpeed(0);
  Motor4.run(RELEASE);
  lcd.clear();
  lcd.setCursor(5,0);
  lcd.print("REACHED");
  lcd.setCursor(4,1);
  lcd.print("RECEPTION");
  delay(3000);
  lcd.clear();};
  Motor1.setSpeed(70);
  Motor1.run(FORWARD);
  Motor2.setSpeed(70);
  Motor2.run(FORWARD);
  Motor3.setSpeed(70);
  Motor3.run(FORWARD);
  Motor4.setSpeed(70);
  Motor4.run(FORWARD);
 }
  else if(uid == tagUID5){
  {Motor1.setSpeed(0);
  Motor1.run(RELEASE);
  Motor2.setSpeed(0);
  Motor2.run(RELEASE);
  Motor3.setSpeed(0);
  Motor3.run(RELEASE);
  Motor4.setSpeed(0);
  Motor4.run(RELEASE);
  lcd.clear();
  lcd.setCursor(5,0);
  lcd.print("REACHED");
  lcd.setCursor(3,1);
  lcd.print("OFFICE AREA");
  delay(3000);
  lcd.clear();};
  Motor1.setSpeed(70);
  Motor1.run(FORWARD);
  Motor2.setSpeed(70);
  Motor2.run(FORWARD);
  Motor3.setSpeed(70);
  Motor3.run(FORWARD);
  Motor4.setSpeed(70);
  Motor4.run(FORWARD);
 }
}