#pragma config(Sensor, S1,     xhome,          sensorTouch)
#pragma config(Sensor, S2,     yHome,          sensorSONAR)
#pragma config(Sensor, S3,     zhome,          sensorTouch)
#pragma config(Motor,  motorA,          xMotor,        tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  motorB,          yMotor,        tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          zMotor,        tmotorNXT, PIDControl, reversed, encoder)


void zresetowaniePozycjiX();
void zresetowaniePozycjiY();
void zresetowaniePozycjiZ();

void przesunX(int docelowaX);
void przesunY(int docelowaY);
void przesunZ(int docelowaZ);

void wiercenie(int tablicax ,int tablicay, int tablicawiercenie);

int tablicax[5]={-51,-52,-53,-54,-55,};
int tablicay[5]={50,51,53,54,55,};
int tablicawiercenie[5]={55,56,57,58,59,};


task main()
{
      zresetowaniePozycjiX();         
      wait1Msec(1);
      zresetowaniePozycjiY();
      wait1Msec(1);
      zresetowaniePozycjiZ();
      wait1Msec(1);                        //reseting positions
   
      wait1Msec(1);                     
      przesunX(-50);
      wait1Msec(1000);
      przesunY(-50);
      wait1Msec(1000);
      przesunZ(50);
      wait1Msec(4000);                  //movement to brick
      
      nMotorEncoder[motorA] = 0;   nxtDisplayTextLine(1,"%d", nMotorEncoder[motorA]);
      nMotorEncoder[motorB] = 0;   nxtDisplayTextLine(2,"%d", nMotorEncoder[motorB]);
      nMotorEncoder[motorC] = 0;   nxtDisplayTextLine(3,"%d", nMotorEncoder[motorC]); 
                                          
                                                //reset motor encoder & show
      void wiercenie;
      wait10Msec(1000);

}




void wiercenie(int tablicax, int tablicay, int tablicawiercenie)
{
 nxtDisplayTextLine(0,"Test %d %d %d",tablicax,tablicay,tablicawiercenie); 
}




void zresetowaniePozycjiX()
{

   while(SensorValue[S1]==0)
   {
      nxtDisplayTextLine(1,"%d", nMotorEncoder[motorA]);
      nxtDisplayTextLine(2,"%d", SensorValue[S1]);
      motor[motorA] = 10;
   }
      motor[motorA] = 0;
      nMotorEncoder[motorA] = 0;
}

void zresetowaniePozycjiY()
{

   while(SensorValue[S2]>5)
   {
      nxtDisplayTextLine(1,"%d", nMotorEncoder[motorB]);
      nxtDisplayTextLine(2,"%d", SensorValue[S2]);
      motor[motorB] =10;
   }
      motor[motorB] =0;
      nMotorEncoder[motorB] = 0;
}
void zresetowaniePozycjiX()
{

   while(SensorValue[S1]==0)
   {
      nxtDisplayTextLine(1,"%d", nMotorEncoder[motorA]);
      nxtDisplayTextLine(2,"%d", SensorValue[S1]);
      motor[motorA] = 10;
   }
      motor[motorA] = 0;
      nMotorEncoder[motorA] = 0;
}
void zresetowaniePozycjiZ()
{

   while(SensorValue[S3]==0)
   {
      nxtDisplayTextLine(1,"%d", nMotorEncoder[motorC]);
      nxtDisplayTextLine(2,"%d", SensorValue[S3]);
      motor[motorC] = -10;
   }
      motor[motorC] = 0;
      nMotorEncoder[motorC] = 0;
}

void przesunX(int docelowaX)
{
      int aktualna1=nMotorEncoder[motorA];
      int roznica1=aktualna1-docelowaX;

      while(aktualna1 != docelowaX)
      {
      aktualna1 = nMotorEncoder[motorA];
      roznica1 = aktualna1 - docelowaX;
     motor[motorA] =-10;
      wait1Msec(-roznica1);
   {
      nxtDisplayTextLine(1,"%d", aktualna1);
      nxtDisplayTextLine(2,"%d", docelowaX);
      nxtDisplayTextLine(3,"%d", roznica1);
     }
   }
   motor[motorA] = 0;
}

void przesunY(int docelowaY)
{
      int aktualna = nMotorEncoder[motorB];
      int roznica = aktualna - docelowaY;

      while (aktualna != docelowaY)
      {
      aktualna = nMotorEncoder[motorB];
      roznica = aktualna - docelowaY;
     motor[motorB]=-10;
      wait1Msec(docelowaY);

      nxtDisplayTextLine(1,"%d", aktualna);
      nxtDisplayTextLine(2,"%d", docelowaY);
      nxtDisplayTextLine(3,"%d", roznica);
   }
   motor[motorB] = 0;
}

void przesunZ(int docelowaZ)
{
      int aktualna2=nMotorEncoder[motorC];
      int roznica2=aktualna2-docelowaZ;

      while(aktualna2 != docelowaZ)
      {
      aktualna2 = nMotorEncoder[motorC];
      roznica2 = aktualna2 - docelowaZ;
     motor[motorC] =10;
      wait1Msec(--roznica2);
   {
      nxtDisplayTextLine(1,"%d", aktualna2);
      nxtDisplayTextLine(2,"%d", docelowaZ);
      nxtDisplayTextLine(3,"%d", roznica2);
     }
   }
   motor[motorC] = 0;
}