/*
   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 = 200;           // interval at which to blink (milliseconds)
unsigned long currentMillis;
unsigned long currentMillis1;


#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();
  // 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(200);
    right();
    delay(200);
  }

  // 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(200);
    left();
    delay(200);
  }

  // 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(200);
    left();
    delay(200);
  }
  //--------------------------------------------------------------------------------------------------------
}//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, 130); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 130); // 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, 130); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 130); // 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, 200); // Setting speed of Motors on the LEFT
  analogWrite(ENB, 200); // Setting speed of Motors on the RIGH
}
void fight() {


  if (distance <= maximumRange) {
    FRONTattack();
    //delay(500);
  }

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

}//end of fight function
void FRONTattack() {
  currentMillis1 = millis();
  if (currentMillis1 - previousMillis1 >= interval) {
    previousMillis1 = currentMillis1;
    analogWrite(ENA, 220);
    analogWrite(ENB, 220);
    digitalWrite(IN1, 0);
    digitalWrite(IN2, 1);
    digitalWrite(IN3, 0);
    digitalWrite(IN4, 1);
  }
}
void Seaching()
{
  currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    right();
  }

}

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