#define MLF 2
#define MLB 3
#define MRF 5
#define MRB 6

#define SL A6
#define SR A7
#define LED 13

void setup() {
  pinMode(MLF,OUTPUT);
  pinMode(MLB,OUTPUT);
  pinMode(MRF,OUTPUT);
  pinMode(MRB,OUTPUT);

  /*pinMode(M1,OUTPUT);
  pinMode(M2,OUTPUT);
  pinMode(M3,OUTPUT);
  pinMode(M4,OUTPUT);
  pinMode(SL,INPUT);
  pinMode(SR,INPUT);
  Serial.begin(9600);
  sensorCalibrate();*/
}

int lightest, darkest, i;

void loop() {
  //detectionAlgo();
  //delay(10);
  turnRight();
  delay(5000);
  turnLeft();
  delay(5000);
}

void turnOff(){
  digitalWrite(MLF,LOW);
  digitalWrite(MLB,LOW);
  digitalWrite(MRF,LOW);
  digitalWrite(MRB,LOW);
}

void turnRight(){
  turnOff();
  digitalWrite(MLF,HIGH);
  digitalWrite(MLB,LOW);
  digitalWrite(MRF,LOW);
  digitalWrite(MRB,HIGH);
}

void turnLeft(){
  turnOff();
  digitalWrite(MLF,LOW);
  digitalWrite(MLB,HIGH);
  digitalWrite(MRF,HIGH);
  digitalWrite(MRB,LOW);
}

void driveForward(){
  turnOff();
  digitalWrite(MLF,HIGH);
  digitalWrite(MLB,LOW);
  digitalWrite(MRF,HIGH);
  digitalWrite(MRB,LOW);
}

void driveBack(){
  turnOff();
  digitalWrite(MLF,LOW);
  digitalWrite(MLB,HIGH);
  digitalWrite(MRF,LOW);
  digitalWrite(MRB,HIGH);
}


void sensorCalibrate(){
  int leftRead, rightRead;
  i=0;
  lightest=1024;
  darkest=0;
  while(i<100){
    leftRead = analogRead(SL);
    rightRead = analogRead(SR);
    if(leftRead > darkest)
      darkest = leftRead;
    if(rightRead > darkest)
      darkest = rightRead;
    if(leftRead < lightest)
      lightest = leftRead;
    if(rightRead < lightest)
      lightest = rightRead;
    if(i % 10 == 0)
      digitalWrite(LED,HIGH);
    if(i % 10 == 5)
      digitalWrite(LED,LOW);
    delay(50);
    i++;
  }
  digitalWrite(LED,LOW);
}

bool blackLine(char sensor){
  if(analogRead(sensor) > 0.7 * darkest)
    return true;
  else
    return false;
}

void detectionAlgo(){
  if(blackLine(SL) && blackLine(SR)){
    turnOff();
    digitalWrite(LED,HIGH);
  }
  else
    if(blackLine(SL))
      turnLeft();
    else
      if(blackLine(SR))
        turnRight();
      else
        driveForward();
}