/*
   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 ]

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

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

#define echoPin 11        // Echo Pin
#define trigPin 12        // Trigger Pin

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

long duration, distance;  // Variables to calculate distance


#define LEDPin 13         // Onboard LED

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

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

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

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

void setup() {

  // Delay of 5000 milli-seconds = 5 seconds
  delay(5000);

  // Declare motor driver pins as OUTPUT
  for (int i = 5; 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);

  // Declare internal LED on pin 13 as OUTPUT
  pinMode(LEDPin, OUTPUT);


  // Moving car forward to initiate attack
  front();

  // 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);

  // Conrolling car direction based on Infra-Red Sensors:
  //--------------------------------------------------------------------------------------------------------

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

  // If [RIGHT] sensor is seeing [Black] and [LEFT] sensor is seeing [White] -> [move Back and to the Right]
  else if (sensor1State == HIGH && sensor2State == LOW) {
    digitalWrite(LEDPin, LOW);
    back();
    delay(200);
    right();
    delay(500);
  }

  // If [RIGHT] sensor is seeing [White] and [LEFT] sensor is seeing [Black] -> [move Back and to the Left]
  else if (sensor1State == LOW && sensor2State == HIGH) {
    digitalWrite(LEDPin, LOW);
    back();
    delay(200);
    left();
    delay(500);
  }

  // If [RIGHT] sensor is seeing [White] and [LEFT] sensor is seeing [White] -> [move Back and to the Right]
  else if (sensor1State == LOW && sensor2State == LOW) {
    digitalWrite(LEDPin, LOW);
    back();
    delay(500);
    right();
    delay(500);
  }
  //--------------------------------------------------------------------------------------------------------
}//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() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
  analogWrite(ENA, 150); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 150); // Setting speed of Motors on the RIGH
}

// Moving [RIGH] motors [backwards] and [LEFT] motors [backwards]
void back() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
  analogWrite(ENA, 150); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 150); // 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, 255); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 255); // 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, 255); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 255); // Setting speed of Motors on the RIGH
}

//---------------------------------------------------------------------------------------------------------/End of [ Car Directions Functions ]

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

//--------------------------------------------------------------------------------------------------------[ Attack Function ]

void fight() {

  // 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;

  // If object in range of attack start Attacking:
  if (distance <= maximumRange) {
    digitalWrite(LEDPin, HIGH);
    front();
    delay(200);
  }

  // If object is not in range of attack start Searching:
  else {
    digitalWrite(LEDPin, LOW);
    right();
    delay(500);
  }

}//end of fight function

//---------------------------------------------------------------------------------------------------------/End of [ Attack Function ]

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