#pragma config(Sensor, S2,     Back,                sensorSONAR)
#pragma config(Sensor, S3,     Front,               sensorSONAR)
#pragma config(Sensor, S4,     Side,                sensorSONAR)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


void moveForward()
{
  nMotorEncoder(motorB) = 0;
  nMotorEncoderTarget(motorB) = 600;      // move car forward to bring in level with car it is parking behind.
  motor(motorB) = -45;
  wait1Msec(2500);
}

void park()
{
  nMotorEncoder[motorC] = 0;
  nMotorEncoderTarget[motorC]=180;
  motor(motorC)=75;                     //turn wheels right
  wait1Msec(1000);

  nMotorEncoder[motorB] = 0;
  nMotorEncoderTarget[motorB]=900;
  motor(motorB)=25;
  wait1Msec(2000);                     //move car back

  nMotorEncoder[motorC] = 0;
  nMotorEncoderTarget[motorC]=180;
  motor(motorC)=-75;                    //turn wheels left
  wait1Msec(1000);

  nMotorEncoder[motorC] = 0;
  nMotorEncoderTarget[motorC]=180;
  motor(motorC)=-75;                    //turn wheels left
  wait1Msec(1000);

  nMotorEncoder[motorB] = 0;
  nMotorEncoderTarget[motorB]=350;
  motor(motorB)=35;                     //move car back
  wait1Msec(2000);

  nMotorEncoder[motorC] = 0;
  nMotorEncoderTarget[motorC]=180;
  motor(motorC)=75;                     //turn wheels right
  wait1Msec(1000);

  nMotorEncoder[motorB] = 0;
  nMotorEncoderTarget[motorB]=120;
  motor(motorB)=-35;                    //move car forward
  wait1Msec(1000);

}

task main()
{
  int A;
  int B;
  int totalDistance;
  int distance = 10;
  nMotorEncoder(motorB) = 0;

  while(true)
  {
    motor(motorB) = -35;

  if(SensorValue(S4)>distance)
    {
      A = nMotorEncoder(motorB);
    }

  if(SensorValue(S4)<distance)
    {
      B = nMotorEncoder(motorB);
    }

  totalDistance = B - A;

  if(totalDistance>750)
    {
      moveForward();
      park();
      StopAllTasks();
    }
    else
    {
      motor(motorB) = 0;
    }
  }
}