//*!!Sensor,    S2,               sonarL, sensorSONAR,      ,                    !!*//
//*!!Sensor,    S3,           tiltSensor, sensorReflection,      ,               !!*//
//*!!Sensor,    S4,               sonarR, sensorSONAR,      ,                    !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors sonarL               = (tSensors) S2;   //sensorSONAR        //*!!!!*//
const tSensors tiltSensor           = (tSensors) S3;   //sensorReflection   //*!!!!*//
const tSensors sonarR               = (tSensors) S4;   //sensorSONAR        //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

int balSpeed = 75, spdFast = 85, spdMed = 75, spdSlow = 65;
int nomSonar = 255;
int i;

task balance()
{
  while (true)
  {
	  if (SensorValue(tiltSensor) < 36)
      {
      	//only go so far back
      	if(nMotorEncoder[motorB] > -200)
      	{
         motor[motorB] = - balSpeed;
        }
      }
      else if (SensorValue(tiltSensor) > 44)
      {
      	//only go so far forwards
      	if(nMotorEncoder[motorB] < 200)
      	{
        	motor[motorB] =  balSpeed;
        }
      }
      else
      {
      	motor[motorB] = 0;
      }
      wait1Msec(1);	  //wait a bit between each reading
      //motor[motorB] = 0;
  }
}

void avoidL()
{
	 //backup a bit
   motor[motorA] =  -spdMed;
   motor[motorA] =  -spdMed;
   wait10Msec(150);
   motor[motorA] =  spdFast;
   motor[motorA] =  -spdFast;
   wait10Msec(200);
   motor[motorA] =  spdMed;
   motor[motorA] =  spdMed;
}

void avoidR()
{
	 //backup a bit
   motor[motorA] =  -spdMed;
   motor[motorA] =  -spdMed;
   wait10Msec(150);
   motor[motorA] =  -spdFast;
   motor[motorA] =  spdFast;
   wait10Msec(200);
   motor[motorA] =  spdMed;
   motor[motorA] =  spdMed;
}

void avoidMidFar()
{
	 //backup a bit
   motor[motorA] =  -spdMed;
   motor[motorA] =  -spdMed;
   wait10Msec(250);
   motor[motorA] =  -spdFast;
   motor[motorA] =  spdFast;
   wait10Msec(250);
   motor[motorA] =  spdMed;
   motor[motorA] =  spdMed;
}

void avoidMidClose()
{
	 //backup far
   motor[motorA] =  -spdMed;
   motor[motorA] =  -spdMed;
   wait10Msec(350);
   motor[motorA] =  -spdFast;
   motor[motorA] =  spdFast;
   wait10Msec(250);
   motor[motorA] =  spdMed;
   motor[motorA] =  spdMed;
}


void avoidObstacle(int nomSonarL, int nomSonarR)
{
   if ((nomSonarL > 25) && (nomSonarR > 25))
   //no obstacles left or right
   {
    motor[motorA] =  spdMed;
    motor[motorC] =  spdMed;
   }

   else if ((nomSonarL < 25) && (nomSonarL > 15) && (nomSonarR > 25))
   //getting closer on left - turn slightly to right
   {
    motor[motorA] =  spdMed;
    motor[motorC] =  spdSlow;
   }

   else if ((nomSonarL <= 15) && (nomSonarR > 25))
   //very close on left
   {
     avoidL();
   }

   else if ((nomSonarR < 25) && (nomSonarR > 15) && (nomSonarL > 25))
   //getting closer on right - turn slightly to right
   {
    motor[motorA] = spdMed;
    motor[motorC] = spdSlow;
   }

   else if ((nomSonarR <= 15) && (nomSonarL > 25))
   //very close on right
   {
     avoidR();
   }

   else if ((nomSonarR <= 25) && (nomSonarR > 15) && (nomSonarL <= 25) && (nomSonarR > 15))
   //object directly in front. Not so close.
   {
     avoidMidFar();
   }

   else
   {
     avoidMidClose();
   }
}

task calibrateSonar()
{
  while(true)
  {
   //normalize the sonar readings before using them.
   for (i = 0; i < 10; i++)
	 {
 	  nomSonar = nomSonar + SensorValue(sonar);
		//nomSonarR = nomSonarR + SensorValue(sonarR);
		wait1Msec(1);
	 }
	 nomSonar = nomSonar / 10;
	 //nomSonarR = nomSonarR / 10;

 	 //nxtDisplayTextLine(1,"Sonar L:%d",nomSonarL);
   nxtDisplayTextLine(2,"Sonar :%d",nomSonar);

   //avoidObstacle(nomSonarL,nomSonarR);
   //reset the sonar avg values
//   nomSonarL = 255;
   nomSonar = 255;
  }
}

task main()
{
	nMotorEncoder[motorB] = 0;		//motor encoder initialized back to 0
  start balance();

  while (true)
  {
 	 //nomSonarL = SensorValue(sonarL);
 	 //nomSonarR = SensorValue(sonarR);
   //nxtDisplayTextLine(1,"Sonar L:%d",SensorValue(sonarL));
   //nxtDisplayTextLine(2,"Sonar R:%d",SensorValue(sonar));
 //  avoidObstacle(nomSonarL, nomSonarR);
  }
}