#define leftNearSensor     A3
#define leftFarSensor      A4
#define rightCenterSensor  A2
#define rightNearSensor    A1
#define rightFarSensor     A0

int leftNearReading;
int leftFarReading;
int rightCenterReading;
int rightNearReading;
int rightFarReading;

int replaystage;


#define leapTime 200

#define leftMotor1  7
#define leftMotor2  4

#define rightMotor1 8
#define rightMotor2 12

#define led 13

char path[30] = {};
int pathLength;
int readLength;

void setup(){
  
  pinMode(leftNearSensor, INPUT);
  pinMode(leftFarSensor, INPUT);
  pinMode(rightCenterSensor, INPUT);
  pinMode(rightNearSensor, INPUT);
  pinMode(rightFarSensor, INPUT);
    
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);
  pinMode(led, OUTPUT);
  //Serial.begin(115200);
  digitalWrite(led, HIGH);
  delay(1000);
}




void loop()
{
	readSensors();
	if(leftFarReading>200 && (leftNearReading<200 || rightCenterReading<200 || rightNearReading<200) && rightNearReading>200)
	{
	if ((!digitalRead(leftNearReading)) && (!digitalRead(rightCenterReading)) && (!digitalRead(rightNearReading)))       // if all black
         {  MotorControl(1,1); Serial.println("forward");   }
        else if ((digitalRead(leftNearReading)) && (!digitalRead(rightCenterReading)) && (!digitalRead(rightNearReading)))    // if left on white // S3 out
         {  MotorControl(1,0); Serial.println("right"); }
        else if ((digitalRead(leftNearReading)) && (digitalRead(rightCenterReading)) && (!digitalRead(rightNearReading)))    // if left & middle on white // S3 & s2 out
         {  MotorControl(1,0); Serial.println("right sharp"); }
        else if ((!digitalRead(leftNearReading)) && (!digitalRead(rightCenterReading)) && (digitalRead(rightNearReading)))    // if right on white // s1 out
         {  MotorControl(0,1); Serial.println("left"); }
        else if ((!digitalRead(leftNearReading)) && (digitalRead(rightCenterReading)) && (digitalRead(rightNearReading)))    // if right & middle on white // S2 & s1 out
         {  MotorControl(0,1); Serial.println("left sharp"); }
	}
	else
	{
	leftHandWall();
	}
}


void readSensors()
{
  
  leftNearReading    = analogRead(leftNearSensor);
  leftFarReading     = analogRead(leftFarSensor);
  rightCenterReading = analogRead(rightCenterSensor);
  rightNearReading   = analogRead(rightNearSensor);
  rightFarReading    = analogRead(rightFarSensor);  

}

void MotorControl(int driveL, int driveR)
{
switch (driveL) {
      
    case 0:                  // lft STOP
      digitalWrite (leftMotor1,LOW);
      digitalWrite (leftMotor2,LOW);
      break;
      
    case 1:                  // lft FORWARD
      digitalWrite (leftMotor1,HIGH);
      digitalWrite (leftMotor2,LOW);
      break;
      
    case 2:                  // lft REVERSE
      digitalWrite (leftMotor1,LOW);
      digitalWrite (leftMotor2,HIGH);
      break;
      
    default:break;
  } 
  
switch (driveR) {
      
    case 0:                  // rgt STOP
      digitalWrite (rightMotor1,LOW);
      digitalWrite (rightMotor2,LOW);
      break;
      
    case 1:                  // rgt FORWARD
      digitalWrite (rightMotor1,HIGH);
      digitalWrite (rightMotor2,LOW);
      break;
      
    case 2:                  // rgt REVERSE
      digitalWrite (rightMotor1,LOW);
      digitalWrite (rightMotor2,HIGH);
      break;
      
    default:break;
  }  
}
void leftHandWall()
{
		if(leftFarReading<200 && rightCenterReading<200)
		{
		digitalWrite(leftMotor1,HIGH);
		digitalWrite(leftMotor2,LOW);
		digitalWrite(rightMotor1,HIGH);
		digitalWrite(rightMotor2,LOW);
		delay(100);
		turnLeft();
		}
		

		if(rightFarReading<200 && rightCenterReading<200)
		{
		digitalWrite(leftMotor1,HIGH);
		digitalWrite(leftMotor2,LOW);
		digitalWrite(rightMotor1,HIGH);
		digitalWrite(rightMotor2,LOW);
		delay(100);
		readSensors();

			if(rightCenterReading<200)
			{
			straight();
			}
			else
			{
			turnRight();
			}
		}


		if(leftFarReading<200 && leftNearReading<200 && rightCenterReading<200 && rightNearReading<200 && rightNearReading<200)
		{
		digitalWrite(leftMotor1,HIGH);
		digitalWrite(leftMotor2,LOW);
		digitalWrite(rightMotor1,HIGH);
		digitalWrite(rightMotor2,LOW);
		delay(100);
		readSensors();

			if(leftFarReading<200 && leftNearReading<200 && rightCenterReading<200 && rightNearReading<200 && rightNearReading<200)
			{
			done();
			}
			else
			{
			turnLeft();
			}
		}


				

		if(leftFarReading>200 && leftNearReading>200 && rightCenterReading>200 && rightNearReading>200 && rightNearReading>200)
		{
		digitalWrite(leftMotor1,HIGH);
		digitalWrite(leftMotor2,LOW);
		digitalWrite(rightMotor1,HIGH);
		digitalWrite(rightMotor2,LOW);
		delay(100);
		turnAround();
                
		}

   		path[pathLength]='S';
 		pathLength++;
   		if(path[pathLength-2]=='B')
		{
   		shortPath();
  		}
return;
}


		void done()
		{
  		digitalWrite(leftMotor1, LOW);
    		digitalWrite(leftMotor2, LOW);
    		digitalWrite(rightMotor1, LOW);
   		digitalWrite(rightMotor2, LOW);
  		replaystage=1;
  		path[pathLength]='D';
  		pathLength++;
 			while(analogRead(leftFarSensor)<200)
			{
   			digitalWrite(led, LOW);
   			delay(150);
   			digitalWrite(led, HIGH);
   			delay(150);
 			}
 		delay(10000);
  		replay();
		}

void turnLeft()
{
  	while(analogRead(rightCenterSensor)<200)
	{
     		digitalWrite(leftMotor1, LOW);
    		digitalWrite(leftMotor2, HIGH);
    		digitalWrite(rightMotor1, HIGH);
    		digitalWrite(rightMotor2, LOW);
    		delay(2);
     		digitalWrite(leftMotor1, LOW);
   		digitalWrite(leftMotor2, LOW);
    		digitalWrite(rightMotor1, LOW);
    		digitalWrite(rightMotor2, LOW);
    		delay(1);
  	}
    	
	if(replaystage==0)
	{
   	 path[pathLength]='L';
         pathLength++;
         	if(path[pathLength-2]=='B')
		{
                shortPath();
      		}
  	}
}



void turnRight()
{

 
   	while(analogRead(rightCenterSensor)<200)
	{
    	digitalWrite(leftMotor1, HIGH);
    	digitalWrite(leftMotor2, LOW);
    	digitalWrite(rightMotor1, LOW);
    	digitalWrite(rightMotor2, HIGH);
    	delay(2);
     	digitalWrite(leftMotor1, LOW);
    	digitalWrite(leftMotor2, LOW);
    	digitalWrite(rightMotor1, LOW);
    	digitalWrite(rightMotor2, LOW);
    	delay(1);
	}
	  
  	if(replaystage==0)
	{
  		path[pathLength]='R';
 	 	pathLength++;
  		if(path[pathLength-2]=='B')
		{
    	       	 shortPath();	
		}
  	}
 
}
  

void straight()
{
  
 	if(leftFarReading>200 && (leftNearReading<200 || rightCenterReading<200 || rightNearReading<200) && rightNearReading>200)
	{
    	digitalWrite(leftMotor1, HIGH);
    	digitalWrite(leftMotor2, LOW);
    	digitalWrite(rightMotor1, HIGH);
    	digitalWrite(rightMotor2, LOW);
    	delay(1);
    	digitalWrite(leftMotor1, LOW);
    	digitalWrite(leftMotor2, LOW);
    	digitalWrite(rightMotor1, LOW);
    	digitalWrite(rightMotor2, LOW);
    	delay(5);
    	return;
  	}
}


void turnAround()
{
    	digitalWrite(leftMotor1, HIGH);
    	digitalWrite(leftMotor2, LOW);
    	digitalWrite(rightMotor1, HIGH);
    	digitalWrite(rightMotor2, LOW);
    	delay(150);
  	while(analogRead(rightCenterSensor)<200)
		{
     		digitalWrite(leftMotor1, LOW);
    		digitalWrite(leftMotor2, HIGH);
    		digitalWrite(rightMotor1, HIGH);
    		digitalWrite(rightMotor2, LOW);
    		delay(2);
     		digitalWrite(leftMotor1, LOW);
    		digitalWrite(leftMotor2, LOW);
    		digitalWrite(rightMotor1, LOW);
    		digitalWrite(rightMotor2, LOW);
    		delay(1);
  		}
  		
	path[pathLength]='B';
  	pathLength++;
  	straight();
}



void shortPath()
{
 	int shortDone=0;
  	if(path[pathLength-3]=='L' && path[pathLength-1]=='R')
	{
   	pathLength-=3;
    	path[pathLength]='B';
   	//Serial.println("test1");
    	shortDone=1;
 	}
   
  	if(path[pathLength-3]=='L' && path[pathLength-1]=='S' && shortDone==0)
	{
    	pathLength-=3;
    	path[pathLength]='R';
    	//Serial.println("test2");
    	shortDone=1;
  	}
   
  	if(path[pathLength-3]=='R' && path[pathLength-1]=='L' && shortDone==0)
	{
    	pathLength-=3;
    	path[pathLength]='B';
    	//Serial.println("test3");
    	shortDone=1;
  	}
  
   
  	if(path[pathLength-3]=='S' && path[pathLength-1]=='L' && shortDone==0)
	{
    	pathLength-=3;
    	path[pathLength]='R';
    	//Serial.println("test4");
    	shortDone=1;
  	}
     
  	if(path[pathLength-3]=='S' && path[pathLength-1]=='S' && shortDone==0)
	{
    	pathLength-=3;
    	path[pathLength]='B';
    	//Serial.println("test5");
    	shortDone=1;
  	}
    
	if(path[pathLength-3]=='L' && path[pathLength-1]=='L' && shortDone==0)
	{
    	pathLength-=3;
    	path[pathLength]='S';
    	//Serial.println("test6");
    	shortDone=1;
  	}
  
  path[pathLength+1]='D';
  path[pathLength+2]='D';
  pathLength++;
  //Serial.print("Path length: ");
  //Serial.println(pathLength);
  //printPath();
}


void replay()
{
   	readSensors();
  	if(leftFarReading>200 && (leftNearReading<200 || rightCenterReading<200 || rightNearReading<200) && rightNearReading>200)
	{
    	straight();
 	}
 	 else
	{
   		 if(path[readLength]=='D')
		{
       		digitalWrite(leftMotor1, HIGH);
    		digitalWrite(leftMotor2, LOW);
    		digitalWrite(rightMotor1, HIGH);
    		digitalWrite(rightMotor2, LOW);
    		delay(100);
    		digitalWrite(leftMotor1, LOW);
    		digitalWrite(leftMotor2, LOW);
    		digitalWrite(rightMotor1, LOW);
   		digitalWrite(rightMotor2, LOW);
     		endMotion();
    		}
   		 if(path[readLength]=='L')
		{
    		digitalWrite(leftMotor1, HIGH);
    		digitalWrite(leftMotor2, LOW);
   		digitalWrite(rightMotor1, HIGH);
    		digitalWrite(rightMotor2, LOW);
    		delay(leapTime);
      		turnLeft();
    		}
    		if(path[readLength]=='R')
		{
       		digitalWrite(leftMotor1, HIGH);
    		digitalWrite(leftMotor2, LOW);
    		digitalWrite(rightMotor1, HIGH);
    		digitalWrite(rightMotor2, LOW);
    		delay(leapTime);
    		turnRight();
    		}
    		if(path[readLength]=='S')
		{
    		digitalWrite(leftMotor1, HIGH);
    		digitalWrite(leftMotor2, LOW);
    		digitalWrite(rightMotor1, HIGH);
    		digitalWrite(rightMotor2, LOW);
    		delay(leapTime);
    		straight();
    		}
    
    	readLength++;
  	}
    
  replay();
  
}

	void endMotion()
	{
    	digitalWrite(led, LOW);
   	delay(500);
   	digitalWrite(led, HIGH);
   	delay(200);
    	digitalWrite(led, LOW);
   	delay(200);
   	digitalWrite(led, HIGH);
   	delay(500);
  	endMotion();
	}