#pragma config(StandardModel, "Virtual Worlds NXT")
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/* Virtual Worlds Configuration (Type - Name):
Motor Port A: NXT Motor - gripperMotor
Motor Port B: NXT Motor - rightMotor
Motor Port C: NXT Motor - leftMotor
Sensor Port 1: Touch Sensor - touch
Sensor Port 2: Compass Sensor - compass
Sensor Port 3: Light Sensor - light
Sensor Port 4: Sonar Sensor - sonar
*/

void forward(int p, int t){
   motor[motorC] = p;
   motor[motorB] = p;
   wait1Msec(t);
}

void leftPoint(int t){
   motor[motorC] = -20;
   motor[motorB] = 20;
   wait1Msec(t);
}

void rightPoint(int t){
   motor[motorC] = 20;
   motor[motorB] = -20;
   wait1Msec(t);
}

void stop(int t){
   motor[motorC] = 0;
   motor[motorB] = 0;
   wait1Msec(t);
}

//gives power and number of encoder counts for robot to move forward
void forwardEncoded(int p, int e){
   nMotorEncoder[leftMotor] = 0;
   while(nMotorEncoder[leftMotor] < e){
      motor[leftMotor] = p;
      motor[rightMotor] = p;
   }
}

//give a negative encoder value and negative power value
void backwardEncoded(int p, int e){
   nMotorEncoder[leftMotor] = 0;
   while(nMotorEncoder[leftMotor] > e){
      motor[leftMotor] = p;
      motor[rightMotor] = p;
   }
}

void rightPointEncoded(int p, int e){
   nMotorEncoder[leftMotor] = 0;
   while(nMotorEncoder[leftMotor] < e){
      motor[leftMotor] = p;
      motor[rightMotor] = -p;
   }
}

void leftPointEncoded(int p, int e){
   nMotorEncoder[rightMotor] = 0;
   while(nMotorEncoder[rightMotor] < e){
      motor[leftMotor] = -p;
      motor[rightMotor] = p;
   }
}
//note time passed is in 10 ms intervals
void encoderLineFollow(int e){
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoder[leftMotor] = 0;

   while(nMotorEncoder[leftMotor] < e && nMotorEncoder[rightMotor] < e){
      //line follow
      if(SensorValue[light] < 43) {
      motor[leftMotor]  = 5;
      motor[rightMotor] = 35;
      }
      else{
      motor[leftMotor]  = 35;
      motor[rightMotor] = 5;
      }

   }
}
   //aligns to the given compass sensor reading
   //note: doesnt work when reading has to wrap around 360 ie: current reading is 259, given parameter is 1
void align(int direction){
   while (SensorValue[compass] < direction){
      motor[leftMotor] = 10;
   }
   while(SensorValue[compass] > direction){
      motor[rightMotor] = 10;
   }
}

task main()
{
   while(true){
      
      while(SensorValue[sonar] > 25){
         
         if(SensorValue[light] < 50){
            rightPointEncoded(20,300);
            stop(500);
         }
         
         else{
            motor[leftMotor] = 75;
            motor[rightMotor] = 75;
         }
         
      }
      
      motor[leftMotor] = 0;
      motor[rightMotor] =0 ;
   }
   
}

(ubyte)SensorValue(sonarSensor)