#include <Servo.h>
#include <irsensor.h>

unsigned irPinLeft = 4, irPinRight = 6, numberOfWhiteAndGreyStrips = 50;

double stripLength = 100, dividerLength = stripLength/numberOfWhiteAndGreyStrips;


irsensor leftIr(irPinLeft, stripLength, numberOfWhiteAndGreyStrips);
irsensor rightIr(irPinRight, stripLength, numberOfWhiteAndGreyStrips);

Servo rightServo;
Servo leftServo;
unsigned leftServoPin = 8, rightServoPin = 10;

unsigned slowForward = 110, slowBackward = 80, fastForward = 180, fastBackward = 0, mediumForward=120,mediumBackward = 70;


unsigned whiteGreyDivider = 50, greyBlackDivider = 100;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("Start");
}

void loop() {
  // put your main code here, to run repeatedly: 
  unsigned endDistance = 50;
  navigate(endDistance,1);
  returnHome(endDistance);
}




void navigate(unsigned distance, unsigned directions){
  unsigned slow=92,medium=92,fast=92;
  if(directions == 1){
   slow = slowForward;
   medium = mediumForward;
   fast = fastForward; 
  }else{
    slow = slowBackward;
    medium = mediumBackward;
    fast = fastBackward;
  }
  rightServo.write(slow);
  leftServo.write(slow);
  unsigned leftValue = 0, rightValue = 0, d = 0, leftD = 0, rightD = 0;
  unsigned whiteLeft=0, greyLeft=0, whiteRight=0, greyRight=0, blackLeft=0, blackRight=0;
  unsigned iLeft=0, iRight = 0;
  
  while(d<distance){
    leftValue = leftIr.RCtime();
    rightValue = rightIr.RCtime();
    if(leftValue < whiteGreyDivider){
      if(iLeft!=0){//off of black
       leftServo.write(slow); 
      }
      iLeft=0;      
      whiteLeft++;
    }else if(leftValue < greyBlackDivider){
      if(iLeft!=0){//off of black
       leftServo.write(slow); 
      }
      iLeft=0;
      greyLeft++;
    }else{ // black
      if(iLeft==19){
        leftServo.write(fast);
      }else{
        leftServo.write(medium);
      }
      blackLeft++;
      iLeft++;
      if(iLeft==20){
        iLeft=0;
      }
    }
    
    if(rightValue < whiteGreyDivider){
      if(iRight!=0){//off of black
       rightServo.write(slow); 
      }
      iRight = 0;
      whiteRight++;  
    }else if(leftValue < greyBlackDivider){
      if(iRight!=0){//off of black
       rightServo.write(slow); 
      }
      iRight = 0;
      greyRight++;    
    }else{// black
      if(iRight==19){
        rightServo.write(fast);
      }else{
        rightServo.write(medium);
      }
      blackRight++;
      iRight++;
      if(iRight==20){
        iRight=0;
      }
    }
  }
  
  leftD = (whiteLeft+greyLeft)*dividerLength;
  rightD = (whiteRight+greyRight)*dividerLength;
  d=(leftD+rightD)/2;
  
}




void returnHome(unsigned distance){
  navigate(distance, 0);
}