// Defining the sensor pins
int SENSOR[6]= {0, 28, 27, 26, 25, 24}; //S1 to S5

// Defining the clapper pin and damp sensor pin, may be absent
#define CLP 22
#define NEAR 23

// L298N direction control pins
// Same in in L293D
#define IN2 50
#define IN1 51
#define IN4 52
#define IN3 53

// Speed control pins, PWM signal always
#define ENA 44
#define ENB 45

// Defining the take color
// 0 for black
// 1 for white
int IN_TRACK = false;
int OUT_OF_TRACK = true;

// sensor value, initially all 0
int S1=0, S2=0, S3=0, S4=0, S5=0, clp = 0, near=0;



int carStop();
int straight();
int turnLeft();
int turnRight();
int rotateLeft();
int rotateRight();

void setup() {
  // Sensor pin as input pins
  pinMode(SENSOR[1], INPUT);
  pinMode(SENSOR[2], INPUT);
  pinMode(SENSOR[3], INPUT);
  pinMode(SENSOR[4], INPUT);
  pinMode(SENSOR[5], INPUT);

  // clap and bump snesor pin as input pins
  pinMode(CLP, INPUT);
  pinMode(NEAR, INPUT);


  // motor direction control pins as output pin
  pinMode(IN2, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(IN3, OUTPUT);

  // motor speed control pins as output pins , PWM Pins
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  // setting full speed
  analogWrite(ENA, 255);
  analogWrite(ENB, 255);

  // to see ant serial moniter, use in debug
  Serial.begin(9600);

}

void loop() {
  //----------------------------------------
  // Sensor read
  S1 = digitalRead(SENSOR[1]);
  S2 = digitalRead(SENSOR[2]);
  S3 = digitalRead(SENSOR[3]);
  S4 = digitalRead(SENSOR[4]);
  S5 = digitalRead(SENSOR[5]);
  near = digitalRead(NEAR);
  clp = digitalRead(CLP);
  //--------------------------------------------

  // See at serial monitor
  Serial.println("S1 : "+String(S1) +", "+"S2 : "+String(S2) +", "+"S3 : "+String(S3) +", "+"S4 : "+String(S4) +", "+"S5 : "+String(S5) +", ""NEAR : "+String(near) +", ");

  if(near == true && clp==true){
    // stop if bump snesor is activate
    carStop();
    Serial.println("SOMTHING IS INFROT.");

  }else if( S1 == OUT_OF_TRACK && S2 == OUT_OF_TRACK && S3 == OUT_OF_TRACK && S4 == OUT_OF_TRACK && S5 == OUT_OF_TRACK){
    // No track available
    Serial.println("YOU MAY BE LOST, TAKE A 180 DEG. TURN !!!");

  }else if( S1 == IN_TRACK && S2 == IN_TRACK && S3 == IN_TRACK && S4 == IN_TRACK && S5 == IN_TRACK){
    // all sensor at track, It may be a checkpoint. do as per need
    Serial.println("YOU MAY BE AT CHECK POINT !!!");

  }else if(S3 == OUT_OF_TRACK){
    // Center sensor is not at track
    if(S5 == IN_TRACK || S4 == IN_TRACK){
      // right sensor at track
      turnRight();

    }else if(S1 == IN_TRACK || S2 == IN_TRACK){
      //left sensor at track
      turnLeft();

    }else{
      // No need to left-right
      straight();

    }
  }
  delay(10);
}

int carStop(){
  // Changing the direction control pins
  digitalWrite(IN2, LOW);
  digitalWrite(IN1, LOW);

  digitalWrite(IN4, LOW);
  digitalWrite(IN3, LOW);
}
int straight(){
  // Changing the direction control pins
        digitalWrite(IN2, HIGH);
        digitalWrite(IN1, LOW);

        digitalWrite(IN4, HIGH);
        digitalWrite(IN3, LOW);
}
int turnLeft(){
  // Changing the direction control pins
        digitalWrite(IN2, HIGH);
        digitalWrite(IN1, LOW);

        digitalWrite(IN4, LOW);
        digitalWrite(IN3, LOW);
}
int turnRight(){
        digitalWrite(IN2, LOW);
        digitalWrite(IN1, LOW);

        digitalWrite(IN4, HIGH);
        digitalWrite(IN3, LOW);
}
int rotateLeft(){
        digitalWrite(IN2, HIGH);
        digitalWrite(IN1, LOW);

        digitalWrite(IN4, LOW);
        digitalWrite(IN3, HIGH);
}
int rotateRight(){
        digitalWrite(IN2, LOW);
        digitalWrite(IN1, HIGH);

        digitalWrite(IN4, HIGH);
        digitalWrite(IN3, LOW);
}