#include <Servo.h>
#include<NewPing.h>

#define redLED_PIN A4
#define blueLED_PIN A5
#define switch_PIN 2

int switch_status = 0;

//hc-sr04 sensor
#define TRIGGER_PIN A1
#define ECHO_PIN A0
#define max_distance1 100
#define max_distance2 50
#define Buzzer 13

//ir sensor code #1
#define irLeft_FM A2
#define irRight_FM A3

//ir sensor code #2
#define irLeft_LF 10
#define irMid_LF  !digitalRead(11)
#define irRight_LF 12

//ir sensor code #1
int IR_L_FM ;
int IR_R_FM ;

//ir sensor code #2
int IR_L_LF ;
int IR_M_LF ;
int IR_R_LF ;


int pos = 0;


Servo servo;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, max_distance1);
NewPing sonar2(TRIGGER_PIN, ECHO_PIN, max_distance2);

int distance = 0;
int leftDistance;
int rightDistance;


int ENA = 6;                           //ENA connected to digital pin 10
int DCMotor_IN1 = 8;                      // LEFT MOTOR
int DCMotor_IN2 = 7;                      // LEFT MOTOR

int DCMotor_IN3 = 5;                      // RIGHT MOTOR
int DCMotor_IN4 = 4;                      // RIGHT MOTOR
int ENB = 3;                           //ENB connected to digital pin ?


//int SpeedControl = A0;
int MOTOR_SPEED1 = 170;
int MOTOR_SPEED2 = 210;
int MOTOR_SPEED3 = 250;

boolean object;

void setup() {

  Serial.begin(1200);

  pinMode(redLED_PIN, OUTPUT);
  pinMode(blueLED_PIN, OUTPUT);
  pinMode(switch_PIN, INPUT);
  digitalWrite(redLED_PIN, HIGH);
  digitalWrite(blueLED_PIN, HIGH);

  pinMode(irLeft_FM, INPUT);
  pinMode(irRight_FM, INPUT);
  pinMode(irLeft_LF, INPUT);
  pinMode(irMid_LF, INPUT);
  pinMode(irRight_LF, INPUT);

  //  pinMode(irMid, INPUT);

  pinMode(Buzzer, OUTPUT);

  servo.attach(9);
  {
    for (pos = 90; pos <= 180; pos += 1) {
      servo.write(pos);
      delay(15);
    } for (pos = 180; pos >= 0; pos -= 1) {
      servo.write(pos);
      delay(15);
    } for (pos = 0; pos <= 90; pos += 1) {
      servo.write(pos);
      delay(15);
    }
  }

  pinMode(ENA, OUTPUT); // initialize ENA pin as an output
  pinMode(ENB, OUTPUT); // initialize ENB pin as an output
  pinMode(DCMotor_IN1, OUTPUT); // initialize MOTOR_A1 pin as an output   ( LEFT MOTOR )
  pinMode(DCMotor_IN2, OUTPUT); // initialize MOTOR_A2 pin as an output   ( LEFT MOTOR )
  pinMode(DCMotor_IN3, OUTPUT); // initialize MOTOR_B1 pin as an output   ( RIGHT MOTOR )
  pinMode(DCMotor_IN4, OUTPUT); // initialize MOTOR_B2 pin as an output   ( RIGHT MOTOR )
  digitalWrite(redLED_PIN, LOW);
  digitalWrite(blueLED_PIN, LOW);
}

void loop() {
  switch_status = digitalRead(switch_PIN);

  if (switch_status) {
    digitalWrite(redLED_PIN, HIGH);
    digitalWrite(blueLED_PIN, LOW);
    loop_code_1();
  }
  else {
    digitalWrite(redLED_PIN, LOW);
    digitalWrite(blueLED_PIN, HIGH);
    loop_code_2();
  }

}// end of LOOP


//External functions

void loop_code_1() {
  IR_L_FM = digitalRead (irLeft_FM);
  Serial.print("IR_L: ");
  Serial.print(IR_L_FM);

  IR_R_FM = digitalRead (irRight_FM);
  Serial.print("   /  IR_R: ");
  Serial.println(IR_R_FM);

  // put your main code here, to run repeatedly:

  delay(50);

  distance = getDistance();


  if ((IR_R_FM == 1) && (distance >= 11 && distance <= 40) && (IR_L_FM == 1)) {

    moveForward();
    Serial.println("moveForward");


  }
  else if ((IR_R_FM == 1) && (IR_L_FM == 0)) {

    moveRight();
    Serial.println("moveRight");


  }
  else if ((IR_R_FM == 0) && (IR_L_FM == 1)) {

    moveLeft();
    Serial.println("moveLeft");


  }

  else if ((IR_R_FM == 0) && (distance <= 20) && (IR_L_FM == 0)) {

    moveBackward;
    Serial.println("moveBackward");


  }
  else if (distance > 1 && distance < 10) {

    Stop();
    Serial.println("Stop");


  }
}
void loop_code_2() {

  IR_L_LF = digitalRead (irLeft_LF);
  Serial.print("IR_L_LF: ");
  Serial.print(IR_L_LF);
  IR_M_LF = digitalRead (irMid_LF);
  Serial.print("  /  IR_M_LF: ");
  Serial.print(IR_M_LF);
  IR_R_LF = digitalRead (irRight_LF);
  Serial.print("   /  IR_R_LF: ");
  Serial.println(IR_R_LF);


  if (IR_L_LF == 0 && IR_M_LF == 1 && IR_R_LF == 0 ) {
    objectAvoid();
    Serial.println("FW");
    //forword

  }

  else if ((IR_L_LF == 0 && IR_M_LF == 0 && IR_R_LF == 1 ) || (IR_L_LF == 0 && IR_M_LF == 1 && IR_R_LF == 1 )) {   // when irRight_LF = 1
    objectAvoid();
    Serial.println("TR");
    //rightturn
    moveRight();
  }
  else if ((IR_L_LF == 1 && IR_M_LF == 0 && IR_R_LF == 0 ) || (IR_L_LF == 1 && IR_M_LF == 1 && IR_R_LF == 0 )) {    // when irLeft_LF = 1
    objectAvoid();
    Serial.println("TL");
    //leftturn
    moveLeft();
  }
  else if (IR_L_LF == 1 && IR_M_LF == 0 && IR_R_LF == 1 ) {
    Serial.println("BK");
    //Backward
    moveBackward();
  }

  else if (IR_L_LF == 0 && IR_M_LF == 0 && IR_R_LF == 0 ) {
    Serial.println("F_Line");
    //Find Line
    findLine();
  }

  else if (IR_L_LF == 1 && IR_M_LF == 1 && IR_R_LF == 1 ) {
    //Stop
    Stop();
  }

}

int getDistance() {
  delay(50);
  int cm = sonar.ping_cm();
  return cm;
}

int getDistance2() {
  delay(50);
  int cm = sonar2.ping_cm();
  return cm;
}

///////////////////////////////////////////////////from code 2↓
void objectAvoid() {
  distance = getDistance2();
  if (distance <= 20) {
    //stop
    Stop();
    Serial.println("Obstcal Stop");

    lookLeft();
    lookRight();
    delay(100);

    if (rightDistance <= leftDistance) {
      //left
      object = true;
      turn();
      Serial.println("moveLeft");
    } else {
      //right
      object = false;
      turn();
      Serial.println("moveRight");
    }
    delay(100);
  }
  else {
    //forword
    Serial.println("No Obstcal");
    moveForward();
  }
}


int lookLeft () {
  //lock left
  servo.write(150);
  delay(500);
  leftDistance = getDistance2();
  delay(100);
  servo.write(90);
  Serial.print("Left:");
  Serial.print(leftDistance);
  return leftDistance;
  delay(100);
}

int lookRight() {
  //lock right
  servo.write(30);
  delay(500);
  rightDistance = getDistance2();
  delay(100);
  servo.write(90);
  Serial.print("   ");
  Serial.print("Right:");
  Serial.println(rightDistance);
  return rightDistance;
  delay(100);
}

void turn() {
  if (object == false) {
    Serial.println("turn Right");
    moveRight();
    delay(700);
    moveForward();
    delay(800);
    moveLeft();
    delay(900);

    if (digitalRead(irRight_LF) == 1) {
      loop();
    } else {
      moveForward();
    }
  }
  else {
    Serial.println("turn left");
    moveLeft();
    delay(700);
    moveForward();
    delay(800);
    moveRight();
    delay(900);

    if (digitalRead(irLeft_LF) == 1) {
      loop();
    } else {
      moveForward();
    }
  }
}

void findLine() {
  if (object == false) {
    Serial.println("turn Right");
    moveRight();
    delay(700);
    moveForward();
    delay(800);
    moveRight();
    delay(900);

    if (digitalRead(irLeft_LF) == 0, digitalRead(irMid_LF) == 0 && digitalRead(irRight_LF) == 0) {
      loop();
    } else {
      moveForward();
    }
  }
  else {
    Serial.println("turn left");
    moveLeft();
    delay(700);
    moveForward();
    delay(800);
    moveRight();
    delay(900);
    if (digitalRead(irLeft_LF) == 0, digitalRead(irMid_LF) == 0 && digitalRead(irRight_LF) == 0) {
      loop();
    } else {
      moveForward();
    }
  }
}
///////////////////////////////////////////////////from code 2↑
void moveForward() {
  analogWrite(ENA, MOTOR_SPEED1);
  analogWrite(ENB, MOTOR_SPEED1);

  digitalWrite(DCMotor_IN1, HIGH);   //( LEFT MOTOR )
  digitalWrite(DCMotor_IN2, LOW);              //( LEFT MOTOR )

  digitalWrite(DCMotor_IN3, HIGH);   //( RIGHT MOTOR )
  digitalWrite(DCMotor_IN4, LOW);              //( RIGHT MOTOR )

}

void moveBackward() {
  analogWrite(ENA, MOTOR_SPEED1);
  analogWrite(ENB, MOTOR_SPEED1);

  digitalWrite(DCMotor_IN1, LOW);     //( LEFT MOTOR )
  digitalWrite(DCMotor_IN2, HIGH);              //( LEFT MOTOR )

  digitalWrite(DCMotor_IN3, LOW);     //( RIGHT MOTOR )
  digitalWrite(DCMotor_IN4, HIGH);              //( RIGHT MOTOR )
}

void Stop() {

  digitalWrite(Buzzer, HIGH);
  delay(50);
  digitalWrite(Buzzer, LOW);
  delay(500);
  digitalWrite(Buzzer, HIGH);
  delay(50);
  digitalWrite(Buzzer, LOW);
  delay(500);

  analogWrite(ENA, MOTOR_SPEED1);
  analogWrite(ENB, MOTOR_SPEED1);

  digitalWrite(DCMotor_IN1, LOW);       //( LEFT MOTOR )
  digitalWrite(DCMotor_IN2, LOW);       //( LEFT MOTOR )

  digitalWrite(DCMotor_IN3, LOW);       //( RIGHT MOTOR )
  digitalWrite(DCMotor_IN4, LOW);       //( RIGHT MOTOR )

}

void moveRight() {

  analogWrite(ENA, MOTOR_SPEED2);
  analogWrite(ENB, MOTOR_SPEED1);

  digitalWrite(DCMotor_IN1, HIGH);  //255- MOTOR_SPEED);   //( LEFT MOTOR )
  digitalWrite(DCMotor_IN2, LOW);              //( LEFT MOTOR )

  digitalWrite(DCMotor_IN3, LOW);        //( RIGHT MOTOR )
  digitalWrite(DCMotor_IN4, HIGH);//MOTOR_SPEED);               //( RIGHT MOTOR )

}
void moveLeft() {
  analogWrite(ENA, MOTOR_SPEED1);
  analogWrite(ENB, MOTOR_SPEED2);

  digitalWrite(DCMotor_IN1, LOW);      //( LEFT MOTOR )
  digitalWrite(DCMotor_IN2, HIGH);//MOTOR_SPEED);             //( LEFT MOTOR )

  digitalWrite(DCMotor_IN3, HIGH);  //255- MOTOR_SPEED); //( RIGHT MOTOR )
  digitalWrite(DCMotor_IN4, LOW);            //( RIGHT MOTOR )

}