/*
   Connections:
   --------------------------------------
   sensor1Pin ---> Pin 3  on Arduino Uno
   sensor2Pin ---> Pin 4  on Arduino Uno
   echoPin    ---> Pin 11 on Arduino Uno
   trigPin    ---> Pin 12 on Arduino Uno
   LEDPin     ---> Pin 13 on Arduino Uno
   IN1        ---> Pin 5  on Arduino Uno
   IN2        ---> Pin 6  on Arduino Uno
   IN3        ---> Pin 7  on Arduino Uno
   IN4        ---> Pin 8  on Arduino Uno
   ENA        ---> Pin 9  on Arduino Uno
   ENB        ---> Pin 10 on Arduino Uno
   ---------------------------------------
*/

//**************************************************\\

//----------------------------------------------------------[ Pins and Global Variables ]
unsigned long previousMillis = 0;        // will store last time LED was updated
unsigned long previousMillis1 = 0;
// constants won't change:
const long interval = 400;           // interval at which to blink (milliseconds)

bool frontFlag = 0;
bool rightFlag = 0;

#define sensor1Pin 2      // 1st Infra-Red sensor pin on the (RIGHT) <---- Very Important
int sensor1State = 0;     // variable for reading sensor 1 status

#define sensor2Pin 3      // 2nd Infra-Red sensor pin on the (LEFT)  <---- Very Important
int sensor2State = 0;     // variable for reading sensor 2 status

// Motor Driver Board Pins
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENA = 11;
int ENB = 10;

#define echoPin A1        // Echo Pin
#define trigPin A0        // Trigger Pin

int maximumRange = 25;    // Maximum range to start attacking

long duration, distance;  // Variables to calculate distance


//-------------------------------------------------------/End of [ Pins and Global Variables ]

//**************************************************\\

//-------------------------------------------------------------[ Setup Function ]

void setup() {
  Serial.begin(9600);
  // Delay of 5000 milli-seconds = 5 seconds
  delay(5000);
  // Declare motor driver pins as OUTPUT
  for (int i = 6; i < 11; i ++)
  {
    pinMode(i, OUTPUT);
  }
  // Declare Ultrasonic sensor pins as OUTPUT and INPUT
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  // Declare Infra-Red Sensor pins as INPUT
  pinMode(sensor1Pin, INPUT);
  pinMode(sensor2Pin, INPUT);

  // Moving car forward to initiate attack
  front(120);
  // This delay controls the speed of the car when going in any direction above this delay
  delay(300);

}

//---------------------------------------------------------------------------------------------------------/End of [ Setup Function ]
//**************************************************\\

//--------------------------------------------------------------------------------------------------------[ Loop Function ]

void loop() {

  // Reading Infra-Red Sensors and saving their status to corresponding variables
  sensor1State = digitalRead(sensor1Pin);
  sensor2State = digitalRead(sensor2Pin);
  // Triggering the Ultrasonic Wave :
  digitalWrite(trigPin, LOW);//     |
  delayMicroseconds(2);//           |
  digitalWrite(trigPin, HIGH);//    |
  delayMicroseconds(10);//          |
  digitalWrite(trigPin, LOW);//     |
  //---------------------------------

  //Sensing Ultrasonic Wave on echo Pin and callculating the duration
  duration = pulseIn(echoPin, HIGH);

  //Calculate the distance (in cm) based on the speed of sound.
  distance = duration / 58.2;
  //Serial.println(distance);
  // If object in range of attack start Attacking:
  // Conrolling car direction based on Infra-Red Sensors:
  //--------------------------------------------------------------------------------------------------------

  // If [RIGHT] sensor is seeing [Black] and [LEFT] sensor is seeing [Black] -> [FIGHT]
  if (sensor1State == 1 && sensor2State == 1) {
    fight();
  }

  // If [RIGHT] sensor is seeing [Black] and [LEFT] sensor is seeing [White] -> [move Back and to the Right]
  else if (sensor1State == 1 && sensor2State == 0) {
    back();
    delay(400);
    right();
    delay(400);
    frontFlag = 0;
    rightFlag = 0;
  }

  // If [RIGHT] sensor is seeing [White] and [LEFT] sensor is seeing [Black] -> [move Back and to the Left]
  else if (sensor1State == 0 && sensor2State == 1) {

    back();
    delay(400);
    left();
    delay(400);
    frontFlag = 0;
    rightFlag = 0;
  }

  // If [RIGHT] sensor is seeing [White] and [LEFT] sensor is seeing [White] -> [move Back and to the Right]
  else if (sensor1State == 0 && sensor2State == 0) {
    back();
    delay(400);
    left();
    delay(400);
    frontFlag = 0;
    rightFlag = 0;
  }
  //--------------------------------------------------------------------------------------------------------
}//end of loop

//---------------------------------------------------------------------------------------------------------/End of [ Loop Function ]

//**************************************************\\

//--------------------------------------------------------------------------------------------------------[ Car Directions Functions ]

// Stopping [RIGH] motors and [LEFT] motors
void stopM() {
  analogWrite(ENA, 0); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 0); // Setting speed of Motors on the RIGH
}

// Moving [RIGH] motors [forward] and [LEFT] motors [forward]
void front(int s) {
  analogWrite(ENA, s); // Setting speed of Motors on the LEFT
  analogWrite(ENB, s); // Setting speed of Motors on the RIGH
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
}

// Moving [RIGH] motors [backwards] and [LEFT] motors [backwards]
void back() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
  analogWrite(ENA, 250); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 250); // Setting speed of Motors on the RIGH
}

// Moving [LEFT] motors [forward] and [RIGHT] motors [backwards]
void right() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
  analogWrite(ENA, 250); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 250); // Setting speed of Motors on the RIGH
}

// Moving [RIGH] motors [forward] and [LEFT] motors [backwards]
void left() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
  analogWrite(ENA, 250); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 250); // Setting speed of Motors on the RIGH
}
void FRONTattack() {

  if (millis() - previousMillis1 >= 500) {
    previousMillis1 = millis();
    front(250);
  }
}
void Seaching()
{
  if (millis() - previousMillis >= interval && frontFlag == 0 && rightFlag == 0) {
    Serial.println(millis() - previousMillis);
    previousMillis = millis();
    front(200);
    frontFlag = 1;
    Serial.println("front ------ frontFlag = 1");

  }
  else if (millis() - previousMillis >= interval && frontFlag == 1 && rightFlag == 0) {
    Serial.println(millis() - previousMillis);
    previousMillis = millis();
    right();
    rightFlag = 1;
    Serial.println("right ------ rightFlag = 1");

  }
  else if (millis() - previousMillis >= interval && frontFlag == 1 && rightFlag == 1) {
    Serial.println(millis() - previousMillis);
    previousMillis = millis();
    right();
    frontFlag = 0;
    rightFlag = 0;
    Serial.println("left ------ Flags = 00");

  }
}

void fight() {
  if (distance <= maximumRange) {
    FRONTattack();
    frontFlag = 0;
    rightFlag = 0;
    //delay(500);
  }

  // If object is not in range of attack start Searching:
  else {
    Seaching();
  }

}//end of fight function


//**************************************************\\