int distance = 0;
int check = 0;
int timesec = 0;
int timemsec = 0;

task main()

{
	wait1Msec(10000);
	ClearTimer(T2);
	SensorType[S3] = sensorSONAR;
	SensorType[S1] = sensorTouch;
	SensorType[S4] = sensorLightActive;

/////////////////////////////////////
// SEARCH FOR BALL
////////////////////////////////////
	SensorValue[S3] = 0;
	motor[motorA] = -53;
	motor[motorC] = 45;
	wait1Msec(500);
	while(SensorValue[S3] < 1 || SensorValue[S3] > 75){}
	distance = SensorValue[S3];


////////////////////////////////////
// MOVE FORWARD TO BALL
////////////////////////////////////
distance = distance*14;

motor[motorA] = 78;
motor[motorC] = 69;

nMotorEncoder[motorA] = 0;

while(nMotorEncoder[motorA] < distance){}
motor[motorA] = 0;
motor[motorC] = 0;
wait1Msec(100);
///////////////////////////////////
// SEARCH AGAIN IF BALL IS NOT YET IN GOOD POSITION
/////////////////////////////////
if(SensorValue[S3] < 21)
{
			motor[motorA] = 58;
			motor[motorC] = 50;


			while(SensorValue[S1] == 0){}
			motor[motorA] = 0;
			motor[motorC] = 0;


			motor[motorA] = -63;
			motor[motorC] = -55;
			nMotorEncoder[motorA] = 0;
			while(nMotorEncoder[motorA] > -120){}
			motor[motorA] = 0;
			motor[motorC] = 0;
}
else
{

			SensorValue[S3] = 0;
			motor[motorA] = 38;
			motor[motorC] = -30;

			while(SensorValue[S3] < 1 || SensorValue[S3] > 23){}

			motor[motorA] = 0;
			motor[motorC] = 0;

			wait1Msec(300);
			motor[motorA] = 58;
			motor[motorC] = 50;


			while(SensorValue[S1] == 0){}
			motor[motorA] = 0;
			motor[motorC] = 0;

			wait1Msec(100);
			motor[motorA] = -63;
			motor[motorC] = -54;
			nMotorEncoder[motorA] = 0;
			while(nMotorEncoder[motorA] > -120){}
			motor[motorA] = 0;
			motor[motorC] = 0;

}
///////////////////////////////////////////////
//GRAB BALL
/////////////////////////////////////////
nMotorEncoder[motorB] = 0;
motor[motorB] = -40;
while(nMotorEncoder[motorB] > -100){}
motor[motorB] = 0;

/////////////////////////////////////////////
//GO TO BLACK LINE
/////////////////////////////////////////
	motor[motorA] = -100;
	motor[motorC] = -92;
	while(SensorValue[S4] > 50){}
/////////////////////////////////////////
//PREPARE GOOD POSITION FOR LINEFOLLOWING
//////////////////////////////////////

	motor[motorA] = 100;
	motor[motorC] = 92;
	wait1Msec(250);
	motor[motorA] = -78;
	motor[motorC] = 70;
	while(SensorValue[S4] > 50){}
	wait1Msec(20);
	while(SensorValue[S4] < 50){}
	wait1Msec(50);
//////////////////////////////////////////
//FOLLOW LINE FOR 21 SECONDS
////////////////////////////////////////
ClearTimer(T1);

		while(time100[T1] < 210)

			{

					if(SensorValue[S4] < 39)
					{

						motor[motorA] = 40 ; motor[motorC] = 100;
						wait1Msec(300);


					}
					else
					{

						motor[motorA] = 100 ; motor[motorC] = 40;


					}


			}


///////////////////////////////////////////
//LOOK FOR MOUNT
//////////////////////////////////////////


	motor[motorA] = 58;
	motor[motorC] = -50;
	SensorValue[S3] = 0;
	wait1Msec(5);

	while(check == 0)

	{
		if(SensorValue[S3] > 1){check = 1;}
		if(SensorValue[S3] < 50){check = 1;}

		if(SensorValue[S3] > 60){check = 0;}
		if(SensorValue[S3] == 0){check = 0;}
	}

	motor[motorA] = 0;
	motor[motorC] = 0;

distance = SensorValue[S3];
wait1Msec(100);

////////////////////////////////////
// MOVE FORWARD TO MOUNT
////////////////////////////////////
distance = distance*15;

motor[motorA] = 78;
motor[motorC] = 69;
nMotorEncoder[motorA] = 0;
while(nMotorEncoder[motorA] < distance){}

motor[motorA] = 0;
motor[motorC] = 0;
wait1Msec(100);

///////////////////////////////////
// SEARCH AGAIN IF MOUNT IS NOT YET IN GOOD POSITION
/////////////////////////////////
if(SensorValue[S3] < 16)
{
			motor[motorA] = 58;
			motor[motorC] = 50;


			while(SensorValue[S1] == 0){}
			motor[motorA] = 0;
			motor[motorC] = 0;

			wait1Msec(100);
			motor[motorA] = -38;
			motor[motorC] = -30;
			nMotorEncoder[motorA] = 0;
			while(nMotorEncoder[motorA] > -120){}
			motor[motorA] = 0;
			motor[motorC] = 0;
}
else
{

			SensorValue[S3] = 0;
			motor[motorA] = -40;
			motor[motorC] = 30;
			wait1Msec(5);

			while(SensorValue[S3] < 1 || SensorValue[S3] > 16 ){}

			motor[motorA] = 0;
			motor[motorC] = 0;

			wait1Msec(100);
			motor[motorA] = 58;
			motor[motorC] = 50;


			while(SensorValue[S1] == 0){}
			motor[motorA] = 0;
			motor[motorC] = 0;

			wait1Msec(100);
			motor[motorA] = -38;
			motor[motorC] = -31;
			nMotorEncoder[motorA] = 0;
			while(nMotorEncoder[motorA] > -130){}
			motor[motorA] = 0;
			motor[motorC] = 0;

}
///////////////////////////////////////////////
//RELEASE BALL
/////////////////////////////////////////


wait1Msec(500);
nMotorEncoder[motorB] = 0;
motor[motorB] = 30;
while(nMotorEncoder[motorB] < 100){}
motor[motorB] = 0;

wait1Msec(300);

motor[motorA] = -100;
motor[motorC] = -100;
while(SensorValue[S4] > 50){}
wait1Msec(5);
while(SensorValue[S4] < 50){}
wait1Msec(250);

motor[motorA] = 0;
motor[motorC] = 0;
/*////////////////////////////////////////////////////////////
The next few line are used to display the timer on the screen.
The reason for this is that variables (and timers) can not be higher than
32768. So if the robot needs more than 32768 miliseconds the timer starts
counting again from -32768. I fixed that by subtracting 32768 miliseconds.
Then it converts the rest in seconds and miliseconds, and then it adds the
32 seconds and 768 miliseconds.
/////////////////////////////////////////////////////////////*/

timemsec = time1[T2];

if(time1[T2] < 0)//Actually, if greater than 32768
{
	timemsec = time1[T2] - 32768;

	timesec = timemsec / 1000;
	timesec = timesec * 1000;
	timemsec = timemsec - timesec;
	timesec = timesec / 1000;

  timesec = timesec + 32;
  timemsec = timemsec + 768;

  if(timemsec > 1000)
  {
  timesec = timesec + 1;
  timemsec = timemsec - 1000;
  }

}
else
{
	timemsec = time1[T2];
	timesec = timemsec / 1000;
	timesec = timesec * 1000;
	timemsec = timemsec - timesec;
	timesec = timesec / 1000;
}



nxtDisplayBigStringAt(0, 45, "%d sec", timesec);
nxtDisplayBigStringAt(0, 30, "%d msec", timemsec);

PlaySound(soundBeepBeep);

while(true){}

}