int LineWidth = 15;
int RoomOneAndTwoVal = 20;

int leftwall = 45;
int rightwall =45;

/*globals*/
int search[100];
int rots, i, t, p, inRoom, currRoom;
int counterThing = 2;
int useFlipBumper = 0;




void main(){
	int tmp;
	
	inRoom=0;
	printf("Push run to     start.");

	
	while(1){
		if(runbutton()){
			break;
		}
		wait(0.00001);
	}
	printf("\nGoing...");
	
	currRoom = 0;
	drive(0,0);
	forwards(20);
	while(1){     
		useFlipBumper = 0;
		if(currRoom == 0 || currRoom == 2){ RightWallFollow(); }
		if(currRoom != 0 && currRoom != 2){ LeftWallFollow(); }
		tmp=CheckRoom(currRoom);
		counterThing = 0;
		if(tmp != 0){
			currRoom++;
			if(tmp == 2){
			    printf("\nAll your fire are belong to us!!!");
				headHome(currRoom);
				return;
				break;
			}
		}
		
	}	
}

void headHome(int currRoom){
	int rots,surfC,tripped,i;
	motor(3, 0);
	rotation(1);
	counterThing++;
	printf("Thing: %d\n",counterThing);
	drive(0, 16);
	wait(0.5);
		
	rots = 0;
	surfC=0;
	tripped = 0;
	while(1){
		rots = rots + rotation(1); /*start counting how far we've gone*/
		LeftWallFollow();
		if( tripped == 1 ){
			/*/printf("have left room\n");/*/
			useFlipBumper = 0;
		} else {
			useFlipBumper = 1;
		}
		if( surface() < 210 ){
			surfC = surfC + 1;
			if(tripped == 1 && surfC > LineWidth){
				drive(0,0); /*Stop once we have gone far enough and it is white*/
				beep();
				printf("\n\nDone!!!!!!!!!!!!!");
				return;
			}
		} else {
		    if(surfC > 0 && surfC < 7 && rots > 30){ /* avearge line = 4 rots.*/
		 		if(tripped == 1){
		 			/*printf("LINE HIT");*/
		 			DriveFor(-20,0,0.6);
		 			DriveFor(0,20,0.6);
		 			drive(20,0);
		 			i = 0;
	 				while(ir_detector()==0 && i<12){
	 					wait(0.1);
	 					i = i + 1;
	 					printf("%d ",i);
	 				}
		 			drive(0,0);
		 			StopAll();
		 		} else {
		 			tripped = 1;
		 		}
		    }
		    /*if(surfC > 0){
		    	printf("%d ",surfC);
			}*/
			surfC = 0;
		}				
	}
}

void LeftWallFollow(){
	if(ir_detector()>0){
		drive(17, 30);	
		return;
	}
	drive(17, (analogport(7)-leftwall)/2);
	BumbChecker();

}


void RightWallFollow(){
	if(ir_detector()>0){
		drive(17, -30);	
		return;
	}
	drive(17, -((analogport(6)-rightwall)/2));
	BumbChecker();	

}

void BumbChecker(){
	int s;
	int b = bumper();
	if(b == 0){return;}
	if(b & 0b0001 || b & 0b0010){
		drive(-20, 0);
		wait(0.5);	
		if(useFlipBumper == 1 && b & 0b0010){  
			printf("Usefliper %d\n", b); 
			drive(0, -20);	   
	   	} else {
	   		drive(0, 20);
	   	}
		wait(0.5);
	} else {
		drive(20, 0);
		wait(0.2);
	}	
}


int checkUV(void){
	int s;
	for(s=0;s<1000;s++){
		if(digitalport(2)>0){ return 1; }
		wait(0.00001);
	}
	return 0;
}	

void DriveFor(int forward, int spin, float waitv){
	drive(forward,spin);
	wait(waitv);
}

void DriveUntilSurface(int left, int right){
	while(surface()>160){
		motor(1, left);
        motor(2, right);	
	}
}
void StopAll(void){
	motor(1, 0);
	motor(2, 0);
	motor(3, 0);
}
	
int CheckRoom(int roomNum){
/*returns 1 if the room had no fire, 2 if there was a fire and we put it out. returns 0 if it did not enter a room*/
	int s, b, t, l, cf, sur, last, delta, new, first;
	int contains_candle = 0; /*True if this room contains a candle*/
	int hit_white = 0;
	int candle_blasted = 1;
	int bash = 0;
	
	inRoom--;
	first = 0;
	if(surface()<160){
		inRoom = 120;
	
		/*travel to the UVTron checking position*/
		DriveFor(20,0,0.6);	
		drive(0, 0);		
		
			
		while(checkUV()==1 || candle_blasted == 0){ /*Will run until the fire is pwned.*/
			if(first == 0){
				DriveFor(20,0,1.0);	
				first = 1;
			}
			if(hit_white == 1){ /*If hit_white is true, back up and turn a bit*/
				drive(-14, 0);
				wait(0.4);
				drive(0, -12);
				wait(1.0);	
			}
			contains_candle = 1;
			hit_white = 0;
			bash = 0;
			findFire(); /*spin to the fire*/
			candle_blasted = 0;
			motor(1, 20); /*drive to the fire*/ 
			motor(2, 21);
			while(ir_detector()==0){
				int sur = surface();
				/*printf("SURFACE: %d IR: %d",sur,ir_detector());*/
				b = bumper();
				if(b & 0b0001 || b & 0b0010){
					DriveFor(-20,0,0.4);
					bash = 1;
					if(b & 0b0001){
						DriveFor(0,20,0.3);
					} else {
						DriveFor(0,-20,0.3);
					}
					DriveFor(20,0,0.4);
					break;
				}
				if(sur<160){
					hit_white = 1;
					drive(0,0);
					motor(3,100);
					wait(10.0);
					motor(3,0);
					candle_blasted = 1;
					break;
				}
				wait(0.001);
			}
			if(candle_blasted == 0){
				if(bash == 0){
					DriveFor(-19,0,0.4);
				}
				StopAll();
			}
		}
		if(contains_candle == 1){
			/*Spin around and avoid bashing into candle.*/
			/*DriveFor(0,-20,0.5);*/
			StopAll();
			return 2;
		}

		
		/*now exit*/
		if(roomNum == 0 || roomNum == 3){/*exit using leftwall following*/
			DriveUntilSurface(-20,-20);
			DriveFor(-20,0,0.8);
			DriveFor(0,-17,1.4); /*This may be wrong*/
			StopAll();			
			/*wait(1.0);*/
		} else {/*exit using right wall following*/
			DriveUntilSurface(-20,-20);
  			DriveFor(-20,0,0.7);
  			DriveFor(0,17,0.4);
  			if(roomNum != 1){
				DriveFor(0,17,0.2);
			} 
  			StopAll();
/*			wait(1.0);		*/
			if(roomNum != 1){
				DriveFor(20,0,1.0);
			}
			StopAll();
/*			wait(1.0);					*/
		}
		return 1;
	}
	return 0;
}


void findFire(void){
	int o =0;
	int b;
	int rots=0; rotation(1);
	drive(0,12);
	motor(3,0);
	while(1){
	
		if(digitalport(1)){
			o=o+1;
			/*printf("O: %d\n", o);/*/
		}else{ o = 0; }
		drive(0,12);		
		b = bumper();
		if(ir_detector() || b & 0b0001 || b & 0b0010){
			drive(-20,0);	
			wait(0.04);	
			rotation(1);
		} else {
			rots = rots + rotation(1);
		}
		wait(0.00001);
		if(o>1){
						
			/*wait(0.1);*/
			
			drive(0,0);
			/*wait(1.0); SPWAIT*/
			break;
		}
		if(rots>70){
			/*drive(13, 0);
			while(ir_detector()==0){
				if(surface()<160){break;}
				drive(20,0);} see the while loop here
			rots = 0;
			rotation(1);
			drive(0,13);*/	
			return;
		}		
	}
}		

int surface()
{
    return analogport(5);
}



void forwards(int ticks){
	int rots; rots=0; rotation(1);
	while(rots<ticks){
		motor(1, 18);
		motor(2, 20);				
		rots=rots+rotation(1);
	}
	drive(0, 0);
}