 //LBL = S
 void simplifyMaze()
 {
-  pathLength++;
-  
-  if(pathLength < 3 || path[pathLength-2] != backward)
-    return;
-
-  int totalAngle = 0;
-  int i;
-  for(i=1;i<=3;i++)
-  {
-    switch(path[pathLength-i])
-    {
-      case right:
-        totalAngle += 90;
-  break;
-      case left:
-  totalAngle += 270;
-  break;
-      case backward:
-  totalAngle += 180;
-  break;
-    }
-  }
-
-  // Get the angle as a number between 0 and 360 degrees.
-  totalAngle = totalAngle % 360;
-
-  // Replace all of those turns with a single one.
-  switch(totalAngle)
-  {
-    case 0:
-  path[pathLength - 3] = forward;
-  break;
-    case 90:
-  path[pathLength - 3] = right;
-  break;
-    case 180:
-  path[pathLength - 3] = backward;
-  break;
-    case 270:
-  path[pathLength - 3] = left;
-  break;
-  }
-
-  // The path is now two steps shorter.
-  pathLength -= 2;
-  
-} 
+	if (pathLength < 3 || path[pathLength - 2] != backward)
+	{
+		return;
+	}
+
+	int totalAngle = 0;
+
+	for (unsigned char i = 1; i <= 3; i++)
+	{
+		switch (path[pathLength - i])
+		{
+		case right:
+			totalAngle += 90;
+			break;
+		case left:
+			totalAngle += 270;
+			break;
+		case backward:
+			totalAngle += 180;
+			break;
+		}
+	}
+
+	// Get the angle as a number between 0 and 360 degrees.
+	totalAngle = totalAngle % 360;
+
+	// Replace all of those turns with a single one.
+	switch (totalAngle)
+	{
+	case 0:
+		path[pathLength - 3] = forward;
+		break;
+	case 90:
+		path[pathLength - 3] = right;
+		break;
+	case 180:
+		path[pathLength - 3] = backward;
+		break;
+	case 270:
+		path[pathLength - 3] = left;
+		break;
+	}
+
+	// The path is now two steps shorter.
+	pathLength -= 2;
+}
 
 #pragma endregion
 

		Serial.print('\t');
	}

	Serial.println((int)position - 2500);
}

void printPath(){
  Serial.println("+++++++++++++++++");
  int x;
  while(x<=pathLength){
  Serial.println(path[x]);
  x++;
  }
  Serial.println("+++++++++++++++++");
}


		moveBothMotors(maxMotorSpeed, backward, maxMotorSpeed, forward);
		checkForNewLineOnSide(left);

		if (isFirstRun)
		{
			path[pathLength] = left;
			pathLength++;
			simplifyMaze();
		}
		else
		{
			pathPositionInLaterRun++;
		}
		break;

	case forward:
