//For millis function:
//Updating the millis for two event
unsigned long previousMillis1 = 0;
unsigned long previousMillis2 = 0;
const unsigned long event_intervalR = 400; //The time to move right (in milliseconds)
const unsigned long event_intervalF = 700; //The time to move front (in milliseconds)

//motors:
int IN1 = 4;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENA = 5;
int ENB = 6;

//IR sensor:
#define senRPin 3
int senRState = 0;

#define senLPin 2
int senLState = 0;

#define senBPin 10
int senBState = 0;

// needed in searching function
bool frontFlag = 0;
bool rightFlag = 0;

//Ultasonic sensor:
#define echoPin A0
#define trigPin A1
int maximumRange = 90; //in cm
//For calculating the distance to any object in front of the robot
long duration, distance;

void setup() {
  Serial.begin(9600);

  //motor
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, INPUT);
  pinMode(ENB, INPUT);
  //-------------------------------------------------------------
  //IR sensor
  pinMode(senRPin, INPUT);
  pinMode(senLPin, INPUT);
  pinMode(senBPin, INPUT);
  //-------------------------------------------------------------
  //Ultasonic sensor:
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  //The robot will stop 5 sec befor moving
  stopM();
  delay(5000);
  //Start moving front to initiate attack
  front(70);
}

void loop() {
  //IR sensor
  //Reading the ir sensors states
  senRState = digitalRead(senRPin);
  senLState = digitalRead(senLPin);
  senBState = digitalRead(senBPin);
  Serial.print(senBState);
  Serial.print(senLState);
  Serial.println(senRState);

  //Ultra sensor
  // Triggering the Ultrasonic Wave :
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  //Sensing Ultrasonic Wave on echo Pin and calculating the duration
  duration = pulseIn(echoPin, HIGH);
  distance = duration / 58.2;
  Serial.println(distance);

  //Check IR Sensor
  //If right&left sensor is seeing [White] -> [FIGHT]
  if (senRState == 0 && senLState == 0 && senBState == 0) {
    fight();
  }
  //If right sensor is seeing [Black], left sensor is seeing [White] -> [move Back and to the left]
  else if (senRState == 1 && senLState == 0 && senBState == 0) {
    back(60);//86
    delay(850);
    left();
    delay(300);
  }
  //If right sensor is seeing [White], left sensor is seeing [Black] -> [move Back and to the right]
  else if (senRState == 0 && senLState == 1 && senBState == 0) {
    back(70);
    delay(850);
    right( );
    delay(300);
  }
  //If right&left sensor is seeing [Black] -> [move Back and to the right]
  else if (senRState == 1 && senLState == 1 && senBState == 0) {
    back(70);//90
    delay(850);
    right();
    delay(300);
  }
  //If back sensor is seeing [Black] -> [move front and to the right]
  else{
    front(60);//90
    delay(300);
    right();
    delay(300);
  }
}

//---------------------------------function-------------------------
void stopM() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 0);
}
void front(int s) {
  analogWrite(ENA, s);
  analogWrite(ENB, s);
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
}
void back(int t) {
  analogWrite(ENA, t);
  analogWrite(ENB, t);
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
}
void right() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
  analogWrite(ENA, 60);
  analogWrite(ENB, 60);
}
void left() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
  analogWrite(ENA, 60);
  analogWrite(ENB, 60);
}

// To attacking the object with a faster speed
void FRONTattack() {
  if (millis() - previousMillis2 >= 500) {
    previousMillis2 = millis();
    front(85);
  }
}

// Looking or searching around to find any object
void Seaching()
{
  //First event in searching mode is moving front for an interval
  if (millis() - previousMillis1 >= event_intervalF && frontFlag == 0 && rightFlag == 0) {
    Serial.println(millis() - previousMillis1);
    previousMillis1 = millis(); //update millis value
    front(80);
    frontFlag = 1;
    Serial.println("front ------ frontFlag = 1");
  }

  //Second event in searching mode is moving right for an interval
  else if (millis() - previousMillis1 >= event_intervalR && frontFlag == 1 && rightFlag == 0) {
    Serial.println(millis() - previousMillis1);
    previousMillis1 = millis(); //update millis value
    right();
    rightFlag = 1;
    Serial.println("right ------ rightFlag = 1");
  }

  //Third event in searching mode is moving left for an interval
  else if (millis() - previousMillis1 >= event_intervalR && frontFlag == 1 && rightFlag == 1) {
    Serial.println(millis() - previousMillis1);
    previousMillis1 = millis(); //update millis value
    left();
    frontFlag = 0;
    rightFlag = 0;
    Serial.println("left ------ Flags = 1");

  }
}

//To either go search for an object or attacking that object
void fight() {
  if (distance <= maximumRange) {
    FRONTattack();
    frontFlag = 0;
    rightFlag = 0;
  }

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