#define NINETY 180
#define ONEEIGHTY 370
#define NEAR 19
#define STEPS 40
#define CORRECTION 6

task main(){

int x = 0, y = 0, rotation = 1;

SetSensorLowspeed(IN_4);
while( y < STEPS){
while((y<STEPS) && (SensorUS(IN_4)>NEAR)){
RotateMotor(OUT_BC,60,90);
RotateMotor(OUT_C,40,CORRECTION);

if(rotation == 1)
y = y + 1;
if(rotation == 2)
x = x + 1;
if(rotation == 3)
y = y - 1;
if(rotation == 4)
x = x - 1;
}

if(y < STEPS){
 if((rotation == 1) && (x < 0)){
  RotateMotor(OUT_B,60,NINETY);
  RotateMotor(OUT_C,60,-NINETY);
  rotation = 2;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,NINETY);
   RotateMotor(OUT_C,60,-NINETY);
   rotation = 3;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,NINETY);
    RotateMotor(OUT_C,60,-NINETY);
    rotation = 4;
    }
 }


 else if((rotation == 1) && (x >= 0)){
  RotateMotor(OUT_B,60,-NINETY);
  RotateMotor(OUT_C,60,NINETY);
  rotation = 4;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,-NINETY);
   RotateMotor(OUT_C,60,NINETY);
   rotation = 3;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,-NINETY);
    RotateMotor(OUT_C,60,NINETY);
    rotation = 2;
    }
 }
 //3
 else if(rotation == 2){
  RotateMotor(OUT_B,60,-NINETY);
  RotateMotor(OUT_C,60,NINETY);
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,-NINETY);
   RotateMotor(OUT_C,60,NINETY);
   rotation = 4;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,-NINETY);
    RotateMotor(OUT_C,60,NINETY);
    rotation = 3;
    }
 }

 //5
 else if((rotation == 3) && (x < 0)){
  RotateMotor(OUT_B,60,ONEEIGHTY);
  RotateMotor(OUT_C,60,-ONEEIGHTY);
  y=y+1;
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,NINETY);
   RotateMotor(OUT_C,60,-NINETY);
   rotation = 2;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,ONEEIGHTY);
    RotateMotor(OUT_C,60,-ONEEIGHTY);
    x=x+1;
    rotation = 4;
    }
 }
 //6
 else if((rotation == 3) && (x >= 0)){
  RotateMotor(OUT_B,60,ONEEIGHTY);
  RotateMotor(OUT_C,60,-ONEEIGHTY);
  y=y-1;
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,NINETY);
   RotateMotor(OUT_C,60,-NINETY);
   rotation = 4;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,ONEEIGHTY);
    RotateMotor(OUT_C,60,-ONEEIGHTY);
    x=x-1;
    rotation = 2;
    }
 }
 //7
 else if((rotation == 4) && (x < 0))
 {
  RotateMotor(OUT_B,60,NINETY);
  RotateMotor(OUT_C,60,-NINETY);
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,NINETY);
   RotateMotor(OUT_C,60,-NINETY);
   rotation = 2;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,NINETY);
    RotateMotor(OUT_C,60,-NINETY);
    rotation = 3;
    }
 }
 //8
 else if((rotation == 4) && (x >= 0)){
  RotateMotor(OUT_B,60,NINETY);
  RotateMotor(OUT_C,60,-NINETY);
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,ONEEIGHTY);
   RotateMotor(OUT_C,60,-ONEEIGHTY);
   y=y+3 ;
   rotation = 3;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,-NINETY);
    RotateMotor(OUT_C,60,NINETY);
    rotation = 2;
    }
 }
}
}

if(x>0){
RotateMotor(OUT_B,60,-NINETY);
RotateMotor(OUT_C,60,NINETY);
RotateMotor(OUT_BC,60,(90*(abs (x))));
RotateMotor(OUT_B,60,NINETY);
RotateMotor(OUT_C,60,-NINETY);
}
if(x<0){
RotateMotor(OUT_B,60,NINETY);
RotateMotor(OUT_C,60,-NINETY);
RotateMotor(OUT_BC,60,(90*(abs (x))));
RotateMotor(OUT_B,60,-NINETY);
RotateMotor(OUT_C,60,NINETY);
}
OnFwd(OUT_A,60);
Wait(3500);
Off(OUT_A);
RotateMotor(OUT_B,60,-ONEEIGHTY);
RotateMotor(OUT_C,60,ONEEIGHTY);
RotateMotor(OUT_BC,60,270);

//Kinda Restart the program, reset variables................................................................................................

x=0;
y=0;
rotation=1;

while( y < STEPS){
while((y<STEPS) && (SensorUS(IN_4)>NEAR)){
RotateMotor(OUT_BC,60,90);
RotateMotor(OUT_C,40,CORRECTION);

if(rotation == 1)
y = y + 1;
if(rotation == 2)
x = x + 1;
if(rotation == 3)
y = y - 1;
if(rotation == 4)
x = x - 1;
}

if(y < STEPS){
 if((rotation == 1) && (x < 0)){
  RotateMotor(OUT_B,60,NINETY);
  RotateMotor(OUT_C,60,-NINETY);
  rotation = 2;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,NINETY);
   RotateMotor(OUT_C,60,-NINETY);
   rotation = 3;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,NINETY);
    RotateMotor(OUT_C,60,-NINETY);
    rotation = 4;
    }
 }


 else if((rotation == 1) && (x >= 0)){
  RotateMotor(OUT_B,60,-NINETY);
  RotateMotor(OUT_C,60,NINETY);
  rotation = 4;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,-NINETY);
   RotateMotor(OUT_C,60,NINETY);
   rotation = 3;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,-NINETY);
    RotateMotor(OUT_C,60,NINETY);
    rotation = 2;
    }
 }
 //3
 else if(rotation == 2){
  RotateMotor(OUT_B,60,-NINETY);
  RotateMotor(OUT_C,60,NINETY);
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,-NINETY);
   RotateMotor(OUT_C,60,NINETY);
   rotation = 4;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,-NINETY);
    RotateMotor(OUT_C,60,NINETY);
    rotation = 3;
    }
 }

 //5
 else if((rotation == 3) && (x < 0)){
  RotateMotor(OUT_B,60,ONEEIGHTY);
  RotateMotor(OUT_C,60,-ONEEIGHTY);
  y=y+1;
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,NINETY);
   RotateMotor(OUT_C,60,-NINETY);
   rotation = 2;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,ONEEIGHTY);
    RotateMotor(OUT_C,60,-ONEEIGHTY);
    x=x+1;
    rotation = 4;
    }
 }
 //6
 else if((rotation == 3) && (x >= 0)){
  RotateMotor(OUT_B,60,ONEEIGHTY);
  RotateMotor(OUT_C,60,-ONEEIGHTY);
  y=y-1;
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,NINETY);
   RotateMotor(OUT_C,60,-NINETY);
   rotation = 4;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,ONEEIGHTY);
    RotateMotor(OUT_C,60,-ONEEIGHTY);
    x=x-1;
    rotation = 2;
    }
 }
 //7
 else if((rotation == 4) && (x < 0))
 {
  RotateMotor(OUT_B,60,NINETY);
  RotateMotor(OUT_C,60,-NINETY);
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,NINETY);
   RotateMotor(OUT_C,60,-NINETY);
   rotation = 2;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,NINETY);
    RotateMotor(OUT_C,60,-NINETY);
    rotation = 3;
    }
 }
 //8
 else if((rotation == 4) && (x >= 0)){
  RotateMotor(OUT_B,60,NINETY);
  RotateMotor(OUT_C,60,-NINETY);
  rotation = 1;
   if(SensorUS(IN_4)<NEAR){
   RotateMotor(OUT_B,60,ONEEIGHTY);
   RotateMotor(OUT_C,60,-ONEEIGHTY);
   y=y+3 ;
   rotation = 3;
   }
    if(SensorUS(IN_4)<NEAR){
    RotateMotor(OUT_B,60,-NINETY);
    RotateMotor(OUT_C,60,NINETY);
    rotation = 2;
    }
 }
}
}
}